LOAM: Lidar Odometry and Mapping in Real-time

目录

文章主要算法分为5个小部分：

A. Feature Point Extraction

定义评价表面平滑程度smoothness的计算方法如下：

根据smoothness程度选取关键特征点，一帧内最大的几个点为edge points，一帧内比较小的几个点为planar points。
为了均匀的分配环境的特征点，我们把一根扫描线scan分为4块相同的子区域。每一个子区域提供最大2个边线点和4个平面点。一个点能被选成一个边线或者平面点的条件是：只有它的c值大于或者小于一个阈值，同时被选中的点的数目不能超过最大值。
两种情况下的点不能被选为特征点：

1. 平行于雷达发射laser beam的点不能选

2. 在遮挡处的角点不能选，换了个角度可能就是一个普通可观测的点

B. Finding Feature Point Correspondence

论文里面讲的是里程计估计的是一帧内(a sweep)的运动，用$T_k$表示一帧数据k的开始。在每一帧扫描的末尾，会把这期间扫描到的点云$P_k$ 重投影到第k+1帧$t_{k+1}$，同时用符号$\overline{P_k}$表示重投影的点云。在下一帧k+1扫描的时候，$\overline{P_k}$和$P_{k+1}$被一起用来估计lidar的运动。

假设现在$\overline{P_k}$和$P_{k+1}$两帧数据都有了，然后现在通过这两帧数据来找相关性correspondences。根据$P_{k+1}$数据，我们可以通过上一节提到的方法来找到边界点和平面点。我们用${\xi }_{k+1}$和$H_{k+1}$分别表示边界点和平面点的集合。我们可以从$\overline{P_k}$找到边界线与${\xi }_{k+1}$里面的点对应，平面点和$H_{k+1}$中的点对应。

在第K+1帧数据sweep刚开始扫描的时候，$P_{k+1}$还是一个空的集合。当随着雷达扫描的时候，$P_{k+1}$里面的点慢慢变多。随着$P_{k+1}$集合里面的点慢慢变多，里程计递归的估计雷达6自由度的姿态运动估计。在每次迭代的过程中，${\xi }_{k+1}$和$H_{k+1}$都会被重投影到当前的位姿估计下的帧的开始处。我们用${\tilde{\xi }}_{k+1}$和${\tilde{H}}_{k+1}$分别表示重投影后的点云。在${\tilde{\xi }}_{k+1}$和${\tilde{H}}_{k+1}$里面的每一个点，我们会基于3D-kdtree的结构的$\overline{P_k}$里找到最近的近邻点。

下图表示的是寻找边缘线作为边缘点的对应的过程。我们用i表示${\tilde{\xi }}_{k+1}$其中的一个点。边线可以用两个点来表示。
我们用j表示i在$\overline{P_k}$里面的最近邻的点，同时用l表示i在两次连续扫描中j中的最近邻的点，同时用（j，l）的形式表示和i的相关性。然后为了验证j和l都是边线上的点，我们根据前面的方法来检查表面的平滑程度。我们尤其需要j和l来自不同的扫描线因为一根扫描线不可能包含超过一个点来自同一条边界线。除非这个边界线是在扫描平面上的。如果这样的话，边界线会出现退化，同时看起来像扫描平面上的一条直线，同时在这条线上的特征点一开始也不会被提取。

下图显示了寻找平面数据作为平面点的对应的过程。我们用i表示${\tilde{H}}_{k+1}$里面的一个点，平面特征需要用三个点表示。我们找到i在$\overline{P_k}$里面最近的点，就用j表示。然后我们找到其他两个点，分别是l和m，一个是和j在通一个scan上，另一个在j所在的scan相邻的scan上。这个保证了三个点是non-collinear非共线的，为了验证这三个点都是平面点，我们再一次检查这三个点的平滑程度。

当找到特征点的相关性之后，我们推导一种表达式用来计算特征点到其相关点距离的公式。我们会最小化这些特征的距离来得到雷达的相对运动。我们开始从边点特征开始，对于一个属于${\tilde{\xi }}_{k+1}$的点i，如果（j, l）是相关的边线，这样点到线的距离公式可以这样计算：

如果是平面特征点的话，误差函数可以这样计算：

C. Motion Estimation

雷达的运动在一个sweep内可以建模成常角速度和线速度。这样做允许我们在同一个sweep内对不同的位姿变换进行线性插值。我们用t表示当前的时间戳，然后用$t_{k+1}$表示下一个k+1 sweep开始的时间戳。我们用$T_{k+1}^L$表示$t_{k+1}$和t之间的雷达位姿变换。$T_{k+1}^L$包含了雷达6自由度的刚性变换。

假如在k+1帧点云$P_{k+1}$内存在一个点i, 同时用$t_i$表示其时间戳，同时用$T_{k+1,i}^L$表示这两个时间戳之间的相对变换，因此第i个点的位姿变换可以通过线性插值的方法得到$T_{k+1,i}^L$，插值公式如下：
$T_{(k+1,i)}^L=(t_i-t_{k+1})/(t-t_{k+1})\ast T_{k+1}^L$

我们仍然使用${\xi }_{k+1}$和$H_{k+1}$表示从$P_{k+1}$点云提取出来的边点和平面点。${\tilde{\xi }}_{k+1}$和${\tilde{H}}_{k+1}$依然是将这些点投影到$t_{k+1}$ Sweep帧开始的时候的点集合。为了求解出雷达的运动关系，我们在${\xi }_{k+1}$和${\tilde{\xi }}_{k+1}$，$H_{k+1}$和${\tilde{H}}_{k+1}$之间建立一个几何关系。根据插值公式，我们可以得到如下的公式: $X_{(k+1,i)}^L=R{\tilde{X}}_{(k+1,i)}^L+T_{(k+1,i)}^L(1:3)$

根据上面的公式，我们可以推导出一个边点和边线的的几何关系。相同的，我们还可以推导出一个平面点和相关平面点的几何关系
最后，我们通过Levenberg-Marqiard方法求解雷达的运动，整合上面的公式，对于每一个特征点来说，我们获得了一个非线性的公式$f(T_{k+1}^L)=d$， f的一行和一个特征点相关，同时d包含相关的距离。我们对$T_{k+1}^L$求雅阁比矩阵，用符号J表示，这里$J=deltaf/delta{T}_{k+1}^L$。 然后可以把d 误差最小化到0 通过非线性迭代。

$\lambda$是一个被Levenberg-Marquardt 方法决定的因子。

D. Lidar Odometry Algorithm

lidar 里程计算法如上图所示。算法的输入是上一次扫描sweep的点云数据$\overline{P_k}$和当前扫描的点云数据$P_{k+1}$，还有上次递归的位姿变换$T_{k+1}^L$。如果一帧新的数据开始了，$T_{k+1}^L$位姿会清零。然后算法会从$P_{k+1}$点云中构建${\xi }_{k+1}$和$H_{k+1}$。对于每一个特征点，我们找到其相关的在$\overline{P_k}$。运动估计被适配成一个鲁棒的拟合问题。算法会分配一个双方的权重在每一个特征点上。特征相关性距离越大的会附上越小的权重，如果距离大于一定的阈值会被认为是离群点，同时会附上0权重。然后位姿会被更新，一旦收敛条件满足或者达到最大优化次数，非线性的优化会结束。一旦算法到达了一次扫描的尾部，$P_{k+1}$会被投影到$t_{k+2}$帧时间戳。否则，只有位姿$T_{k+1}^L$会返回为下一次递归做准备。

E. Lidar Mapping

建图算法比里程计算法的频率更低，同时只是一次sweep调用一次。在第k+1帧的末尾的时候，lidar odometry会生成一帧无畸变的点云${\bar{P}}_{k+1}$ 和一个pose $T_{k+1}^L$，包含lidar在一帧扫描期间的运动。建图算法匹配和配准${\bar{P}}_{k+1}$在世界坐标系中，如图所示。我们使用$Q_k$表示地图中的点云，累计直到sweep k同时用$T_k^W$ 表示雷达在帧的末尾在地图中位姿。根据里程计的输出结果，建图算法扩展$T_k^W$对于k+1帧的扫描到k+2帧的扫描。为了获取$T_{k+1}^W$，${\bar{P}}_{k+1}$会被投影到世界坐标系，用${\bar{Q}}_{k+1}$表示，然后算法匹配两者。

特征点提取的方法和前面的章节提到的是一样的。但是数量是10倍。为了发现特征点的相关性，我们存储$Q_k$到10m的立方体的格子里。立方头内和${\bar{Q}}_{k+1}$相关的点被提取和存储在一个3D树里面。我们在特征点附近的一个特定区域内找到点，并用$S^{/}$表示这个周围点的集合。对于一个边的点，我们只保留$S^{/}$内边点的集合。对于一个平面点，我们只保留平面内的patch。然后我们计算$S^{/}$的协方差矩阵，用M表示。分别用V和E表示M的特征值和特征向量。如果$S^{/}$分布在一条边线上，V会包含一个明显比其他两个大的特征向量，同时E中的对应的特征向量代表线的朝向。另一方面，如果$S^{/}$分布在一个平面上，V会包含两个比较大的和一个明显比较小的特征向量，同理E中的对应特征向量代表面的朝向。同时边线的位置和平面的位置是由$S^{/}$的几何中心决定的。

为了计算特征点到其相关联的点的距离，我们选择线上的两个点，平面的三个点，这允许我们用前面的公式来计算距离。此处的非线性优化可以使用前面提到的优化方法来求解。

位姿变换的积分在框图9中可以看到。蓝色部分区域表示lidar建图后的位姿，在每一帧扫描完成之后。橙色区域部分表示的是雷达里程计的输出结果，以10hz左右的帧率。lidar相对于地图的位姿由这两种变换组成，和lidar 里程计的频率一样。

总结

第k帧的数据怎么投影到k+1帧的开始？