欢迎访问我的博客首页。

1. LOAM 概述

1.1 主要贡献

  LOAM 旨在使用一个运动的三维激光雷达进行运动估计和地图创建。由于采集时刻不同，运动的激光雷达采集到的点存在畸变，如左侧的点云。LOAM 使用两个并行的算法解决这个问题：里程计算法估计激光雷达的速度并校正点云畸变；建图算法匹配并注册点云到地图中。

1.2 硬件

  北阳 UTM-30LX 是单线二维激光雷达，它的激光发射器沿水平方向转动，每隔 0.25° 发射一个激光束。所以其水平分辨率是 $0.25$°。

  为了实现三维扫描，激光雷达被安装在一个电机上。该电机以 40 转每秒的速度，在竖直方向的 180° 范围内以逐行扫描的方式扫描 40 行。所以其竖直分辨率是 $180÷40=4.5$°。

  北阳电机提供激光雷达上位机软件 UrgBenri，可以很方便的进行数据的采集、显示、存储。下面是 UrgBenri 存储的数据内容，第 28 行是激光发射器旋转 270° 采集到的 $270÷0.25=1080$ 个点，第 30 行是采集第一个点时的时间戳。

[model]
UTM-30LX(Hokuyo Automatic Co.,Ltd.)
[frontStep]
540
[minDistance]
23
[maxDistance]
60000
[motorSpeed]
2400
[totalSteps]
1440
[captureMode]
GD_Capture_mode
[scanMsec]
25
[startStep]
0
[endStep]
1080
[serialNumber]
00904066
[timestamp]
793464
[logtime]
2013:1:12:15:6:32:312
[scan]
392;397;398;400;403;403;407;409;409;409;411;411;422;424;424;422;420;...
[timestamp]
793514
[logtime]
2013:1:12:15:6:32:351
[scan]

  上面的 1080 个点是一次扫描（scan）采集到的点。电机完成一轮逐行扫描，产生 $1080\times 40=4.32$ 万个点，称为一次扫描覆盖（sweep）。

1.3 符号表示与任务描述

  LOAM 的任务是，根据三维雷达采集的点云估计雷达自身的运动，并对遍历过的场景建图。LOAM 假设雷达内参已标定且内部运动学已知。内存已标定，就可以对雷达点进行 3D 投影。还假设雷达的角速度和线速度是平滑且连续的，该假设使用 IMU 实现。

  小写的右下角标 $k$ 代表第 $k$ 次扫描覆盖。一次扫描覆盖采集到的点云用 ${\mathcal{P}}_k$ 表示。第 $k$ 次扫描覆盖的开始时刻用 $t_k$ 表示。

  大写的右上角标 $L$、$W$ 代表坐标系。激光雷达坐标系 $L$ 是一个三维坐标系，原点在雷达的几何中心，$x$ 轴向左，$y$ 轴向上，$z$ 轴向前，如图 2。激光雷达第 $k$ 次扫描覆盖采集到的点 $i$ 在雷达坐标系中的坐标用 $X_{(k,i)}^L$ 表示。$T_k^L(t)$ 表示把雷达在时刻 $t$ 采集到的一个点投影到时刻 $t_k$ 所对应的变换（投影就是坐标变换）。

  世界坐标系 $W$ 与激光雷达初始坐标系重合。激光雷达第 $k$ 次扫描覆盖采集到的点 $i$ 在世界坐标系中的坐标用 $X_{(k,i)}^W$ 表示。$T_k^W(t)$ 表示把采集于时刻 $t$ 的一点投影到世界坐标系所对应的变换。

  LOAM（lidar odometry and mapping）问题可以概况为：求点云 ${\mathcal{P}}_k$ 的位姿，并用这些点云创建地图。

1.4 软件

  每个扫描过程中，都会把激光雷达采集到的点 $\hat{\mathcal{P}}$ 变换到激光雷达坐标系 $L$ 中，得到 $\mathcal{P}$。

  第 $k$ 次扫描采集的点云 ${\mathcal{P}}_k$ 会被两个算法处理：里程计算法计算两次连续扫描间的激光雷达运动；运动估计算法对 $\mathcal{P}$ 祛畸变。这两个算法的工作频率是 10 赫兹。

  算法的输出用于创建地图。创建地图时，把祛畸变的点云与地图匹配并加入地图，工作频率是 1 赫兹。

2. 特征点提取

  点云 ${\mathcal{P}}_k$ 在横竖方向的分辨率不一样，即它不是均匀分布的。但是它的每行点云是共面的，40 行点云对应 40 个平面。因此，利用每行点云的共面关系提取特征点。

  特征点包含边缘点和平面点。假设 $i$ 是 ${\mathcal{P}}_k$ 中第 $m$ 行 $n$ 列的一点，局部点集 $\mathcal{S}$ 是第 $m$ 行第 $n\pm x$ 列点云。点云 $i$ 的局部表面平滑性：

$$c=\frac{1}{|\mathcal{S}|\cdot ||X_{(k,i)}^L||}||\sum _{j\in \mathcal{S},j\ne i}(X_{(k,i)}^L-X_{(k,j)}^L)||.\text{\hspace{1em}(1)}$$

为了消除尺度影响，使用点 $i$ 到激光雷达中心的距离 $||X_{(k,i)}^L||$ 除局部表面平滑性。

  把一次扫描采集到的点按 $c$ 的大小排序，再根据 $c$ 的大小选择特征点：从大于阈值 $5\times 10^{-3}$ 的点中选择较大的点当作边缘点，从小于阈值 $5\times 10^{-3}$ 的点中选择较小的点当作平面点。为了使特征点均匀分布，把扫描平面分成四个子区域，在每个子区域中最多提取 2 个边缘点和 4 个平面点。特征点选取的注意事项：

1. 特征点应该足够稀疏，即，特征点旁边的点不应再被当作特征点。
2. 特征点所在的局部表面不应该与激光束平行，如图 4(a)。因为这样的点通常不可靠。
3. 特征点不应该在遮挡区域的边界，如图 4(b)。由于橘色区域被遮挡，点 $C$ 会被认为是边缘点。而实际上它是平面点，因为换个角度就能看到遮挡区域。

  为了避免上述三种点被当作特征点：要求采集点 $i$ 的激光束与 $\mathcal{S}$ 的表面法向量的夹角必须大于 10°；$\mathcal{S}$ 内的点不能被障碍物阻断，如图 4(b) 的 D 点。

  图 5 是在一个走廊场景中提取的特征点。黄色代表边缘点，红色代表平面点。激光雷达以 0.5 米每秒的速度向图左侧的墙移动，造成墙上出现运动畸变。

3. 特征点匹配

  里程计算法用于估计激光雷达在一个扫描内的运动。第 $k-1$ 次扫描采集到的点云 ${\mathcal{P}}_{k-1}$ 会被投影到时刻 $t_k$ 得到 ${\bar{\mathcal{P}}}_{k-1}$，从 ${\mathcal{P}}_k$ 中提取的特征点也会被投影到时刻 $t_k$。投影后的点用于估计雷达的运动。

  图 6 中，蓝色的线表示的第 $k-1$ 次扫描采集到的点云 ${\mathcal{P}}_{k-1}$。在第 $k-1$ 次扫描的结束时刻 $t_k$，${\mathcal{P}}_{k-1}$ 被投影到时刻 $t_k$，投影后用 ${\bar{\mathcal{P}}}_{k-1}$ 表示，如图中的绿线。在第 $k$ 次扫描过程中，${\bar{\mathcal{P}}}_{k-1}$ 和新采集到的点云 ${\mathcal{P}}_k$（黄线）一起用于估计激光雷达的运动。

  特征点匹配时，先使用前面讨论的特征点提取算法从 ${\mathcal{P}}_k$ 中提取边缘点 ${\mathcal{E}}_k$ 和平面点 ${\mathcal{H}}_k$。再从 ${\bar{\mathcal{P}}}_{k-1}$ 中选择边缘线和平面块作为边缘点和平面点的匹配。

  注意，${\mathcal{P}}_k$ 在第 $k$ 次扫描开始时是空的，它包含的点云数量随着扫描逐渐增长。里程计在采集过程中递归地估计激光雷达的 6 自由度运动。${\mathcal{E}}_k$ 和 ${\mathcal{H}}_k$ 被投影到扫描开始时刻，得到 ${\tilde{\mathcal{E}}}_k$ 和 ${\tilde{\mathcal{H}}}_k$。我们需要在 ${\bar{\mathcal{P}}}_{k-1}$ 中为 ${\tilde{\mathcal{E}}}_k$ 和 ${\tilde{\mathcal{H}}}_k$ 中的每一个特征点找到最近点。为了快速查找，${\bar{\mathcal{P}}}_{k-1}$ 以三维 KD 树的形式存储，存储的形式是 $L_k$。

  图 7(a) 是为一个边缘点匹配一个边缘线的过程。边缘线由来自 ${\bar{\mathcal{P}}}_{k-1}$ 的两个点 $j$、$l$ 表示。假设 $i$ 是 ${\tilde{\mathcal{E}}}_k$ 中的一点，$j$ 是 ${\bar{\mathcal{P}}}_{k-1}$ 中离 $i$ 最近的点，$l$ 是 ${\bar{\mathcal{P}}}_{k-1}$ 中与 $j$ 相邻的两行中离 $i$ 最近的点。于是，边缘线 $(j,l)$ 就是边缘点 $i$ 的匹配。然后使用公式 (1) 验证 $j$ 和 $l$ 是边缘点，要求 $c>5\times 10^{-3}$。

  之所以要求 $j$ 和 $l$ 来自不同的扫描行，是考虑到一次扫描不能从同一条边缘线采集到两个及以上的点，除非边缘线在扫描平面上。如果边缘线在扫描平面上，它就退化成一条直线，这条线上的特征点一开始也就不会被当成特征点。

  图 7(b) 是为一个平面点匹配一个平面块的过程。平面块由来自 ${\bar{\mathcal{P}}}_{k-1}$ 的三个点 $j$、$l$、$m$ 表示。假设 $i$ 是 ${\mathcal{H}}_k$ 中的一点，$j$ 是 ${\bar{\mathcal{P}}}_{k-1}$ 中离 $i$ 最近的点，$l$ 是 ${\bar{\mathcal{P}}}_{k-1}$ 中与 $j$ 同行且离 $i$ 第二近的点，$m$ 是来自 $j$ 旁边两行离 $i$ 最近的点。这样可以保证三个点不共线。然后使用公式 (1) 验证 $j$、$l$、$m$ 都是平面点，要求 $c<5\times 10^{-3}$。

  图 7(a) 表示为 ${\tilde{\mathcal{E}}}_k$ 中的一个边缘点 $i$ 匹配边缘线，图 7(b) 表示为 ${\tilde{\mathcal{H}}}_k$ 中的一个平面点 $i$ 匹配平面块。$j$ 是从 ${\bar{\mathcal{P}}}_{k-1}$ 中找到的离 $i$ 最近的点。橘色线代表的点和 $j$ 来自同一行扫描，蓝色线代表橘色线前后的两行扫描。在图 a 中，为了找到边缘线，在蓝色线中寻找点 $l$。在图 b 中，为了找到平面块，分别在橘色线和蓝色线中寻找点 $l$ 和点 $m$。

  接下来计算特征点到它的匹配对象的距离，然后最小化所有距离求得激光雷达的运动。

  ${\tilde{\mathcal{E}}}_k$ 中的一点 $i$ 到直线 $(j,l)$ 的距离

$$d_{\mathcal{E}}=\frac{\left|\begin{array}{c}({\tilde{X}}_{(k,i)}^L-{\bar{X}}_{(k-1,j)}^L)\times ({\tilde{X}}_{(k,i)}^L-{\bar{X}}_{(k-1,l)}^L)\end{array}\right|}{\left|\begin{array}{c}{\bar{X}}_{(k-1,j)}^L-{\bar{X}}_{(k-1,l)}^L\end{array}\right|},\text{\hspace{1em}(2)}$$

其中 ${\tilde{X}}_{(k,i)}^L$、${\bar{X}}_{(k-1,j)}^L$、${\bar{X}}_{(k-1,l)}^L$ 分别是 $i$、$j$、$l$ 在坐标系 $L_k$ 中的坐标。${\tilde{\mathcal{H}}}_k$ 中的一点 $i$ 到平面 $(j,l,m)$ 的距离

$$d_{\mathcal{H}}=\frac{\left|\begin{array}{c}{\tilde{X}}_{(k,i)}^L-{\bar{X}}_{(k-1,j)}^L\\ ({\bar{X}}_{(k-1,j)}^L-{\bar{X}}_{(k-1,l)}^L)\times ({\bar{X}}_{(k-1,j)}^L-{\bar{X}}_{(k-1,m)}^L)\end{array}\right|}{\left|\begin{array}{c}({\bar{X}}_{(k-1,j)}^L-{\bar{X}}_{(k-1,l)}^L)\times ({\bar{X}}_{(k-1,j)}^L-{\bar{X}}_{(k-1,m)}^L)\end{array}\right|}.\text{\hspace{1em}(3)}$$

公式 (2) 和公式 (3) 的推导分别参见附录 8.2 和 8.3。

4. 运动估计

  激光雷达在一个扫描覆盖内的运动被建模为角速度和线速度都是匀速的运动，这样就可以根据时间戳使用线性插值的方式求得一个扫描内各时刻的位姿。假设 $t_k$ 是第 $k$ 次扫描覆盖的开始时刻，$t$ 是当前时刻。${\mathbf{T}}_k^L(t)$ 是雷达从时刻 $t$ 变换回时刻 $t_k$ 的六自由度位姿变换，${\mathbf{T}}_k^L(t)=[{\tau }_k^L(t),{\theta }_k^L(t){]}^T$，其中 ${\tau }_k^L(t)=[t_x,t_y,t_z{]}^T$ 和 ${\theta }_k^L(t)=[{\theta }_x,{\theta }_y,{\theta }_z{]}^T$ 分别是在 $L_k$ 坐标系中的平移和旋转。给定 ${\theta }_k^L(t)$，旋转矩阵可以根据罗德里格斯（Rodrigues）公式定义为

$${\mathbf{R}}_k^L(t)=e^{{\hat{\theta }}_k^L(t)}=\mathbf{I}+\frac{{\hat{\theta }}_k^L(t)}{||{\theta }_k^L(t)||}sin||{\theta }_k^L(t)||+(\frac{{\hat{\theta }}_k^L(t)}{||{\theta }_k^L(t)||}{)}^2(1-||cos{\theta }_k^L(t)||).\text{\hspace{1em}(4)}$$

其中，${\hat{\theta }}_k^L(t)$ 是 ${\theta }_k^L(t)$ 的斜对称矩阵。

  假设 $i$ 是 ${\mathcal{P}}_k$ 中的一点，$t(k,i)$ 是它的时间戳，${\mathbf{T}}_{(k,i)}^L$ 是 $[t_k,t_{(k,i)}]$ 间某个时刻的位姿变换。${\mathbf{T}}_{(k,i)}^L$ 可以通过对 ${\mathbf{T}}_k^L(t)$ 线性插值计算得到：

$${\mathbf{T}}_{(k,i)}^L=\frac{t_{(k,i)}-t_k}{t-t_k}{\mathbf{T}}_k^L(t).\text{\hspace{1em}(5)}$$

注意，${\mathbf{T}}_k^L(t)$ 随着时间变化，所以上面的插值也随着时间的变化而不同。下面的公式把边缘点 ${\mathcal{E}}_k$ 和平面点 ${\mathcal{H}}_k$ 变换到扫描的开始时刻，得到 ${\tilde{\mathcal{E}}}_k$ 和平面点 ${\tilde{\mathcal{H}}}_k$：

$${\tilde{\mathbf{X}}}_{(k,i)}^L={\mathbf{R}}_{(k,i)}^L{\mathbf{X}}_{(k,i)}^L+{\tau }_{(k,i)}^L,\text{\hspace{1em}(6)}$$

其中，${\mathbf{X}}_{(k,i)}^L$ 是 ${\mathcal{E}}_k$ 或 ${\mathcal{H}}_k$ 中的一点，${\tilde{\mathbf{X}}}_{(k,i)}^L$ 是 ${\mathbf{X}}_{(k,i)}^L$ 变换后的点。${\mathbf{R}}_{(k,i)}^L$ 和 ${\tau }_{(k,i)}^L$ 是 ${\mathbf{T}}_{(k,i)}^L$ 包含的旋转矩阵和平移向量。

  公式（2）用于计算投影后的点到其匹配对象的距离，公式（6）定义投影操作。把这两个公式合写成一个公式：

$$f_{\mathcal{E}}({\mathbf{X}}_{(k,i)}^L,{\mathbf{T}}_k^L(t))=d_{\mathcal{E}},i\in {\mathcal{E}}_k.\text{\hspace{1em}(7)}$$

该公式把一个边缘点 ${\mathbf{X}}_{(k,i)}^L$ 投影到时刻 $t_k$，并求投影后的点到与其匹配的边缘线的距离 $d_{\mathcal{E}}$。同样，由公式 (3) 和公式 (6) 可以得到 ${\mathcal{H}}_k$ 中的一个平面点到与其匹配的平面块的距离：

$$f_{\mathcal{H}}({\mathbf{X}}_{(k,i)}^L,{\mathbf{T}}_k^L(t))=d_{\mathcal{H}},i\in {\mathcal{H}}_k.\text{\hspace{1em}(8)}$$

  最后使用莱文贝格－马夸特（Levenberg–Marquardt）算法求解激光雷达的运动。把公式 (7) 和 (8) 写成一个统一的形式，就得到一个非线性函数：

$$\mathbf{f}({\mathbf{T}}_{\mathbf{k}}^{\mathbf{L}}(\mathbf{t}))=\mathbf{d},\text{\hspace{1em}(9)}$$

其中，$\mathbf{f}$ 的每行对应一个特征点，$\mathbf{d}$ 是相应的距离。我们计算 $\mathbf{f}$ 关于 ${\mathbf{T}}_k^L(t)$ 的雅可比矩阵 $\mathbf{J}$，其中 $\mathbf{J}=\partial f/\partial {\mathbf{T}}_k^L(t)$。然后使用非线性迭代最小化 $\mathbf{d}$ 就可以求解公式 (9)：

$${\mathbf{T}}_k^L(t)\leftarrow {\mathbf{T}}_k^L(t)-({\mathbf{J}}^T\mathbf{J}+\lambda diag({\mathbf{J}}^T\mathbf{J}){)}^{-1}{\mathbf{J}}^T\mathbf{d}.\text{\hspace{1em}(10)}$$

其中 $\lambda$ 是莱文贝格-马夸特算法需要的一个因数。

5. 里程计算法

  下面是第 $k$ 次扫描覆盖过程中，里程计算法的迭代过程。它的输入有三个：

1. ${\bar{\mathcal{P}}}_{k-1}$: 第 $k-1$ 次扫描覆盖采集到的点 ${\mathcal{P}}_{k-1}$ 在时刻 $t_k$ 的投影。
2. ${\mathcal{P}}_k$: 从时刻 $t_k$ 到时刻 $t$ 期间采集到的点，点的数量随着 $t$ 的增大而增多。
3. ${\mathbf{T}}_k^L(t)$: 时刻 $t$ 到时刻 $t_k$ 的变换，其中 $t_k\le t\le t_{k+1}$。该算法在时间段 $[t_k,t_{k+1}]$ 中迭代：${\mathbf{T}}_k^L(t)$ 在 $t_k$ 时刻被赋予初始值 0，然后在每次迭代过程中更新，在 $t_{k+1}$ 时刻迭代结束即为时刻 $t_{k+1}$ 到时刻 $t_k$ 的变换。

${\mathbf{T}}_k^L(t)$ 的初始值为 0。从第二次迭代开始，${\mathbf{T}}_k^L(t)$ 使用上次迭代结束时它的值。${\mathbf{T}}_k^L(t)$ 的更新方法：

1. 从 ${\mathcal{P}}_k$ 中选择特征点 ${\mathcal{E}}_k$ 和 ${\mathcal{H}}_k$。
2. 使用 ${\mathbf{T}}_k^L(t)$ 把它们变换到 $t_k$ 时刻，得到 ${\tilde{\mathcal{E}}}_k$ 和 ${\tilde{\mathcal{H}}}_k$。
3. 假设 $i$ 是 ${\tilde{\mathcal{E}}}_k$ 或 ${\tilde{\mathcal{H}}}_k$ 中的一个点。在 ${\bar{\mathcal{P}}}_{k-1}$ 中找 $i$ 的匹配对象边缘线或平面块，计算 $i$ 到该匹配对象的距离。调整 ${\mathbf{T}}_k^L(t)$ 使距离总和减小，此时的 ${\mathbf{T}}_k^L(t)$ 即为本次迭代的结果。

当距离总和收敛或达到最大迭代次数时停止迭代。由于里程计的频率是 10 赫兹，所以距离该算法上次被调用的时间达到 100 毫秒时，该算法再次被调用。因为在这 100 毫秒中，${\mathcal{P}}_k$ 中又增加了一些点。

  为了区分特征匹配的质量，为每个距离添加一个权重：特征点和它的匹配对象间的距离越大，权重越小；距离大于阈值，权重为 0。权重的定义如下：

$$w=\{\begin{array}{cc}(1-{\alpha }^2{)}^2& -1<\alpha <1,\\ 0& otherwise,\end{array}\text{\hspace{1em}(11)}$$

其中 $\alpha =\frac{r}{6.9459\sigma \sqrt{1-h}}$，$r$ 是最小二乘对应的残差，$\sigma$ 是残差的中位数绝对离差，$h$ 是 $(\mathbf{J}({\mathbf{J}}^{\mathbf{T}}\mathbf{J}){)}^{-1}{\mathbf{J}}^T$ 对角线上的杠杆值或对应元，$\mathbf{J}$ 是公式 (10) 中说过的雅可比矩阵。

6. 地图创建算法

  第 $k$ 次扫描覆盖结束时，里程计算法计算出一个位姿变换 ${\mathbf{T}}_k^L(t)$，其中 $t=t_{k+1}$。该变换可以祛除 ${\mathcal{P}}_k$ 中的畸变得到 ${\bar{\mathcal{P}}}_k$。

  蓝色曲线代表雷达在地图中的位姿 ${\mathbf{T}}_{k-1}^W(t_k)$，该位姿是地图创建算法在第 $k-1$ 次扫描周期内计算出来的。橘色曲线代表雷达在第 $k$ 次扫描过程中的运动 ${\mathbf{T}}_k^L(t_{k+1})$，该运动是里程计算法计算出来的。根据 ${\mathbf{T}}_{k-1}^W(t_k)$ 和 ${\mathbf{T}}_k^L(t_{k+1})$，里程计算法产生的无畸变点云被投影到地图中成为 ${\mathcal{Q}}_k$（绿线），并与地图中存在的点云 ${\mathcal{Q}}_{k-1}$ （黑线）匹配。

  地图创建算法在每个扫描周期内仅被调用一次，它把 ${\bar{\mathcal{P}}}_k$ 与地图匹配并注册 ${\bar{\mathcal{P}}}_k$ 到世界坐标系 { W } \{W\} {W} 中，如图 8 所示。

  定义 ${\mathcal{Q}}_{k-1}$ 是截至第 $k-1$ 次扫描结束时，地图中积累的点云；${\mathbf{T}}_{k-1}^W(t_k)$ 是第 $k-1$ 次扫描结束时刻 $t_k$，雷达在地图中的位姿。地图创建算法把 ${\mathbf{T}}_{k-1}^W(t_k)$ 扩展到 $[t_k,t_{k+1}]$ 得到 ${\mathbf{T}}_k^W(t_{k+1})$，把 ${\bar{\mathcal{P}}}_k$ 投影到世界坐标系 { W } \{W\} {W} 得到 ${\bar{\mathcal{Q}}}_k$，然后通过优化雷达位姿 ${\mathbf{T}}_k^W(t_{k+1})$ 匹配 ${\bar{\mathcal{Q}}}_k$ 和 ${\mathcal{Q}}_{k-1}$。

  为了寻找特征点的匹配对象，把地图 ${\mathcal{Q}}_{k-1}$ 中的点云储存在一个 10 立方米的区域内。这个区域与 ${\bar{\mathcal{Q}}}_k$ 重叠的部分中的点会按它们在世界坐标系 $W$ 中的坐标被存储在一个三维 KD 树中。假设 ${\mathcal{S}}^{\prime }$ 是 ${\mathcal{Q}}_{k-1}$ 中，以特征点为中心的一个棱长为 10 厘米的立方体区域。对于一个边缘点，只保留 ${\mathcal{S}}^{\prime }$ 中边缘线上的点；对于一个平面点，只保留 ${\mathcal{S}}^{\prime }$ 中平面块上的点。然后计算 ${\mathcal{S}}^{\prime }$ 的协方差矩阵 $\mathbf{M}$ 及 $\mathbf{M}$ 的特征值 $\mathbf{V}$ 和特征向量 $\mathbf{E}$。这些值决定点云簇的位姿和点线、点面距离。

  如果 ${\mathcal{S}}^{\prime }$ 位于边缘线上，$\mathbf{V}$ 的一个特征值会明显大于另外两个，最大的一个特征值对应的特征向量代表着边缘线的方向。如果 ${\mathcal{S}}^{\prime }$ 位于平面块上，$\mathbf{V}$ 的一个特征值会明显小于另外两个，最小的一个特征值对应的特征向量代表着平面块的方向。这样计算的边缘线和平面块经过 ${\mathcal{S}}^{\prime }$ 的中心。

  为了计算特征点到其匹配对象的距离，在边缘线上选择两个点，在平面块上选择三个点，然后使用公式（2）和公式（3）计算距离。然后使用公式（7）和公式（8）计算距离。不同的是，${\bar{\mathcal{Q}}}_k$ 中的点时间戳都是 $t_{k+1}$。再次使用莱文贝格-马夸特算法进行非线性优化，接着 ${\bar{\mathcal{Q}}}_k$ 被注册到地图。

  为了分布均匀，每次把一次扫描合并到地图后，都使用体素网格滤波器对地图点云下采样。体素网格滤波器使各体素中包含相同数量的点。边缘点和平面点的体素尺寸不同，前者的体素棱长为 5 厘米，后者的体素棱长为 10 厘米。为了限制内存占用，地图大小被限制在以传感器为中心，棱长为 500 米的范围内。

  蓝色区域代表地图创建算法在每个扫描周期内计算出的一个位姿 ${\mathbf{T}}_{k-1}^W(t_k)$。橘色区域代表里程计以 10 赫兹的频率计算出的雷达在当前扫描过程中的运动。雷达的运动估计包含两个变换，频率也是 10 赫兹。

  位姿变换如图 9。蓝色区域代表地图创建算法在每个扫描周期内计算出的一个位姿 ${\mathbf{T}}_{k-1}^W(t_k)$。橘色区域代表里程计以 10 赫兹的频率计算出的变换 ${\mathbf{T}}_k^L(t)$。雷达相对地图的位姿包含这两个变换，频率也是 10 赫兹。

7. 附录

7.1 数量积与向量积

  数量积又称点积、内积，计算方式为：

$$\vec{a}\cdot \vec{b}=\{\begin{array}{l}{x}_1{x}_2+y_1{y}_2\\ |\vec{a}||\vec{b}|cos\theta \end{array}.\text{\hspace{1em}(8.1)}$$

如果知道平面的法向量和平面上一点，由第二个公式可以求得平面外一点到平面的距离。

  向量积又称叉积、外积，计算方式为：

$$\{\begin{array}{l}\vec{a}\times \vec{b}=(a_y{b}_z-a_z{b}_y)\vec{i}+(a_z{b}_x-a_x{b}_z)\vec{j}+(a_x{b}_y-a_y{b}_x)\vec{k}\\ |\vec{a}\times \vec{b}|=|\vec{a}||\vec{b}|sin\theta \end{array}.\text{\hspace{1em}(8.2)}$$

第一个公式求得的是一个垂直于向量 $\vec{a}$ 和 $\vec{b}$ 所在平面的向量。第二个公式求得的是以 $\vec{a}$ 和 $\vec{b}$ 为邻边的平行四边形的面积。

7.2 点到直线的距离

  我们讨论向量形式的点到直线距离公式。如图 8.1（左），点 $i$ 是直线 $jl$ 外一点，$i$、$j$、$l$ 坐标已知，求点 $i$ 到直线 $jl$ 的距离。

  作平行四边形 $ijml$，则其面积是 $|\overrightarrow{ij}\times \overrightarrow{il}|$。因为直线 $jl$ 把四边形分成两个全等的三角形，所以四边形的面积也等于 $|\overrightarrow{Oi}|\cdot |\overrightarrow{jl}|$，其中 $O$ 是过点 $i$ 的直线与直线 $jl$ 的垂足。所以点 $i$ 到直线 $jl$ 的距离

$$|\overrightarrow{Oi}|=\frac{|\overrightarrow{ij}\times \overrightarrow{il}|}{|\overrightarrow{jl}|}.\text{\hspace{1em}(8.3)}$$

7.3 点到平面的距离

  我们讨论向量形式的点到平面距离公式。如图 7（右），已知平面上的三个点 $j$、$l$、$m$ 和平面外一点 $i$ 的坐标，求点 $i$ 到平面的距离。

  由平面上的三点可以求得平面的法向量 $\vec{n}=\overrightarrow{jl}\times \overrightarrow{jm}$，向量 $\overrightarrow{ij}$ 与 $\vec{n}$ 夹角的余弦值 $cos\theta =(\overrightarrow{ij}\cdot \vec{n})/(|\overrightarrow{ij}||\vec{n}|)$。所以点 $i$ 到平面的距离

$$\begin{array}{rl}|\overrightarrow{Oi}|=|\overrightarrow{ij}||cos\theta |=|\overrightarrow{ij}|\frac{|\overrightarrow{ij}\cdot \vec{n}|}{|\overrightarrow{ij}||\vec{n}|}& =\frac{|\overrightarrow{ij}\cdot \vec{n}|}{|\vec{n}|}\\ & =\frac{|\overrightarrow{ij}\cdot (\overrightarrow{jl}\times \overrightarrow{jm})|}{|\overrightarrow{jl}\times \overrightarrow{jm}|}\\ & =|\overrightarrow{ij}||{\vec{n}}_{unit}|\end{array}\text{\hspace{1em}(8.4)}$$

其中 ${\vec{n}}_{unit}$ 是平面的单位法向量。

7.4 协方差矩阵的特征值与特征向量

  矩阵 $M_{3,n}$ 由 $n$ 个三维点的坐标组成，它的协方差矩阵 $C_{3,3}$ 是一个三阶方阵。假设 $C_{3,3}$ 的三个特征值从大到小排列为 ${\lambda }_1$、${\lambda }_2$、${\lambda }_3$，对应的特征向量分别为 ${\vec{v}}_1$、${\vec{v}}_2$、${\vec{v}}_3$。假设 $n$ 个三维点组成的空间区域为 $Z$。

  ${\vec{v}}_1$ 是 $Z$ 中距离最远的两个点所在直线的方向向量，${\lambda }_1$ 是这两个点的距离。${\vec{v}}_2$ 是 $Z$ 中垂直于 ${\vec{v}}_1$ 方向上距离最远的两个点所在直线的方向向量，${\lambda }_2$ 是这两个点的距离。${\vec{v}}_3$ 是 $Z$ 中垂直于 ${\vec{v}}_1$、${\vec{v}}_2$ 方向上距离最远的两个点所在直线的方向向量，${\lambda }_3$ 是这两个点的距离。

  所以，假如这 $n$ 个点分布在一条直线上，${\lambda }_1$ 明显大于 0，${\lambda }_2$ 和 ${\lambda }_3$ 为 0；假如这 $n$ 个点分散在一个平面上，${\lambda }_1$ 和 ${\lambda }_2$ 明显大于 0，${\lambda }_3$ 为 0；假如这 $n$ 个点不共面，${\lambda }_1$、${\lambda }_2$ 和 ${\lambda }_3$ 都明显大于 0。下面使用 numpy 计算 $n$ 个三维点组成的矩阵的协方差矩阵及其特征值与特征向量。

import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d

def show_points(x, y, z, eigenvalues, eigenvectors):
    ax = plt.subplot(projection='3d')
    ax.set_title('3D points')
    ax.scatter(x, y, z, c='gray')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.quiver(1, 2, 3, eigenvectors[:, 0][0], eigenvectors[:, 0][1], eigenvectors[:, 0][2], length=0.1 * eigenvalues[0],
              color='r')
    ax.quiver(1, 2, 3, eigenvectors[:, 1][0], eigenvectors[:, 1][1], eigenvectors[:, 1][2], length=0.1 * eigenvalues[1],
              color='g')
    ax.quiver(1, 2, 3, eigenvectors[:, 2][0], eigenvectors[:, 2][1], eigenvectors[:, 2][2], length=0.1 * eigenvalues[2],
              color='b')
    plt.show()

def get_points(points='line'):
    x = np.linspace(start=-30, stop=30, num=5000)
    if points == 'line':
        y = np.linspace(start=-20, stop=20, num=5000)
    else:
        y = np.random.randint(-20, 20, 5000)
    z = 3 - (4 * (x - 1) + 5 * (y - 2)) / 6.0
    return x, y, z

if __name__ == '__main__':
    x, y, z = get_points(points='plane')
    matrix = np.vstack((x, y, z))  # shape=(3, n)
    # 协方差矩阵。
    covariance_matrix = np.cov(matrix)  # shape=(3, 3)
    # 协方差矩阵的特征值与特征向量。
    eigenvalues, eigenvectors = np.linalg.eig(covariance_matrix)
    # 最大的特征值。
    max_eigenvalue_idx = np.argmax(eigenvalues)
    # 最大的特征值对应的特征向量。
    max_eigenvector = eigenvectors[:, max_eigenvalue_idx]
    # 绘图。
    show_points(x, y, z, eigenvalues, eigenvectors)

8. 参考

1. LOAM 论文 1，CMU，2014。
2. LOAM 论文 2，sci-hub，2017。
3. 激光雷达笔记系列，CSDN。
4. A-LOAM，HKUST-Aerial-Robotics，github。
5. loam_velodyne，github。
6. LeGO-LOAM，github。
7. A-LOAM 建图讲解，博客园。