F-LOAM

欢迎访问我的博客首页。

---

1. 传感器模型与特征提取

---

1.1 传感器模型

---

  机械式三维激光雷达有 M 个激光发射器竖直排列，称为 M 线激光雷达。扫描时，竖直排列的激光发射器沿水平方向匀速旋转，并以固定的时间间隔发射激光脉冲，通过激光脉冲的往返时间测量距离。在一个扫描周期内，激光发射器发射 N 次脉冲，采集到 $M$ 行 $N$ 列个点。这样的一次扫描称为一个扫描或一帧。我们把第 $k$ 次扫描记为 $P_k$，采集到的每个点记为 ${\mathbf{p}}_k^{(m,n)}$，其中 $m\in [1,M]$，$n\in [1,N]$。

$$图1禾赛64线激光雷达竖直排列的64个激光发射器$$

  Velodyne HDL64E 激光雷达的 $M=64，N=4500$，因此它扫描一次采集的点数是 $64\times 4500=28.8$ 万。采集过程中，这一列激光发射器绕着竖直方向旋转，旋转一周采集到的点构成了一帧点云。

$$图2威力登64线激光雷达参数表（左）和采集到的点云（右）$$

  水平分辨率是 $360÷N=0.08$°，竖直分辨率是 $26.8÷64\approx 0.4$°。所以点在水平方向上较稠密，在竖直方向上较稀疏。因此我们看到的点云是竖直方向上相互平行的圆环，如图 2 的右图。

1.2 特征提取

---

  ICP 算法使用原生点云做匹配。每次扫描采集到的点有数万个，而且噪声和动态目标会使待匹配的两帧扫描点不能完全匹配。这使得 ICP 算法效率低且健壮性差。

  特征点匹配算法从传感器获取的原生点云中选择平面特征和边缘特征做匹配，噪声和不重要的点被丢弃。因此，它在实践中更健壮且更高效。

  传感器采集的点在竖直方向上稀疏，在水平方向上稠密，即 $M$ 小于 $N$。因此，水平方向的特征更独特（distinctive），水平方向上的特征检测更不容易出现错误。于是，对每一个点云 ${\mathbf{p}}_k^{(m,n)}$，F-LOAM 沿水平方向计算局部表面（可能是平面也可能是边缘）的平滑度：
$${\sigma }_k^{(m,n)}=\frac{1}{|{\mathcal{S}}_k^{(m,n)}|}\sum _{{\mathbf{p}}_k^{(m,j)}\in {\mathcal{S}}_k^{(m,n)}}(||{\mathbf{p}}_k^{(m,j)}-{\mathbf{p}}_k^{(m,n)}||),\text{\hspace{1em}(1)}$$

其中 ${\mathcal{S}}_k^{(m,n)}$ 是 ${\mathbf{p}}_k^{(m,n)}$ 在水平方向上的临近点，$|{\mathcal{S}}_k^{(m,n)}|$ 是临近点的数量。根据编号 $n$ 可以很容易地获取一个点的临近点 ${\mathcal{S}}_k^{(m,n)}$，且计算量比局部搜索更小。

  实验中，F-LOAM 分别沿顺时针方向和逆时针方向各选 5 个点，也就是选择 ${\mathbf{p}}_k^{(m,n\pm 5)}$ 这 10 个点作为 ${\mathcal{S}}_k^{(m,n)}$，于是 $|{\mathcal{S}}_k^{(m,n)}|=10$。所以，此时的局部表面是一个长方形，它的高度仅容纳 1 行点，宽度容纳 10 列点。

  从公式 （1）可以看出，平滑度 $\sigma$ 在墙之类的平面处较小，在角或者边缘处较大。因此，我们可以在一个扫描平面 ${\mathcal{P}}_k$ 中，使用较小的 $\sigma$ 值筛选出平面特征点 ${\mathcal{S}}_k$，使用较大的 $\sigma$ 值筛选出边缘特征点 ${\mathcal{E}}_k$。

2. 运动估计与畸变补偿

---

  激光雷达在一个时间周期内扫描一次，后一列点的采集时间晚于前一列。采集时，如果激光雷达相对采集场景运动，则采集到的 $N$ 列点云会有 $N$ 个各不相同的位姿，这就是运动畸变。

  激光雷达可以提供每个点在雷达坐标系中的坐标以及采集时间戳，通常认为时间戳相同的点来自同一个坐标系（位姿相同）。

  祛畸变，或者称为畸变补偿，就是求这 $M\times N$ 个点在同一个坐标系中的坐标。畸变补偿的方法有：

1. 使用高频里程计。如果里程计的位姿估计频率足够高，就可以在一次扫描过程中，分多次估计传感器采集到的点的位姿。这种方法对算力有较高要求。
2. 使用惯性传感器 IMU。可以之间测量旋转。这种方法需要额外的传感器。
3. 匀速运动模型。

  LOAM 和 LeGO-LOAM 使用 scan-to-scan 匹配算法，以迭代的方式估计两个相邻扫描间的变换直到收敛，从而修正畸变。F-LOAM 使用第三种畸变补偿算法，计算量更小，精度却与迭代法相当。该算法有以下两步：

1. 第一步：使用匀速运动模型得到粗略的位姿。使用粗略的位姿祛畸变，即把一次扫描采集到的点变换到同一个坐标系。使用祛畸变的点估计精确的位姿。见 2.1 和 2.2 节。
2. 第二步：使用精确的位姿再次祛畸变。见第 4 节。

2.1 匀速运动模型

---

  现在的三维激光雷达频率都高于 10 赫兹，所以两次相临扫描间的时间间隔非常短。因此，可以假设传感器在短时间内的运动是匀速运动：角速度是常量的，速度是线性的。这个假设可以帮助我们预测传感器的运动，从而修正畸变。

  假设传感器在第 $k$ 次扫描时的位姿是 ${\mathbf{T}}_k$。这个位姿可以是该扫描周期内任意某个时刻的位姿，这里我们采用扫描开始时的位姿。在匀速运动的假设下，${\mathbf{T}}_{k\to k-1}$ 等于 ${\mathbf{T}}_{k-1\to k-2}$：
$${\xi }_{k-1}^k=log({\mathbf{T}}_{k\to k-1})=log({\mathbf{T}}_{k-1\to k-2})=log({\mathbf{T}}_{k-2}^{-1}{\mathbf{T}}_{k-1}),\text{\hspace{1em}(2)}$$

其中，${\xi }_{k-1}^k\in \mathfrak{s}e(3)$ 是 ${\mathbf{T}}_{k\to k-1}$ 的李代数表示。函数 $log(\cdot )$ 是从齐次变换矩阵到李代数的映射，使用李代数表示位姿的好处是可以进行线性运算。公式（2）第 3 个等号的推导见公式（5.2）。两次扫描间的时间间隔 $\delta t$ 很短，其变换可以使用线性插值计算出来：

$${\mathbf{T}}_k(\delta t)={\mathbf{T}}_{k-\delta t}={\mathbf{T}}_{k-1}{\mathbf{T}}_{k-\delta t\to k-1}={\mathbf{T}}_{k-1}exp(\frac{N-n}{N}{\xi }_{k-1}^k),\text{\hspace{1em}(3)}$$

其中函数 $exp(\xi )$ 是李代数到李群的映射。公式（3）是什么意思呢？假设 $k$ 和 $k-1$ 分布对应时刻 $t$ 和 $t-1$，则 ${\mathbf{T}}_k(\delta t)$ 表示传感器在时刻 $t-1+\delta t$ 的位姿，${\mathbf{p}}_k^{(m,n)}$ 是传感器在时刻 $t-1+\delta t$ 采集到的点，其中 $0\le \delta t\le 1$。

  至此，我们就分别求得一个扫描内各时刻（其实是各极短时间段内）采集到的点的位姿。

2.2 祛畸变

---

  现在我们要把一个扫描内位姿不同的点变换到同一个坐标系中，也就是本次扫描开始时的传感器坐标系中。当前扫描 ${\mathcal{P}}_k$ 的所有点在开始扫描时的传感器坐标系中的坐标计算方法如下：

P ~ k = { T k ( δ t ) p k ( m , n )   ∣   p k ( m , n ) ∈ P k } . (4) \tilde{\mathcal{P}}_k = \{ {\bf T}_k (\delta t) {\bf p}_k^{(m, n)} \, | \, {\bf p}_k^{(m, n)} \in \mathcal{P}_k \}. \tag{4} P~k​={Tk​(δt)pk(m,n)​∣pk(m,n)​∈Pk​}.(4)

其中顶部的波浪线 ~ 表示祛畸变后的、无失真的。经过公式（4）的祛畸变，我们就可以使用一个位姿 ${\mathbf{T}}_k$ 表示第 $k$ 次扫描采集到的所有点的位姿。使用祛畸变的特征有利于我们估计出更精确的位姿。

3. 位姿估计

---

  位姿估计算法把公式（4）得到的祛畸变后的边缘特征 ${\tilde{\mathcal{E}}}_k$ 和平面特征 ${\tilde{\mathcal{S}}}_k$ 与全局特征地图对齐。

  全局特征地图包含一个边缘特征地图和一个平面特征地图，这两个地图是被分开更新维护的。为了降低搜索代价，边缘特征地图和平面特征地图都以三维 kd 树的方式存储。

  和参考文献 19 一样，全局直线和全局平面通过边缘特征地图和平面特征地图中的临近点估计出来：

  对于每个边缘特征点 ${\mathbf{p}}_{\mathcal{E}}\in {\tilde{\mathcal{E}}}_k$，计算它在全局边缘特征地图中的临近点的协方差矩阵。如果这些点分布在一条直线上，协方差矩阵就会包含一个很大的特征值。把最大的特征值对应的特征向量 ${\mathbf{n}}_{\mathcal{E}}^g$ 看作直线的方向，把直线 ${\mathbf{p}}_{\mathcal{E}}^g$ 的位置看作临近点的几何中心。这样就得到一个全局直线。

  类似的，对于每个平面特征点 ${\mathbf{p}}_{\mathcal{S}}\in {\tilde{\mathcal{S}}}_k$，使用坐标 ${\mathbf{p}}_{\mathcal{S}}^g$ 和曲面法线 ${\mathbf{n}}_{\mathcal{S}}^g$ 可以计算出一个全局平面。与全局直线不同的是，全局平面的法线被看作是最小特征值对应的特征向量。

  一旦获得相应的最优边缘 / 平面，我们就能从 ${\tilde{\mathcal{P}}}_k$ 中找到每个特征点的全局边缘或全局平面。利用这个对应关系，最小化特征点与全局边缘 / 平面间的距离，可以估计当前帧与全局地图的最优位姿。边缘特征到全局边缘的距离是：

$$f_{\mathcal{E}}({\mathbf{p}}_{\mathcal{E}})={\mathbf{p}}_{\mathbf{n}}\cdot (({\mathbf{T}}_k{\mathbf{p}}_{\mathcal{E}}-{\mathbf{p}}_{\mathcal{E}}^g)\times {\mathbf{n}}_{\mathcal{E}}^g),\text{\hspace{1em}(5)}$$

其中 $\cdot$ 是点乘，${\mathbf{p}}_{\mathbf{n}}$ 是单位向量：

$${\mathbf{p}}_{\mathbf{n}}=\frac{({\mathbf{T}}_k{\mathbf{p}}_{\mathcal{E}}-{\mathbf{p}}_{\mathcal{E}}^g)\times {\mathbf{n}}_{\mathcal{E}}^g}{||({\mathbf{T}}_k{\mathbf{p}}_{\mathcal{E}}-{\mathbf{p}}_{\mathcal{E}}^g)\times {\mathbf{n}}_{\mathcal{E}}^g||}.\text{\hspace{1em}(6)}$$

平面特征到全局平面的距离是：

$$f_{\mathcal{S}}({\mathbf{p}}_{\mathcal{S}})=({\mathbf{T}}_k{\mathbf{p}}_{\mathcal{S}}-{\mathbf{p}}_{\mathcal{S}}^g)\cdot {\mathbf{n}}_{\mathcal{S}}^g.\text{\hspace{1em}(7)}$$

  传统的特征匹配算法只优化上面提到的几何距离，并不考虑每个特征点的局部几何分布。然后我们发现，较高局部平滑度的边缘特征和较低局部平滑度的平面特征对匹配更重要。因此提出权重函数平横匹配算法。为了降低计算复杂度，使用公式（1）定义的局部平滑度定义权重函数。假设边缘点 ${\mathbf{p}}_{\mathcal{E}}$ 的局部平滑度是 ${\sigma }_{\mathcal{E}}$，平面点 ${\mathbf{p}}_{\mathcal{S}}$ 的局部平滑度是 ${\sigma }_{\mathcal{S}}$，它们的权重定义如下：

$$\{\begin{array}{cc}W({\mathbf{p}}_{\mathcal{E}})& =\frac{exp(-{\sigma }_{\mathcal{E}})}{{\sum }_{{\mathbf{p}}^{(i,j)}\in {\tilde{\mathcal{E}}}_k}exp(-{\sigma }_k^{(i,j)})}\\ W({\mathbf{p}}_{\mathcal{S}})& =\frac{exp(-{\sigma }_{\mathcal{S}})}{{\sum }_{{\mathbf{p}}^{(i,j)}\in {\tilde{\mathcal{S}}}_k}exp({\sigma }_k^{(i,j)})}\end{array}.\text{\hspace{1em}(8)}$$

其中 ${\mathcal{E}}_k$ 和 ${\mathcal{S}}_k$ 是第 $k$ 次扫描采集到的边缘特征和平面特征。新位姿通过最小化带权重的点边距离 $f_{\mathcal{E}}({\mathbf{p}}_{\mathcal{E}})$ 与点面距离 $f_{\mathcal{S}}({\mathbf{p}}_{\mathcal{S}})$ 的和计算出来：

$$\underset{{\mathbf{T}}_k}{\min }\sum W({\mathbf{p}}_{\mathcal{E}})f_{\mathcal{E}}({\mathbf{p}}_{\mathcal{E}})+W({\mathbf{p}}_{\mathcal{S}})f_{\mathcal{S}}({\mathbf{p}}_{\mathcal{S}}).\text{\hspace{1em}(9)}$$

使用高斯牛顿法求解非线性方程，即可得到最优位姿。雅可比矩阵可以用 $\delta \xi \in \mathfrak{s}e(3)$ 的左扰动模型得出：

$${\mathbf{J}}_p=\frac{\partial \mathbf{T}\mathbf{p}}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{exp(\delta \xi )\mathbf{T}\mathbf{p}-\mathbf{T}\mathbf{p}}{\delta \xi }=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -[\mathbf{T}\mathbf{p}{]}_{\times }\\ 0_{1\times 3}& 0_{1\times 3}\end{array}\right],\text{\hspace{1em}(10)}$$

其中 $[{\mathbf{T}}_k{\mathbf{p}}_k{]}_{\times }$ 把四维齐次坐标 { x , y , z , 1 } \{x,y,z,1\} {x,y,z,1} 转换成三维坐标 { x , y , z } \{x,y,z\} {x,y,z}，并计算它的斜对称矩阵。边缘残差的雅可比矩阵可以这样计算：

$${\mathbf{J}}_{\mathcal{E}}=W({\mathbf{p}}_{\mathcal{E}})\frac{\partial f_{\mathcal{E}}}{\partial \mathbf{T}\mathbf{p}}\frac{\partial \mathbf{T}\mathbf{p}}{\partial \delta \xi }=W({\mathbf{p}}_{\mathcal{E}}){\mathbf{p}}_{\mathbf{n}}\cdot ({\mathbf{n}}_{\mathcal{E}}^g\times {\mathbf{J}}_p),\text{\hspace{1em}(11)}$$

类似地，

$${\mathbf{J}}_{\mathcal{S}}=W({\mathbf{p}}_{\mathcal{S}})\frac{\partial f_{\mathcal{S}}}{\partial \mathbf{T}\mathbf{p}}\frac{\partial \mathbf{T}\mathbf{p}}{\partial \delta \xi }=W({\mathbf{p}}_{\mathcal{S}}){\mathbf{n}}_{\mathcal{E}}^g\cdot {\mathbf{J}}_p.\text{\hspace{1em}(12)}$$

  根据上面的公式求解非线性优化就可以得到里程计估计结果。然后使用该结果计算新的对应关系和新的里程计。反复迭代，直到收敛，即可计算出当前位姿估计 ${\mathbf{T}}_k^{\ast }$。

4. 地图创建与畸变补偿更新

---

  全局地图包含一个全局边缘地图和一个全局平面地图，且基于关键帧更新。当平移大于预设阈值或旋转大于预设阈值时，就会选择每一个关键帧。相比逐帧更新，基于关键帧的地图更新计算量更小。第 2 部分提到，为了降低计算量，畸变补偿基于匀速运动模型而不是迭代运动估计。但这样的精度没有迭代运动补偿高。因此，在第而阶段，基于优化后的 ${\mathbf{T}}_k^{\ast }$ 再次计算畸变：

$$\Delta {\xi }^{\ast }=log({\mathbf{T}}_{k-1}^{-1}\cdot {\mathbf{T}}_k^{\ast }),\text{\hspace{1em}(13a)}$$

P ~ k ∗ = { e x p ( N − n N ⋅ Δ ξ ∗ ) p k ( m , n )   ∣   p k ( m , n ) ∈ P k } . (13b) \tilde{\mathcal{P}}_k^* = \{ exp(\frac{N - n}{N} \cdot \Delta \xi^*) {\bf p}_k^{(m,n)} \,|\, {\bf p}_k^{(m,n)} \in {\mathcal P}_k \}. \tag{13b} P~k∗​={exp(NN−n​⋅Δξ∗)pk(m,n)​∣pk(m,n)​∈Pk​}.(13b)

重新计算的祛畸变边缘特征和平面特征分别更新到全局边缘地图和全局平面地图。然后使用三维体素栅格对地图下采样，以避免内存溢出。

5. 附录

---

5.1 位姿变换的传递性

---

  假设 $\mathbf{T}$ 是 4 阶齐次变换矩阵。从坐标系 $k$ 到坐标系 $k-1$ 的变换可以用 ${\mathbf{T}}_{k\to k-1}$ 表示，从坐标系 $k$ 到世界坐标系的变换通常简写成 ${\mathbf{T}}_k$。

  假设坐标系 $0$ 是世界坐标系，一个点在 $k$ 坐标系中的坐标是 $p_k$，则它在世界坐标系中的坐标：
$$p_0={\mathbf{T}}_k{p}_k,\text{\hspace{1em}(5.1)}$$

其中 $p_0$ 和 $p_k$ 都是 4 维齐次坐标 $(x,y,z,1)$。需要注意的是，${\mathbf{T}}_k$ 在 $p_k$ 左侧而不是右侧。${\mathbf{T}}_{k\to k-1}$ 可以使用 ${\mathbf{T}}_k$ 和 ${\mathbf{T}}_{k-1}$ 表示：
$${\mathbf{T}}_{k\to k-1}={\mathbf{T}}_{0\to k-1}{\mathbf{T}}_{k\to 0}={\mathbf{T}}_{k-1\to 0}^{-1}{\mathbf{T}}_{k\to 0}={\mathbf{T}}_{k-1}^{-1}{\mathbf{T}}_k.\text{\hspace{1em}(5.2)}$$

6. 参考

---

1. F-LOAM 论文，arxiv，2021。
2. 翻译，CSDN。
3. 畸变补偿，CSDN。
4. 畸变补偿，知乎专栏。