LOAM 原理及代码实现介绍
文章目录
-
- LOAM 原理及代码实现介绍
-
- LOAM技术点
- 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 P_k Pk第k次整体sweep中得到的点云。 X ( k , i ) L X^L_{(k,i)} X(k,i)L表示第k个sweep中的i点在雷达坐标系下的坐标。设 i ∈ P k i\in P_k i∈Pk，表示点 i i i是第k次整体sweep中的点。选取 i i i点在同一个scan中相邻的前后的5个点 X ( k , j ) L , j ∈ S , j ≠ i X^L_{(k,j)}, j \in S, j\ne i X(k,j)L,j∈S,j=i，计算 Σ j X ( k , i ) L − X ( k , j ) L \Sigma _jX^L_{(k,i)}-X^L_{(k,j)} ΣjX(k,i)L−X(k,j)L。注意，论文中使用的是平面雷达，每个scan扫描区域是半平面，即，空间的平面，雷达一次scan会得到空间平面与雷达扫描平面交线上的点，会得到一条线的点，但是，如果雷达扫过角点，则就是类似平面v字型的点。）
即：
如果是平面，前后的5个点与i的距离相加=0，c->0
如果是边线角点，则 c > 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 P_k Pk第k次整体sweep中得到的点云；
根据插值将 P k P_k Pk映射到k时刻sweep起始坐标系下(点云去掉运动漂移)，记为 P k ~ \widetilde{P_k} Pk ，则根据插值将 P k + 1 P_{k+1} Pk+1映射到k+1时刻sweep起始坐标系下(点云去掉运动漂移)，记为 P k + 1 ~ \widetilde{P_{k+1}} Pk+1 ;
将第k帧扫描到的特征点 P k P_k Pk映射到第k+1帧的雷达坐标系下，记为 P k + 1 ~ \widetilde{P_{k+1}} Pk+1 .点云匹配：
该作者的点云匹配是基于将 P k + 1 ~ \widetilde{P_{k+1}} Pk+1 和 P k + 1 ~ \widetilde{P_{k+1}} Pk+1 进行匹配。
即将第k个sweep的点云映射到第k+1个sweep起始，将第k+1个sweep的点云映射到第k+1个sweep的起始。匹配 P k + 1 ~ \widetilde{P_{k+1}} Pk+1 和 P k + 1 ~ \widetilde{P_{k+1}} Pk+1 的点云点面关系，计算得到第k+1个sweep中产生的运动。点云特征点被分为，边线点和平面点。
边线约束点两个点确定：设去漂当前帧的点云 i ∈ ξ k + 1 ~ i\in\widetilde{\xi_{k+1}} i∈ξk+1 ，在上一帧点云(映射在k+1时刻坐标系下) ξ k ‾ \overline{\xi_k} ξk中找到距离最近的点 j j j，并在 ξ k ‾ \overline{\xi_k} ξk找到与j相邻scan的点 l l l。
设想两个墙面的竖直墙角线，设雷达偏移较小，相邻的的scan，会出现类似图a的情况，相邻scan会测量到墙角线上的点。因为可以在上一帧中找到两个相邻帧的墙角线上的点，约束当前帧打到墙角上的点到这两个点构成的直线上的距离最小，得到雷达运动T。如下图a所示。平面约束点三个点确定：设去漂当前帧的点云 i ∈ H k + 1 ~ i\in\widetilde{\mathcal{H}_{k+1}} i∈Hk+1 ，在上一帧点云(映射在k+1时刻坐标系下) H k ‾ \overline{\mathcal{H}_k} Hk中找到距离最近的点 j j j，并在 H k ‾ \overline{\mathcal{H}_k} Hk找到与j同scan的最近点 l l l和相邻scan的一个点 m m m。
论文中的雷达是二维平面雷达加旋转得到，每一次scan都是一次平面半圆，一个sweep由加在雷达上的电机转动180度，构成半球。一次scan打到对面一个平面上，就是一排直线排列的点。
橙色线表示点 j j j所在的scan，蓝色线表示与橙色线相邻的两次scan的线。使用velonedy雷达，每个FOV对应的多线构成一个竖直scan，但是点云约束类似。
使用kdtree存储点云信息。
将第k帧扫描到的特征点 P k P_k Pk映射到第k+1帧的雷达坐标系下，记为 P k ‾ \overline{P_k} Pk：，将 P k ‾ \overline{P_k} Pk与第k+1帧的扫描点云 P k + 1 P_{k+1} Pk+1进行点云匹配。在LOAM代码中，计算 P k ‾ \overline{P_k} Pk是使用TransformToEnd()将 P k {P_k} Pk映射到本k时刻sweep的结束，即k+1时刻sweep的开始来得到的。
而当前时刻k+1时刻的点云映射到k+1时刻的初始是使用TransformToStart()实现。
假设k+1时刻的sweep中与前一k时刻的sweep的运动一致 T k + 1 L T^L_{k+1} Tk+1L。使用位置插值方法得到整个sweep点云对应的 T k + 1 , i L T^L_{k+1,i} Tk+1,iL，从而利用TransformToStart()，TransformToEnd()可将点云映射到sweep初始和结束时的坐标系下。位姿插值
论文中，作者采用的是二维雷达加了一个电机旋转，每一次scan得到的点云的xyz是基于雷达的自身的坐标系，就是已经旋转后的雷达坐标系。
设雷达匀速旋转及匀速偏移，通过插值的方法，得到一个完整的sweep中，每一个scan对应的T(q，t)，并将点云坐标变换到每一次的sweep初始坐标系。消除点云在sweep中的因雷达运动而产生的误差，得到undistorted 的点云 P ~ k \widetilde P_k P k。
详细实现在laserOdometry.cpp TransformToStart函数中。
对于边线点 i ∈ P k + 1 i\in P_{k+1} i∈Pk+1，对应找到 P k ‾ \overline{P_k} Pk相邻scan中最近的中两个点(j, l)，则两个点则确定了边线，计算变换矩阵T能使，点i到边线(j, l)的距离最小。j为最近点，然后再j所在scan的相邻scan中，找到l点。
距离约束：
点到线的距离计算公式如下：原理是目标点到两个原始点组成的两个向量构成的平行四边形面积/底边长度。
对于平面点 i ∈ P k + 1 i\in P_{k+1} i∈Pk+1，对应找到 P k ~ \widetilde{P_k} Pk 相邻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对应公式的 X ~ ( k + 1 , i ) L {\widetilde{X}}_{(k+1,i)}^L X (k+1,i)L
注：向量外积在数值上等于这两个向量构成的平行四边形的面积。
其中:
X ‾ k = R X k − 1 + T \overline X_k=RX_{k-1}+T Xk=RXk−1+T X ~ k = R i n t e X k − 1 + T i n t e \widetilde X_k=R_{inte}X_{k-1}+T_{inte} X k=RinteXk−1+Tinte
R i n t e 和 T i n t e R_{inte}和T_{inte} Rinte和Tinte为将每个sweep中不同的scan的雷达坐标变换，用于将sweep下每个scan都映射到sweep的起始坐标系。 R i n t e 和 T i n t e R_{inte}和T_{inte} Rinte和Tinte由 R 和 T R和T R和T插值得到。
而后，利用LM方法计算得到 T k + 1 L T^L_{k+1} Tk+1L，得到该时刻的定位值(雷达里程计)
融合高频粗略的运动估计和优化后的位姿估计，得到准确位姿。根据位姿及得到的点云进行建图，创建点云地图。
-
代码文件：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 o d o m 2 m a p T_{odom2map} Todom2map精优化的点到线约束代码实现过程(A-LOAM)：
- 选取点最近邻5个点，进行协方差矩阵特征值、特征向量计算，若其中一个特征值远大于其他特征值，则说明该点是边线点，其中最大的特征值对应的特征向量就是该线的方向向量。这个判断方法同NICP算法的平面法向量计算。解释参考
- 利用得到的直线的方向向量在该点附近构造2个邻近点，同odom使用同样点到线的距离约束方程进行约束。
精优化的点到面约束代码实现过程(A-LOAM):
- 选取最近邻5个点 ，设平面方程为ax + by + cz + 1 = 0，求解平面法向量X=(a,b,c)，将最近邻5个点带入平面方程，得到 A X = B AX=B AX=B的方程，其中A为有5个点云构成的5*3的矩阵， B = [ − 1 , − 1 , − 1 , − 1 , − 1 ] T B=[-1,-1,-1,-1,-1]^T B=[−1,−1,−1,−1,−1]T，使用QR分解得到平面法向量X=(a,b,c)。并对向量进行归一化得到(a’, b’, c’)。norm = matA0.colPivHouseholderQr().solve(matB0);
- 点( x 0 , y 0 , z 0 x_0,y_0,z_0 x0,y0,z0)到平面的计算公式： a b s ( a x 0 + b y 0 + c z 0 + 1 ) / ( a 2 + b 2 + c 2 ) = a ′ x 0 + b ′ y 0 + c ′ z 0 + 1 / n o r m 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 abs(ax0+by0+cz0+1)/(a2+b2+c2)=a′x0+b′y0+c′z0+1/norm
这个点(角点/平面点)特征的区分不同于雷达里程计的输入的点云提取，是因为，在雷达里程计的计算中，舍去了参考意义小或重复的点云。
下图为雷达里程计的输出和laser mapping下优化位姿变换的输出的关系：
雷达里程计的输出是在雷达坐标系下，高频，有漂移；
地图最后输出位姿变化是全局的，高精度，低频。
- 代码文件：laserMapping.cpp
接收odom发来的odom定位估计以及去误差的点云 P k P_k Pk映射到世界坐标系下，记为 Q k Q_k Qk，并将下采用后的点云存储在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 ′ S' 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精优化的点到面的约束及仿函数，就是点到面的计算公式： a b s ( a x 0 + b y 0 + c z 0 + 1 ) / ( a 2 + b 2 + c 2 ) = a ′ x 0 + b ′ y 0 + c ′ z 0 + 1 / n o r m 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 abs(ax0+by0+cz0+1)/(a2+b2+c2)=a′x0+b′y0+c′z0+1/normLidarDistanceFactor未用
退化问题
定位问题分为两步：1、位姿预测；2、位姿优化
1、位姿预测( x p x_p xp)：使用imu预测姿态，或前端点云计算得到位姿；
2、位姿优化( x u x_u xu)：使用非线性最小二乘优化方法得到的位姿。
退化问题解决思路：
退化问题主要出现在位姿优化，当出现退化现象时，舍弃退化方向的值，使用预测值(由其他传感器估计或者模型估计计算得到)来填充退化方向上的位姿解。
求解问题如下：
可使用如下方法求解：
将 A x = b Ax=b Ax=b两边左乘 A T A^T AT，得到 A T A x = A T b A^TAx=A^Tb ATAx=ATb，保证 A T A A^TA ATA满秩，即可逆，则可根据 x = ( A T A ) − 1 A T b x=(A^TA)^{-1}A^Tb x=(ATA)−1ATb解出 x x x。
作者定义退化因子 D = λ m i n + 1 \mathcal{D}=\lambda_{min}+1 D=λmin+1，
其中 λ m i n \lambda_{min} λmin为 A T A A^TA ATA的特征值，特征值很小时，则表明该特征值对应的方向存在退化现象。
退化现象的表现：
(a)表示求解约束较好的情况，(b)退化情况，在解在蓝色方向上存在退化，表明，蓝色箭头方向解的不确定性大。
令 v 1 , . . . , v m v_1,...,v_m v1,...,vm 对应的特征值小于阈值，因此被视为退化方向向量。
v m + 1 , . . . , v n v_{m+1},...,v_n vm+1,...,vn对应的特征值大于阈值，因此被视为非退化方向向量。
则：
其中：
由以下公式得到上式：
V p x p V_px_p Vpxp将预测值(由传感器或者模型预测得到)映射到退化方向上；
V u x u V_ux_u Vuxu将计算值映射到非退化方向上，其实就是舍弃退化部分；
因此，退化部分使用预测值和计算优化部分的非退化部分。
解释： V f x f = V p x p + V u x u V_fx_f=V_px_p+V_ux_u Vfxf=Vpxp+Vuxu
设预估值为 x p x_p xp，更新值为 x u x_u xu，更新值 x u x_u xu在特征向量 v 1 v_1 v1方向退化，将预估值 x p x_p xp映射到 v 1 v_1 v1方向，将更新值 x u x_u xu映射到 v 2 v_2 v2方向。最后合成为 x f x_f xf。
V p x p V_px_p Vpxp为 x p x_p xp与 V p V_p Vp中的各个特征向量的投影*对应向量的模
而在LOAM中，使用imu作为姿态预测，使用雷达点云的点到线、点到面来优化姿态角，并计算得到偏移。或者三前端计算得到位姿，后端批量优化。
令 Δ x u \Delta x_u Δxu为后端优化相对于前端预测值，得到的位姿偏差。此时：(与论文中的伪代码不同，包括代码中的matP都应为matU), V p V_p Vp应为 V u V_u Vu，即提出 Δ x u \Delta x_u Δxu的非退化部分。
代码结构
#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