LOAM 原理及代码实现介绍

文章目录

    • LOAM 原理及代码实现介绍
      • LOAM技术点
      • LOAM整体框架
      • 退化问题
      • 代码结构

LOAM 原理及代码实现介绍

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

使用传感器介绍:
LOAM 原理及代码实现介绍_第1张图片
如果没有电机旋转,则雷达自身的扫描是一个平面的180度。如下:
LOAM 原理及代码实现介绍_第2张图片
加上电机的旋转,则该设备能扫描的范围为雷达前方的半球形。

LOAM技术点

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

LOAM 原理及代码实现介绍_第3张图片

LOAM整体框架

LOAM 原理及代码实现介绍_第4张图片
将定位与建图分开运行,高频位姿估计+低频优化建图->实现实时,低计算量,低漂移。

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。试验数据表明,大部分点云都是平面点。边线点起到的约束作用较小。
    LOAM 原理及代码实现介绍_第5张图片
    判断公式:
    在这里插入图片描述
    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 iPk,表示点 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,jS,j=i,计算 Σ j X ( k , i ) L − X ( k , j ) L \Sigma _jX^L_{(k,i)}-X^L_{(k,j)} ΣjX(k,i)LX(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则存放是竖直上的线束及水平上的偏转度。也就是包含位置信息的同时,包含了仰角及偏转角信息。
  1. 位姿粗估计(雷达里程计(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}} iHk+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,但是点云约束类似。
    LOAM 原理及代码实现介绍_第6张图片

    使用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} iPk+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} iPk+1,对应找到 P k ~ \widetilde{P_k} Pk 相邻scan中最近的中三个点(j, l,m),则三个点则确定了平面,计算变换矩阵T能使,点i到平面的距离(j, l,m)最小。
    在这里插入图片描述
    点到面距离计算公式如下:原理:目标点到三个原始点组成的三个向量构成的斜方体/底面面积。
    分子:第二行两个向量的叉乘结果值等于j,l,m三个点构成的三角形面积,方向垂直于平面向上。
    结果点乘分子第一行,则可以得到i,j,l,m构成的斜三棱柱的体积。
    分母:等同于分子第二行。
    如下图所示:LOAM 原理及代码实现介绍_第7张图片
    即分别对应了点云匹配中的点到线及点到面的算法。

    向量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=RXk1+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=RinteXk1+Tinte
R i n t e 和 T i n t e R_{inte}和T_{inte} RinteTinte为将每个sweep中不同的scan的雷达坐标变换,用于将sweep下每个scan都映射到sweep的起始坐标系。 R i n t e 和 T i n t e R_{inte}和T_{inte} RinteTinte R 和 T R和T RT插值得到。

而后,利用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的输出。

LOAM 原理及代码实现介绍_第8张图片

  1. 雷达建图(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)

    1. 选取点最近邻5个点,进行协方差矩阵特征值、特征向量计算,若其中一个特征值远大于其他特征值,则说明该点是边线点,其中最大的特征值对应的特征向量就是该线的方向向量。这个判断方法同NICP算法的平面法向量计算。解释参考
    2. 利用得到的直线的方向向量在该点附近构造2个邻近点,同odom使用同样点到线的距离约束方程进行约束。

    精优化的点到面约束代码实现过程(A-LOAM):

    1. 选取最近邻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);
    2. 点( 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) =ax0+by0+cz0+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) =ax0+by0+cz0+1/norm

    LidarDistanceFactor未用

退化问题

定位问题分为两步: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的特征值,特征值很小时,则表明该特征值对应的方向存在退化现象。
退化现象的表现:
LOAM 原理及代码实现介绍_第9张图片(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对应的特征值大于阈值,因此被视为非退化方向向量。
LOAM 原理及代码实现介绍_第10张图片则:
在这里插入图片描述其中:
LOAM 原理及代码实现介绍_第11张图片由以下公式得到上式:
在这里插入图片描述 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
LOAM 原理及代码实现介绍_第12张图片
设预估值为 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 原理及代码实现介绍_第13张图片

代码结构

LOAM 原理及代码实现介绍_第14张图片

#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

你可能感兴趣的:(SLAM,自动驾驶,算法)