Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small Fo

Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV

Points Selection And Feature Extraction 选点和提取特征

A. Points selection 选点

B. Feature extraction 提取特征

Iterative Pose Optimization 迭代位姿优化

A. Residual of edge-to-edge 边到边

我们使用${\xi }_k$和${\xi }_m$分别表示所有的在当前帧和地图的所有边线特征。对于每一个${\xi }_k$中的点，我们在${\xi }_m$中找到最近的5个点。为了加速搜索的速度，我们建立了一个${\xi }_m$的kdtree。而且，这个kd-tree的线程是一旦收到上一针registered 帧或者子帧就会被另一个并行的线程构建。这样做的话当接收到新数据的时候，KD-tree就可以立马被用上。

我们用符号$P_l$代表当前帧内${\xi }_k$的一个点。有一点我们需要注意的是，点$P_l$是在当前雷达局部坐标系的，但是${\xi }_m$表示的是全局地图坐标系里的点，为了找到$P_l$在${\xi }_m$里面最近的点，我们需要把局部雷达坐标系投影到全局坐标系里面，公式如下：

这里的雷达位姿是当前帧内最后一个点被扫描到的时候，同时需要被位姿优化，即还没优化的时候的位姿。这里我们使用雷达一帧内收到最后一个点的位姿来代表雷达整个一帧的位姿，并且所有的点都会基于整个位姿被投影到全局的地图里。同时需要注意的是当前帧的最后一个点实际上就是下一帧的第一个点。（这里文章为什么会这么写？？？？，难道是因为livox扫描的pattern特殊？）

我们用$P_i$表示$P_w$里第i个在${\xi }_m$里最近的点。为了保证$P_i$确实是在一条线上，我们计算$P_w$内m个最近的点均值和方差。本论文里设m为5。如果最大的特征值是第二大特征值的三倍还多，我们认为$P_w$里面最近的点会构成一条线，同时$P_w$会位于这条线上。残差可以这样计算：

这里的部分比loam那里简洁不少

B. Residual of plane-to-plane 平面到平面

和边特征点相似的是，对于当前帧内平面特征集合里面的一个点，我们找到5个地图内最近的平面特征的点。我们同样需要确定5个最近点确实是在同一个平面内，需要计算他们的协方差矩阵。如果最小的特征值是第二小的特征值小三倍，我们计算这两个平面的误差距离如下：

C. Inframe motion compensation 帧内运动补偿

帧内运动补偿
3D点由于是在不同时刻扫描到的，同时还伴随着雷达不停的运动。为了消除运动畸变的影响，我们提出如下两种方法：

C.1 Piecewise Processing 分段处理

一个简单且高效的消除运动畸变的方法就是分段处理。我们将传来的帧分为三个连续的子帧，然后将这三个子帧独立匹配到迄今为止积累的同一张地图。在每一个子帧扫描的期间，所有的点都会被投影到全局地图根据子帧终点的雷达位姿。这样做的话，每一个子帧的时间间隔是原帧的1/3。 尽管这个方法看起来很简单，但是效果出奇的好。另外，这种分段处理的好处是通过并行匹配每个子帧来利用现代 CPU 中的多核结构。

C.2 Linear interpolation 线性插值

另一个常用的运动补偿的方法就是线性插值。我们使用$(R_k,T_k)$表示当前帧的最后一个点对应的雷达位姿，$(R_{k-1},T_{k-1})$表示前一帧，用$(R_{k-1}^k,T_{k-1}^k)$代表两帧之间的相对运动，这样的话：

假设$T_{k-1}$是在之前帧最后一个点的采样时间。对于任何一个在当前帧采样时刻t内的一个点，我们可以计算$s=(t-t_{k-1})/(t_k-t_{k-1})$, 然后位姿可以线性的插值在t时刻：

对于k-1时刻的位姿，当前时间的lidar位姿计算如下：

我们可以通过插值的位姿投影t时刻的点到全局坐标系，表示如下：

D. Outliers rejection, dynamic objects filtering

为了避免移动的物体在真实环境下带来的扫描匹配的精度，我们实现了一个动态目标滤除的方法如下：我们重新找到每一个特征点的最近点，同时添加边到边的残差和平面到平面的残差对于目标的function，我们先迭代一小轮的位姿优化。真个算法如下表示：