LOAM 原理及代码实现介绍[通俗易懂]

LOAM 原理及代码实现介绍

paper:《Lidar Odometry and Mapping in Real-time》
LOAM的参考代码链接：
A-LOAM
A-LOAM-Notes
LOAM-notes

使用传感器介绍：

如果没有电机旋转，则雷达自身的扫描是一个平面的180度。如下：

加上电机的旋转，则该设备能扫描的范围为雷达前方的半球形。

LOAM技术点

二维雷达固定在一个转轴上，实现3维雷达；一次完整的三维扫描为sweep，雷达平面的一次扫描为scan；
将点云分为两类：边线点(edge point角点)和平面点(planar point)，分别对应采取点到线及点到面的约束；在实际的测试中，平面点占总点云的80%。
位姿变化估计采用高频粗估计+低频优化。两套算法来实现定位。(laser odometry & mapping odometry)
其中，laser odometry高频粗估计是在雷达坐标系下，通过点到线及点到面距离，利用LM得到雷达位姿估计；
mapping odometry低频位姿精估计，是在世界坐标系下，通过点到线，点到面的距离，得到世界坐标系下的雷达位姿估计优化。
低频位姿精估计是1HZ，与sweep频率相同。也就是一次完整的sweep会进行一次完整的位姿估计。

LOAM整体框架

将定位与建图分开运行，高频位姿估计+低频优化建图->实现实时，低计算量，低漂移。

LOAM主要包括四个节点：点云提取(scan registration)、雷达里程计(lidar odometry)、雷达建图(lidar mapping) 和位姿集成(transform integration)

点云提取及处理（scan registration）：

根据点的曲率c来将点划分为不同的类别（边/面特征或不是特征），在每一个sweep中，根据曲率对点进行排序，来作为评价局部表面平滑性的标准。
一个sweep指完成一次完整的扫描，一次sweep分为多个scan，每一次sweep的雷达位姿定义为为这一sweep起始时的状态。

特征点提取及处理
LOAM中选用特征点来进行运动估计，特征点选取(edge point角点)和平面点(planar point)，如下黄点为edge point，红点为planar point。试验数据表明，大部分点云都是平面点。边线点起到的约束作用较小。

判断公式：

$P_k$ 第k次整体sweep中得到的点云。 $X^L_{(k,i)}$ 表示第k个sweep中的i点在雷达坐标系下的坐标。

设 $i\in P_k$ ，表示点 $i$ 是第k次整体sweep中的点。选取 $i$ 点在同一个scan中相邻的前后的5个点 $X^L_{(k,j)}, j \in S, j\ne i$ ，计算 $\Sigma _jX^L_{(k,i)}-X^L_{(k,j)}$ 。注意，论文中使用的是平面雷达，每个scan扫描区域是半平面，即，空间的平面，雷达一次scan会得到空间平面与雷达扫描平面交线上的点，会得到一条线的点，但是，如果雷达扫过角点，则就是类似平面v字型的点。）
即：
如果是平面，前后的5个点与i的距离相加=0，c->0
如果是边线角点，则 $c > 0$
c的计算方程的分母，用于归一化c值，防止，因为雷达的距离而带来的c值得过大或小。

可以参考LOAM论文和程序代码的解读中更详细的解释。

代码文件：scanRegistration.cpp：
接收雷达驱动发布的点云，解析驱动点云，计算曲率，并按序排布，按照曲率由高到低，将点云分为sharp，less_sharp，flat，less_flat四种点并发布对应的消息。
雷达驱动发布的点云topic为：/velodyne_points，其中点云的位置是以x,y,z存储的。根据xyz计算点云的仰角(angle)，及偏角(ori)，并对应的计算出点云对应的线号，存放在point.intensity = scanID + scanPeriod * relTime。其中scanID是整数，为点的线数(0-15)，relTime代表水平偏转度(0-1) scanPeriod=0.1。即将偏转度存为归一化的小数。线束为整数。
从而每个点包含的数据包括x,y,z,intensity，intensity则存放是竖直上的线束及水平上的偏转度。也就是包含位置信息的同时，包含了仰角及偏转角信息。

位姿粗估计(雷达里程计(lidar odometry))：（高频）
符号定义：
$P_k$ 第k次整体sweep中得到的点云；
根据插值将 $P_k$ 映射到k时刻sweep起始坐标系下(点云去掉运动漂移)，记为 $\widetilde{P_k}$ ，则根据插值将 $P_{k+1}$ 映射到k+1时刻sweep起始坐标系下(点云去掉运动漂移)，记为 $\widetilde{P_{k+1}}$ ;
将第k帧扫描到的特征点 $P_k$ 映射到第k+1帧的雷达坐标系下，记为 $\widetilde{P_{k+1}}$ .

点云匹配：
该作者的点云匹配是基于将 $\widetilde{P_{k+1}}$ 和 $\widetilde{P_{k+1}}$ 进行匹配。
即将第k个sweep的点云映射到第k+1个sweep起始，将第k+1个sweep的点云映射到第k+1个sweep的起始。匹配 $\widetilde{P_{k+1}}$ 和 $\widetilde{P_{k+1}}$ 的点云点面关系，计算得到第k+1个sweep中产生的运动。

点云特征点被分为，边线点和平面点。
边线约束点两个点确定：设去漂当前帧的点云 $i\in\widetilde{\xi_{k+1}}$ ，在上一帧点云(映射在k+1时刻坐标系下) $\overline{\xi_k}$ 中找到距离最近的点 $j$ ，并在 $\overline{\xi_k}$ 找到与j相邻scan的点 $l$ 。
设想两个墙面的竖直墙角线，设雷达偏移较小，相邻的的scan，会出现类似图a的情况，相邻scan会测量到墙角线上的点。因为可以在上一帧中找到两个相邻帧的墙角线上的点，约束当前帧打到墙角上的点到这两个点构成的直线上的距离最小，得到雷达运动T。如下图a所示。

平面约束点三个点确定：设去漂当前帧的点云 $i\in\widetilde{\mathcal{H}_{k+1}}$ ，在上一帧点云(映射在k+1时刻坐标系下) $\overline{\mathcal{H}_k}$ 中找到距离最近的点 $j$ ，并在 $\overline{\mathcal{H}_k}$ 找到与j同scan的最近点 $l$ 和相邻scan的一个点 $m$ 。

论文中的雷达是二维平面雷达加旋转得到，每一次scan都是一次平面半圆，一个sweep由加在雷达上的电机转动180度，构成半球。一次scan打到对面一个平面上，就是一排直线排列的点。

橙色线表示点 $j$ 所在的scan，蓝色线表示与橙色线相邻的两次scan的线。使用velonedy雷达，每个FOV对应的多线构成一个竖直scan，但是点云约束类似。

使用kdtree存储点云信息。

将第k帧扫描到的特征点 $P_k$ 映射到第k+1帧的雷达坐标系下，记为 $\overline{P_k}$ ：，将 $\overline{P_k}$ 与第k+1帧的扫描点云 $P_{k+1}$ 进行点云匹配。在LOAM代码中，计算 $\overline{P_k}$ 是使用TransformToEnd()将 ${P_k}$ 映射到本k时刻sweep的结束，即k+1时刻sweep的开始来得到的。
而当前时刻k+1时刻的点云映射到k+1时刻的初始是使用TransformToStart()实现。
假设k+1时刻的sweep中与前一k时刻的sweep的运动一致 $T^L_{k+1}$ 。使用位置插值方法得到整个sweep点云对应的 $T^L_{k+1,i}$ ，从而利用TransformToStart()，TransformToEnd()可将点云映射到sweep初始和结束时的坐标系下。

位姿插值
论文中，作者采用的是二维雷达加了一个电机旋转，每一次scan得到的点云的xyz是基于雷达的自身的坐标系，就是已经旋转后的雷达坐标系。
设雷达匀速旋转及匀速偏移，通过插值的方法，得到一个完整的sweep中，每一个scan对应的T(q，t)，并将点云坐标变换到每一次的sweep初始坐标系。消除点云在sweep中的因雷达运动而产生的误差，得到undistorted 的点云 $\widetilde P_k$ 。
详细实现在laserOdometry.cpp TransformToStart函数中。

对于边线点 $i\in P_{k+1}$ ，对应找到 $\overline{P_k}$ 相邻scan中最近的中两个点(j, l)，则两个点则确定了边线，计算变换矩阵T能使，点i到边线(j, l)的距离最小。j为最近点，然后再j所在scan的相邻scan中，找到l点。

距离约束：
点到线的距离计算公式如下：原理是目标点到两个原始点组成的两个向量构成的平行四边形面积/底边长度。

对于平面点 $i\in P_{k+1}$ ，对应找到 $\widetilde{P_k}$ 相邻scan中最近的中三个点(j, l，m)，则三个点则确定了平面，计算变换矩阵T能使，点i到平面的距离(j, l，m)最小。

点到面距离计算公式如下：原理：目标点到三个原始点组成的三个向量构成的斜方体/底面面积。
分子：第二行两个向量的叉乘结果值等于j，l，m三个点构成的三角形面积，方向垂直于平面向上。
结果点乘分子第一行，则可以得到i，j，l，m构成的斜三棱柱的体积。
分母：等同于分子第二行。
如下图所示：
即分别对应了点云匹配中的点到线及点到面的算法。

向量a×向量b=
　　| i j k |
　　|a1 b1 c1|
　　|a2 b2 c2|
　　=(b1c2-b2c1，c1a2-a1c2，a1b2-a2b1)

距离公式解释参见LOAM 论文及原理分析

这里列出平面点的距离约束计算对应代码：

// tripod1，tripod2，tripod3对应公式的j,l,m三点
tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];

//pa,pb,pc为公式中的分母项各分量(利用叉乘的公式得到)，pd为分母项的值
float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z)
   - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x)
   - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y)
   - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);

float ps = sqrt(pa * pa + pb * pb + pc * pc); 
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;

// pointSel对应公式的{\widetilde{X}}_{(k+1,i)}^L
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

pointSel对应公式的 ${\widetilde{X}}_{(k+1,i)}^L$

注：向量外积在数值上等于这两个向量构成的平行四边形的面积。
其中:
$\overline X_k=RX_{k-1}+T$ $\widetilde X_k=R_{inte}X_{k-1}+T_{inte}$
$R_{inte}和T_{inte}$ 为将每个sweep中不同的scan的雷达坐标变换，用于将sweep下每个scan都映射到sweep的起始坐标系。 $R_{inte}和T_{inte}$ 由 $R 和 T$ 插值得到。

而后，利用LM方法计算得到 $T^L_{k+1}$ ，得到该时刻的定位值(雷达里程计)

融合高频粗略的运动估计和优化后的位姿估计，得到准确位姿。根据位姿及得到的点云进行建图，创建点云地图。

代码文件：laserOdometry.cpp
ALOAM中实现代码比较清晰。根据scanRegistration发布的点云信息，寻找角点与平面点的匹配点，并构建点到线及点到面的约束方程，使用ceres进行求解。

点云使用pcl下的kdtree存储。先将点云都变换到sweep起始坐标系，然后在存放上一帧点云的kdtree中查找点云的最邻近点，在找到的最邻近点的相邻帧找到邻近点的最邻近的点。并构造距离方程。

通过迭代得到：q_last_curr，t_last_curr，即上一个sweep其实坐标系相对与这一个sweep的起始坐标系的变换。
累积便可的odom的输出。

雷达建图(lidar mapping): (低频) 一次完整sweep执行一次
地图创建
雷达地图创建采用点云地图，通过迭代得到的每一次sweep对应的雷达的世界坐标系下的位姿变换矩阵，得到点云的世界坐标坐标。
位姿精优化(mapping odometry)
使用的点云数量是高频odom输出的10倍，使用分块(cude)存储点云，但同时处理频率是odom的1/10。
将当前雷达所在位置为中心cube，submap为当前中心cube相邻水平左右2两个cube，竖直上下两个cube，深度前后1个cube中的点云(一个方向上5个cube)，将submap中的点云进行降采样，并筛除不在当前相机视野下的点云，作为target，以当前帧降采样后的特征点云为source的ICP过程。优化变量为里程计的运动估计误差矩阵 $T_{odom2map}$

精优化的点到线约束代码实现过程(A-LOAM)：
1. 选取点最近邻5个点，进行协方差矩阵特征值、特征向量计算，若其中一个特征值远大于其他特征值，则说明该点是边线点，其中最大的特征值对应的特征向量就是该线的方向向量。这个判断方法同NICP算法的平面法向量计算。解释参考
2. 利用得到的直线的方向向量在该点附近构造2个邻近点，同odom使用同样点到线的距离约束方程进行约束。
精优化的点到面约束代码实现过程(A-LOAM):
1. 选取最近邻5个点，设平面方程为ax + by + cz + 1 = 0，求解平面法向量X=(a,b,c)，将最近邻5个点带入平面方程，得到 $A X = B$ 的方程，其中A为有5个点云构成的5*3的矩阵， $B=[-1,-1,-1,-1,-1]^T$ ，使用QR分解得到平面法向量X=(a,b,c)。并对向量进行归一化得到(a’, b’, c’)。norm = matA0.colPivHouseholderQr().solve(matB0);
2. 点( $x_0,y_0,z_0$ )到平面的计算公式： $abs(ax_0 + by_0 + cz_0 + 1) /\sqrt{(a^2 + b^2 + c^2)}=a’x_0 + b’y_0 + c’z_0 + 1/norm$
这个点(角点/平面点)特征的区分不同于雷达里程计的输入的点云提取，是因为，在雷达里程计的计算中，舍去了参考意义小或重复的点云。

下图为雷达里程计的输出和laser mapping下优化位姿变换的输出的关系：
雷达里程计的输出是在雷达坐标系下，高频，有漂移；
地图最后输出位姿变化是全局的，高精度，低频。

代码文件：laserMapping.cpp
接收odom发来的odom定位估计以及去误差的点云 $P_k$ 映射到世界坐标系下，记为 $Q_k$ ，并将下采用后的点云存储在cubes中，就类似三维栅格，利用cubes将点云分块存放，每个cubes的点云。cubes的预留个数：（Width 21）*（Height 11）*（Depth 21）;

LOAM中Mapping线程中的帧与submap的特征匹配，用到的submap就是上图中的黄色区域，submap中的corner特征和surf特征在匹配中作为target(筛除无效点后存放在kdtree[Corner/Surf]FromMap)；而当前帧的单帧点云中的两种特征在匹配中作为source(代码中存放在laserCloudxxxStack2中)。
[centerCubeI,centerCubeJ,centerCubeK]为当前帧点云(source)的cube坐标。因为保证索引的非负性，所以计算时要加上laserCloudCenWidth[/Height/Depth]。

此外要保证【3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18】，因为要保证【submap为当前中心cube相邻水平左右2两个cube，竖直上下两个cube，深度前后1个cube中的点云(一个方向上5个cube)】，
如果[centerCubeI,centerCubeJ,centerCubeK]不满足以上条件，如centerCubeI<3时，则对应调节预留的cubes的分布位置，保证submap的正确提取。

以下图片来自https://www.cnblogs.com/wellp/p/8877990.html

MAP的ICP也是将点云分为边线点(edge/corner)和平面点(planner/sur)，来一起添加约束方程
边线点判断。边线点和平面点的分类，是通过计算点与周围点簇的协方差矩阵的特征值来判断的。
当最大的特征值远大于其他特征值，说明该点位于边线上；
当最小的特征值远小于其他特征值，说明该点位于平面上。

点云的曲率计算与odom一样，点云数量比odom多；
通过分析点云簇 $S^{'}$ 的协方差矩阵，分析边线及平面的方向；
A-LOAM 的laserMapping.cpp集成LOAM的transformMaintenance.cpp中的odom输出的高频及map输出的低频集成高频的定位。q_wmap_wodom ，t_wmap_wodom 来消除odom相对于map的偏差。
当odom定位输出后，如果q_wmap_wodom ，t_wmap_wodom有输出则更新，如何没有，则按先前的q_wmap_wodom ，t_wmap_wodom更新odom的输出，从而实现高频的map定位输出。

代码文件：lidarFactor.hpp
定义ceres的代价结构体及仿函数
LidarEdgeFactor
odom和map中点到线约束结构体及仿函数：使用2个临近点(j,l)确定直线，点到直线的距离作为约束

LidarPlaneFactor
odom点到面的约束结构体及仿函数：当前点p，使用3个邻近点(j,l,m)确定平面，使用(lp – lpj).dot(ljm)，NICP思路

LidarPlaneNormFactor
map精优化的点到面的约束及仿函数，就是点到面的计算公式： $abs(ax_0 + by_0 + cz_0 + 1) /\sqrt{(a^2 + b^2 + c^2)}=a’x_0 + b’y_0 + c’z_0 + 1/norm$

LidarDistanceFactor未用

退化问题

定位问题分为两步：1、位姿预测；2、位姿优化
1、位姿预测( $x_p$ )：使用imu预测姿态，或前端点云计算得到位姿；
2、位姿优化( $x_u$ )：使用非线性最小二乘优化方法得到的位姿。
退化问题解决思路：
退化问题主要出现在位姿优化，当出现退化现象时，舍弃退化方向的值，使用预测值(由其他传感器估计或者模型估计计算得到)来填充退化方向上的位姿解。

求解问题如下：
可使用如下方法求解：
将 $A x = b$ 两边左乘 $A^T$ ，得到 $A^TAx=A^Tb$ ，保证 $A^TA$ 满秩，即可逆，则可根据 $x=(A^TA)^{-1}A^Tb$ 解出 $x$ 。

作者定义退化因子 $\mathcal{D}=\lambda_{min}+1$ ，
其中 $\lambda_{min}$ 为 $A^TA$ 的特征值，特征值很小时，则表明该特征值对应的方向存在退化现象。
退化现象的表现：
(a)表示求解约束较好的情况，(b)退化情况，在解在蓝色方向上存在退化，表明，蓝色箭头方向解的不确定性大。

令 $v_1,…,v_m$ 对应的特征值小于阈值，因此被视为退化方向向量。
$v_{m+1},…,v_n$ 对应的特征值大于阈值，因此被视为非退化方向向量。
则：
其中：
由以下公式得到上式：
$V_px_p$ 将预测值(由传感器或者模型预测得到)映射到退化方向上；
$V_ux_u$ 将计算值映射到非退化方向上，其实就是舍弃退化部分；
因此，退化部分使用预测值和计算优化部分的非退化部分。

解释： $V_fx_f=V_px_p+V_ux_u$

设预估值为 $x_p$ ，更新值为 $x_u$ ，更新值 $x_u$ 在特征向量 $v_1$ 方向退化，将预估值 $x_p$ 映射到 $v_1$ 方向，将更新值 $x_u$ 映射到 $v_2$ 方向。最后合成为 $x_f$ 。

$V_px_p$ 为 $x_p$ 与 $V_p$ 中的各个特征向量的投影*对应向量的模

而在LOAM中，使用imu作为姿态预测，使用雷达点云的点到线、点到面来优化姿态角，并计算得到偏移。或者三前端计算得到位姿，后端批量优化。
令 $\Delta x_u$ 为后端优化相对于前端预测值，得到的位姿偏差。此时：(与论文中的伪代码不同，包括代码中的matP都应为matU), $V_p$ 应为 $V_u$ ，即提出 $\Delta x_u$ 的非退化部分。

代码结构

#loam_velodyne.launch
  <node pkg="loam_velodyne" type="scanRegistration" name="scanRegistration" output="screen"/>
  <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
  </node>
  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen"/>
  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen"/>
  <group if="$(arg rviz)">
    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />
  </group>

scanRegistration：点云提取，特征点分类
laserOdometry：点到线点到面匹配，得到初始定位信息，高频输出10hz
laserMapping：将点云分块处理，寻找最近邻的匹配点，且用于计算点云簇的法向量，类似point-to-plain方法，实现点到线及点到面的约束，最后得到去漂的优化定位值。低频输出。
transformMaintenance：将odom的高频粗匹配与map的低频高精度定位值进行集成，输出高频高精度的定位信息。

aloam 集成代码中的实现在laserMapping.cpp，当得到odom粗匹配则利用map的定位输出进行校正一下，然后发布校正后的定位信息。map校正是通过计算map与odom之间的坐标系关系匹配的。

loam_velodyne源码解析
A-LOAM代码解析

每个cpp文件对应LOAM框架重的一个节点。
推荐两篇讲述论文的博客及文档
LOAM笔记及A-LOAM源码阅读
loam_velodyne
参考：
https://zhuanlan.zhihu.com/p/57351961
LOAM笔记及A-LOAM源码阅读
3D 激光SLAM ->loam_velodyne论文与代码解析Lidar Odometry and Mapping
LOAM论文和程序代码的解读
https://blog.csdn.net/shoufei403/article/details/103664877

LOAM 原理及代码实现介绍[通俗易懂]