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

LOAM LIVOX: A highly efficient, dependable, and ultra-precise LiDAR odometry and mapping suite intended for LiDAR systems with a narrow field of view.

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

A. Points selection 选点

B. Feature extraction 提取特征

Iterative Pose Optimization 迭代位姿优化

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

我们采用ξk和ξm分别代表当前帧和地图中所有的边线特征信息。针对每个ξk中的点，在对应的ξm中查询其最近邻区域内的5个近邻点。为了提高搜索效率，在构建过程中我们采用了基于kd-tree的数据结构来加速计算速度。值得注意的是，在接收上一针注册或子帧事件时（即新数据到达），系统会立即触发相关处理流程以更新KD-tree结构并开始下一步运算

以符号PlP_l的形式表示当前帧内ξk的一个位置点值得注意的是该位置点PlP_l位于当前雷达本地坐标系统中而ξm则表示全局地图坐标的点为了找到与PlP_l最近的ξm中的位置点我们需要将本地雷达坐标系统投影到全球坐标系统中具体操作步骤如下公式所示

在当前处理过程中，在当前处理过程中，在当前处理过程中，在当前处理过程中，在当前处理过程

我们以Pi_Pi的形式代表从集合{ P_w }中选取ξ_m空间中的第i个最近邻点。为了确保这些选中的点确实位于同一直线上,我们需要计算{ P_w }空间内所有前m个最近邻点的均值与方差作为辅助指标,其中默认设置m=5（见附录A）。当主成分分析结果表明最大主因子是次大主因子的大约三倍以上时,则推断这些最近的点将形成一条直线,并且认为{ P_w }空间中的该区域将位于这条直线上（具体公式见附录A）。其残差可通过以下方式计算得出：

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

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

类似于边特征点的是，在当前帧内的平面特征集合中选取一个特定点时，在地图中寻找与之最近并属于同一空间区域范围内的五个平面目标候选物征点，并对这五个候选目标征集进行验证性分析

C. Inframe motion compensation 帧内运动补偿

针对帧内的运动补偿问题进行研究，在实际应用中需要考虑到多个复杂因素。首先，在运行过程中不仅需要处理三维空间中的点位定位问题，并且在数据采集过程中还伴随有雷达持续性的移动情况。其次，在实际应用中还需要考虑到这些因素带来的影响以及相互间的制约关系。为此提出了以下两种解决方案：一种是基于时空一致性约束的方法；另一种是基于几何优化模型的方法

C.1 Piecewise Processing 分段处理

一种简洁而高效的消除运动畸变方案即为分阶段处理。我们将接收的所有帧划分为三个连续的子序列，并分别将这三个子序列独立地匹配到迄今为止积累的地图上。所有跟踪点在每个时间段内都会被投影至基于当前时间段终点位置确定的地图上，并使该时间段内的所有点均映射至该地图中对应位置的所有点上。这样做的结果则是将每个时间段的时间长度缩短为原来的三分之一。虽然这种方法看似简单但其效果令人意外地出色然而这种分阶段处理的优势在于能够充分利用多核处理器的优势

C.2 Linear interpolation 线性插值

另一种常见的用于运动补偿的技术是线性插值法。其中(R_k, T_k)被用来表示当前帧的最后一个雷达位姿点；而前一帧则由(R_{prev}, T_{prev})代表；其相对运动由(R^ k_prev, T^ k_prev)给出

假设Tk−1T_{k-1}是前一帧最后一个采样点的时间。对于任何当前帧采样时刻tt内的点都可以计算出s=(t - t_{k-1}) / (t_k - t_{k-1})；随后，在该时间点处可以通过线性插值确定位姿

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

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

D. Outliers rejection, dynamic objects filtering

为了解决移动物体在真实环境中的扫描匹配精度问题, 我们开发了一种动态目标滤除方法, 并详细描述了其实现流程. 该方法的核心在于重新定位每个特征点在其邻近区域中的最优对应位置. 同时, 我们引入了边与边之间的残差以及平面与平面之间的残差来补充目标函数的信息. 随后进行了一次粗略优化阶段以提升算法的整体效率.