28 Apr 2025
本文将介绍在现有的里程计上，如何加入主方向的提取与结构线筛选、主方向的迭代更新、后端中的主方向约束因子。
整体框图如下：
前置内容
2D 线的表示、3D 线的表示以及之间的转换（投影：3D->2D；三角化：2D->3D）见 TODO
对环境结构的假设
考虑的环境是结构化环境，存在一个垂直主方向，多个水平主方向，多个倾斜主方向。所有水平主方向和垂直主方向垂直，
每个倾斜主方向都和某个水平主方向（可以不是同一个水平主方向）垂直 。
主方向的表达
什么是主方向呢？我们可以认为，许多条平行的 3D 线的共同方向便是主方向（Domain Direction, 简称 DD）
因此对于一个 3D 空间中的方向，我们可以自然地将其表示为 3 维单位向量。
以上我们都还未引入投影的概念，便能得到主方向的第一种表示。接下来我们将考虑两条 3D 的平行线，被一个相机观测，也就是将
它们投影到相机平面上，那么相机平面上的投影具有什么关系？这两条平行线的投影会交汇于一点，这一点在相机平面上，称为 消失点（Vanishing Point）。
如果我们将相机的光心/焦点与消失点连接起来，那么得到的向量是和两条平行线的方向平行的（fx 与 fy 相等是前提）！
所以，消失点 + 光心 = 主方向。
总的来说，对于一组 3D 平行线，我们可以将它们的方向定义为主方向，并用 3D 单位向量 或 2D 消失点 来表征这个方向。
对于本文，将统一使用 3D 单位向量来表达主方向。2D 消失点在相机平面上的位置是没有限制的，因此可能会处于图像之外，
而 3D 单位向量是在一个球面上活动，数值上是有限的，因此在计算上更稳定。
投影平面法向量
3D 线投影到相机平面上得到 2D 线，此时 3D 线与光心可以构成一个平面，我们称之为 投影平面（Projection Plane），
其法向量称之为投影平面法向量。因此一条线可以映射到一个法向量，但一个法向量可能对应多条线，或者说多条不同的线可以映射到
同一个法向量。
在以下的描述中，我们将用这个法向量来表达某一条线。
显而易见，一条线的法向量一定与其主方向垂直 ，这个是下文需要用到的重要结论。
主方向提取与结构线筛选
原理
首先根据相机系下的重力方向（假设已经通过初始得到），可以筛选出属于垂直主方向（vDD）的 2D 线段；
之后利用余下的法向量确定出水平法向量的数量和朝向，同时筛选出方向是水平的线段；
最后，以同样的方法利用最后剩下的法向量确定出倾斜法向量的数量和朝向。
根据重力方向筛选线段思路比较简单：遍历所有法向量，挑出与相机系下重力向量垂直的法向量，这些法向量对应的
线段的 3D 方向大概率是与重力平行的。至于为什么说是大概率，原因还是前文提到的多条不同的线可以映射到同一个
法向量。
最关键的如何用余下的法向量确定出水平主方向的个数与朝向，确定出朝向后，便可以和用重力筛选垂直线段的方法筛选出
水平线段。首先介绍正向的思路（论文中称为分治）：
水平主方向一定与垂直主方向垂直，因此我们可以将可行域限定在
一个平面上的单位向量，显然这个平面和垂直主方向垂直。
如何参数化在这一平面上的向量呢，我们可以寻找一个现有的和垂直主方向垂直的向量，
那便是任意一个垂直内点（上一步筛选得到的垂直方向线段的投影平面法向量），然后旋转一定角度，
得到的便是在可行域内的法向量，此时也将待求的法向量参数化为了一个角度 $\theta$
如何找到合适的角度呢？可以用分治的方法，先把 360 度分成几份，然后计算每个区间中点对应的向量与多少个法向量垂直（做内积），
然后选取垂直数量最多的区间，继续分割，重复以上过程
分治的方法需要预先给定主方向的个数，且复杂度为 $O(nl)$，$n$ 为分割个数，$l$ 为分割层数。
如果这时候我们反过来思考，不是去转这个向量然后看有多少法向量和它垂直，而是让每个法向量给出一个 $\theta$ 合法区间，
在这个合法区间内的向量基本与这个法向量垂直，对于一个真实存在的水平主方向，一定和许多属于这个方向的线的法向量垂直，
所以如果有很多个法向量给出的合法区间有重叠，那么则认为存在一个水平主方向，其 $\theta$ 落在这个区间内，如果有多个重叠，
那就有多个水平主方向。这样，水平主方向的数量、方向以及其内点就都确定了！
具体步骤如下：
选择一个垂直内点（是一个法向量）作为水平主方向的起始参考，水平主方向必定由该垂直内点旋转一定角度得到，
因此用 $\theta$ 参数化候选的水平主方向。$n$ 表示
\[\mathbf{h}_n(\theta_n) = \mathbf{R}_{<\mathbf{v}_m, \theta_n>} \mathbf{n}_3\]
水平主方向 垂直于 水平内点
\[\mathbf{h}_n(\theta_n)^T\mathbf{n}_k = 0\]
放松一下上式的要求，不要求完全垂直，然后展开合并同类项（具体推导 TODO）
\[|\mathbf{h}_n(\theta_n)^T\mathbf{n}_k| < \varepsilon\]
\[f_k(\theta) = |a_k \cdot \cos \theta_n + b_k \cdot \sin \theta_n| \leq \varepsilon\]
求上面的不等式，得到 $\theta$ 的合法区间
\[\theta \in [-\arctan(\frac{a}{b})-\arcsin(\tilde{\varepsilon}),-\arctan(\frac{a}{b})+\arcsin(\tilde{\varepsilon})]\]
对于余下的每个法向量，我们都可计算这么一个区间，接下来需要考虑如何求出所有的重叠区间。
使用的方法可以称之为 投票法。我们把所有区间放在数轴上，数轴向右为正，然后从左向右遍历每个端点，
如果遇到某个区间的左端点，则计数器加一，表示有一个区间加入了，如果遇到某个区间右端点，则计数器减一，
表示有一个区间退出了。这样，我们得到一个计数的序列，找出每个计数极大值的 $\theta_{l_n}$ 和其后续计数值的
$\theta_{r_n}$，这样就可以得到一个或多个重叠区间 $[\theta_{l_n},\ \theta_{r_n}]$
笔者对该算法做了一些改进：
实现
先介绍数据结构
再介绍对数据结构的操作（构造、访问、修改）
最后介绍数据结构之间的关系（如果有多个数据结构在这个模块中的话）
主方向的迭代更新
原理
实现
后端优化增加主方向约束
原理
实现
实验评估
介绍指标
结果
介绍批量运行的思路
参考
Hong Kong World: Leveraging Structural Regularity for Line-Based SLAM
27 Feb 2025
起始配置 和 目标配置之间的连续曲线，称为路径：
\[y: [0, 1] \rightarrow \mathcal{C}\]
$q_I = y(0)$ 是起始配置（initial configuration），$q_G = y(1)$ 是目标配置（goal configuration）。configuration 的含义是机器人某一时刻的一种可能的状态，例如机械臂各个关节的角度、速度、角速度等。
有许多类型的函数可以刻画一条曲线，下面介绍在机器人领域中最常用的三种类型。
分段线性表示（Piecewise-linear paths）
表示一条曲线的最简单的办法便是 $n$ 条相连的线段。
这需要先给定 $n$ 个路标点（或称为配置）$q_0,q_1,\cdots,q_n$，其中 $q_0 = q_I, q_n = q_G$。可以得到：
\[y(s) = interp(q_i, q_{i+1}, sn-i),\ i = \lfloor sn \rfloor\]
相邻两点之间进行线性插值。$i = \lfloor sn \rfloor$ 表示通过 $s$ 来确定线段的序号（线段的序号从 0 开始）。
优点是简单，缺点是存在导数不连续的点，可能导致机器人的急停，难以应用于系统动力学。
多项式函数表示（polynomial）
\[y(s) = poly(\mathbf{c_0}, \mathbf{c_1}, \cdots, \mathbf{c_0};s) = \sum_{i=0}^d \mathbf{c_i}s^i\]
样条函数表示（spline）
将路径表示为 $n$ 条曲线连接而成
\[y(s) = poly(C_i; sn-i)\]
$i = \lfloor sn \rfloor$ 是某段曲线的编号，$C_i = [\mathbf{c}{0,i}, \mathbf{c} {1,i}, \cdots, \mathbf{c}_{d,i}]$ 是该段多项式曲线的系数，不过我们通常不会存储这些系数，因为手动调整这些参数很难使得曲线连续光滑。一般，我们会存储 控制点 $\mathbf{p_1}, \mathbf{p_2}, \cdots, \mathbf{p_k}$ ，多项式的系数可以通过控制点推导出来。
例子 Cubic Hermite splines
假设某段曲线从 $\mathbf{m_0}$ 跑到了 $\mathbf{m_1}$，并期望满足起始和目标速度分别为 $\mathbf{t_0}$ 和 $\mathbf{t_1}$，即：
\[\mathbf{m_0} = poly(C_i;0)\]
\[\mathbf{m_1} = poly(C_i;1)\]
\[\mathbf{t_0} = \frac{\mathrm{d}}{\mathrm{d}u}poly(C_i;s) |_{s = 0}\]
\[\mathbf{t_1} = \frac{\mathrm{d}}{\mathrm{d}u}poly(C_i;s) |_{s = 1}\]
$\mathbf{m_0}, \mathbf{t_0}, \mathbf{m_1}, \mathbf{t_1}$ 就是控制点。
假设分段的多项式曲线的阶数是 3，所以
\[poly(C_i;s) = c_0 + c_1 s + c_2 s^2 + c_3 s^3\]
代入初始点的约束：
\[\mathbf{m_0} = c_0 + c_1 s + c_2 s^2 + c_3 s^3 |_{s=0} = c_0\]
\[\mathbf{t_0} = c_1 + 2 c_2 s + 3 c_3 s^2 |_{s=0} = c_1\]
带入目标点的约束：
\[\mathbf{m_1} = c_0 + c_1 s + c_2 s^2 + c_3 s^3 |_{s=1} = c_0 + c_1 + c_2 + c_3\]
\[\mathbf{t_1} = c_1 + 2 c_2 s + 3 c_3 s^2 |_{s=1} = c_1 + 2 c_2 + 3 c_3\]
把以上四个约束等式列在一起，写成矩阵形式：
\[\begin{bmatrix}
\mathbf{m_0}\\
\mathbf{t_0}\\
\mathbf{m_1}\\
\mathbf{t_1}\\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 & 0 & 0\\
0 & 1 & 0 & 0\\
1 & 1 & 1 & 1\\
0 & 1 & 2 & 3\\
\end{bmatrix}
\begin{bmatrix}
\mathbf{c}_0\\
\mathbf{c}_1\\
\mathbf{c}_2\\
\mathbf{c}_3\\
\end{bmatrix}\]
\[\begin{bmatrix}
\mathbf{c}_0\\
\mathbf{c}_1\\
\mathbf{c}_2\\
\mathbf{c}_3\\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 & 0 & 0\\
0 & 1 & 0 & 0\\
-3 & -2 & 3 & -1\\
2 & 1 & -2 & 1\\
\end{bmatrix}
\begin{bmatrix}
\mathbf{m_0}\\
\mathbf{t_0}\\
\mathbf{m_1}\\
\mathbf{t_1}\\
\end{bmatrix}\]
所以
\[\begin{align}
poly(C_i;s) &=
\begin{bmatrix}
1 & s & s^2 & s^3
\end{bmatrix}
\begin{bmatrix}
\mathbf{c}_0\\
\mathbf{c}_1\\
\mathbf{c}_2\\
\mathbf{c}_3\\
\end{bmatrix}
\\
&=
\begin{bmatrix}
1 & s & s^2 & s^3
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 & 0\\
0 & 1 & 0 & 0\\
-3 & -2 & 3 & -1\\
2 & 1 & -2 & 1\\
\end{bmatrix}
\begin{bmatrix}
\mathbf{m_0}\\
\mathbf{t_0}\\
\mathbf{m_1}\\
\mathbf{t_1}\\
\end{bmatrix}
\end{align}\]
控制点有直观的几何或物理含义，给定这些控制点后，根据最后一个等式，就可以直接得到满足约束的 3 阶段多项式曲线了。
参考
Section III. Motion - Kris Hauser - University of Illinois at Urbana-Champaign
27 Feb 2025
什么是运动规划（motion planning）
运动规划：使用可计算的方法为机器人生成一套运动行为，以完成给定的任务。
Motion planning: the use of computational methods to generate a robot’s motion to achieve a specified task.
运动规划处于自主机器人系统组件中的中心位置。
它的输入可以分成三类：
如果不考虑机器人的动力学，那么运动规划问题就变成了路径规划问题：
Kinematic motion planning (aka Path planning): the use of computational methods generate a continuous path between two robot configurations while respecting joint limits and avoiding obstacles.
运动规划的挑战
为什么运动规划会这么难？可以把运动规划想象成下棋，如果考虑后续的 n 步，每步都有 m 种决策，那么将会有 $m^n$ 种可能性，可能性将随 m、n 增加而迅速增长，更何况实际运动轨迹是连续的场景，因此暴力搜索不是解决办法，我们需要考虑更加精巧的方法来降低复杂度。
运动规划的分类
总体可以分成两类：
启发式方法（Heuristic methods）：启式发 意味着大部分情况下，这些方法能够满足要求，但没有成功或最优性的保证
行为脚本（behavior script）
生成-评分（generate-and-score）
更具原则性的方法（More principled methods）
虫子算法（bug algorithms）
roadmap planners
grid search
visibility graph
cell decomposition
trapezoidal decomposition
approximate cell decomposition
参考
Section III. Motion - Kris Hauser - University of Illinois at Urbana-Champaign
