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

大家好，又见面了，我是你们的朋友全栈君。如果您正在找激活码,请点击查看最新教程,关注关注公众号 “全栈程序员社区” 获取激活教程,可能之前旧版本教程已经失效.最新Idea2022.1教程亲测有效,一键激活。

Jetbrains全系列IDE使用 1年只要46元 售后保障 童叟无欺

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)

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

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

   特征点提取及处理
   LOAM中选用特征点来进行运动估计，特征点选取(edge point角点)和平面点(planar point)，如下黄点为edge point，红点为planar point。试验数据表明，大部分点云都是平面点。边线点起到的约束作用较小。
   
   判断公式：
   
   $P_k$第k次整体sweep中得到的点云。$X_{(k,i)}^L$表示第k个sweep中的i点在雷达坐标系下的坐标。

   设$i\in P_k$，表示点$i$是第k次整体sweep中的点。选取$i$点在同一个scan中相邻的前后的5个点$X_{(k,j)}^L,j\in S,j\ne i$，计算${\Sigma }_j{X}_{(k,i)}^L-X_{(k,j)}^L$。注意，论文中使用的是平面雷达，每个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则存放是竖直上的线束及水平上的偏转度。也就是包含位置信息的同时，包含了仰角及偏转角信息。

2. 位姿粗估计(雷达里程计(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_{k+1}^L$。使用位置插值方法得到整个sweep点云对应的$T_{k+1,i}^L$，从而利用TransformToStart()，TransformToEnd()可将点云映射到sweep初始和结束时的坐标系下。

   位姿插值
   论文中，作者采用的是二维雷达加了一个电机旋转，每一次scan得到的点云的xyz是基于雷达的自身的坐标系，就是已经旋转后的雷达坐标系。
   设雷达匀速旋转及匀速偏移，通过插值的方法，得到一个完整的sweep中，每一个scan对应的T(q，t)，并将点云坐标变换到每一次的sweep初始坐标系。消除点云在sweep中的因雷达运动而产生的误差，得到undistorted 的点云${\tilde{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对应公式的${\tilde{X}}_{(k+1,i)}^L$

注：向量外积在数值上等于这两个向量构成的平行四边形的面积。
其中:
$${\overline{X}}_k=R{X}_{k-1}+T$$$${\tilde{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_{k+1}^L$，得到该时刻的定位值(雷达里程计)

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

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

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

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

3. 雷达建图(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个点带入平面方程，得到$AX=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(a{x}_0+b{y}_0+c{z}_0+1)/\sqrt{(a^2+b^2+c^2)}=a^{\prime }{x}_0+b^{\prime }{y}_0+c^{\prime }{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^{\prime }$的协方差矩阵，分析边线及平面的方向；
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(a{x}_0+b{y}_0+c{z}_0+1)/\sqrt{(a^2+b^2+c^2)}=a^{\prime }{x}_0+b^{\prime }{y}_0+c^{\prime }{z}_0+1/norm$

  LidarDistanceFactor未用

退化问题

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

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

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

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

解释：$V_f{x}_f=V_p{x}_p+V_u{x}_u$

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

$V_p{x}_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