前言

LeGO-LOAM的全称是 Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

其中LeGO就是轻量级和利用地面优化，轻量级的实现就是通过两步的优化方式，利用地面优化的部分也在两步优化的第一步中。

和原始LOAM一样，通过前后两帧点云来估计两帧之间的运动，从而累加得到前端里程计的输出，和上述方法使用线面约束同时优化六自由度帧间位姿不同，LeGO-LOAM的前端分成两个步骤，每个步骤估计三自由度的变量。

通过这种方式进行帧间里程计的运算，可以提供运算效率，使得可以在嵌入式平台中运行

在里程计部分，LeGO-LOAM充分利用了前面存储的标签信息，在原本LOAM的里程计部分，其使用的思路更像是ICP，单纯寻找距离最近的点，而并没有考虑其合理性。在这里，LeGO-LOAM通过标签增加了一次筛选，标签相同的点才会用于优化，通过这种方法，能够大幅提高匹配的准确率。

此外，在优化部分，LOAM使用的是直接将所有参数放在一起，进行LM优化，而在LeGO-LOAM中，作者将LM优化分两步进行，每次优化三个参数，在确保准确性的前提下，减小了35%的计算开销。

利用地面点优化

地面点更符合面特征的性质，因此地面点的优化问题就使用点到面的约束来构建

地面点的约束对x，y，yaw这三个自由度是不能估测的。当这个三个自由度的值发生变化，点到面的残差不会发生显著变化，
地面点的优化，只对pitch、roll及z进行约束和优化

白色的为当前帧，绿色的为上一帧，可以看到当前帧发生yaw的旋转，和发生x、y的平移的时候，当前帧的面点到上一帧的距离都没有变，所有不能把这三个自由度的优化放到面点特征上来。

当z轴发生移动，或者发生pitch和roll角的变化，当前帧的面特征点，到上一帧的面特征点的距离即发生变化。

利用角点优化

第一步优化完pitch、roll和z之后，剩下还有三个自由度的变量需要进行估计，
需要提取角点进行优化，由于多线激光雷达提取的角点通常是垂直的边缘特征，因此对，x、y及yaw有着比较好的能观性，通过角点的优化结合上地面点的结果可以得到六自由度的帧间优化结果。

将上一步分优化的z、pitch和roll的三自由度量，和剩下没有估计的三个自由度进行角点的约束优化。

代码部分

LeGO-LOAM的帧间里程计部分在FeatureAssociation.cpp中。
可以直接找到runFeatureAssociation的函数

        adjustDistortion();//去除畸变calculateSmoothness();//计算曲率markOccludedPoints();//剔除离散点extractFeatures();//提取特征publishCloud(); // cloud for visualization/**2. Feature Association*/if (!systemInitedLM) {checkSystemInitialization();return;}updateInitialGuess();updateTransformation();//帧间里程计核心部分integrateTransformation();publishOdometry();publishCloudsLast(); // cloud to mapOptimization

在这个函数中进行了

去除畸变
计算曲率
剔除离散点
提取特征
帧间里程计核心部分
发布里程计
发布点云

其中帧间里程计核心部分在 updateTransformation() 函数中

    void updateTransformation(){if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return;//地面点帧间里程计估计优化for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear();coeffSel->clear();findCorrespondingSurfFeatures(iterCount1);//通过kdtree 寻找对应的点if (laserCloudOri->points.size() < 10)continue;if (calculateTransformationSurf(iterCount1) == false)//进行优化估计break;}//角点帧间里程计估计优化for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {laserCloudOri->clear();coeffSel->clear();findCorrespondingCornerFeatures(iterCount2);//通过kdtree 寻找对应的点if (laserCloudOri->points.size() < 10)continue;if (calculateTransformationCorner(iterCount2) == false)//进行优化估计break;}}

从这个函数中可以明显看出分成了两部分

第一个for循环是地面点帧间里程计估计优化
第二个for循环是角点帧间里程计估计优化

从这里可以看出surfPointsFlat中的点都是地面点

然后在findCorrespondingSurfFeatures的函数中，进行当前帧的地面点与上一帧中的地面点进行匹配。
其过程和ALOAM中的原理一致。

在findCorrespondingCornerFeatures的函数中，进行当前帧的角点与上一帧中的角点进行匹配。
其过程和ALOAM中的原理一致。

当前帧角点i，映射到上一帧点云中，并找到距离最近的点j，在与j相同的scan中，寻找次近邻点m。
利用映射后的点i到j和m的距离，进行约束

通过两步优化的位姿变换通过里程计数据的方式发送出来可以进行可视化的操作
具体代码在 publishOdometry() 函数中

当前帧角点i，映射到上一帧点云中，并找到距离最近的点j，并在相同scan中找到次近邻点l，在相邻的scan中再找到近邻点m。
利用映射后的点i到j、l、m平面的距离，进行约束。

//发布前端里程计void publishOdometry(){geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);laserOdometry.header.stamp = cloudHeader.stamp;laserOdometry.pose.pose.orientation.x = -geoQuat.y;laserOdometry.pose.pose.orientation.y = -geoQuat.z;laserOdometry.pose.pose.orientation.z = geoQuat.x;laserOdometry.pose.pose.orientation.w = geoQuat.w;laserOdometry.pose.pose.position.x = transformSum[3];laserOdometry.pose.pose.position.y = transformSum[4];laserOdometry.pose.pose.position.z = transformSum[5];pubLaserOdometry.publish(laserOdometry);laserOdometryTrans.stamp_ = cloudHeader.stamp;laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));tfBroadcaster.sendTransform(laserOdometryTrans);}

其topic的名称为：

gazebo测试

前端的里程计在Z轴方向已经发生了漂移。
但是地图没有飘，这个是后端进行优化的结果，将前端漂移的部分优化了回来

3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析相关推荐

3d激光SLAM：LIO-SAM框架—IMU预积分功能数据初始化
前言 LIO-SAM的全称是:Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping 从全称上可以看出,该算法是一个紧耦合的 ...
SLAM论文笔记-使用点和线特征的激光雷达-单目视觉里程计
使用点和线特征的激光雷达-单目视觉里程计首发在泡泡机器人slam公众号标题:Lidar-Monocular Visual Odometry using Point and Line Feature ...
SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中. 由于参与分享汇报的同事有许多是做其他方向的机器人工程师(包括硬件.控制等各方面并 ...
3D激光SLAM:LOAM 论文--算法详细解读
3D激光SLAM:LOAM 论文--算法详细解读 LOAM简介论文里面的符号表示算法部分激光雷达里程计 A 特征点提取 B 找特征点的匹配对 C 运动估计 lidar 建图测试结果 LOAM是 ...
彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总！
地图定位算法是自动驾驶模块的核心,而激光SLAM则是地图定位算法的关键技术,其重要性不言而喻,在许多AI产品中应用非常多(包括但不限于自动驾驶.移动机器人.扫地机等).相比于传统的视觉传感器,激光传感 ...
3D激光SLAM：LeGO-LOAM论文解读---激光雷达里程计与建图
3D激光SLAM:LeGO-LOAM论文解读---激光雷达里程计与建图激光雷达里程计针对LOAM的改进激光雷达建图原文激光雷达里程计激光雷达里程计模块的功能就是:估计相邻帧之间的位姿变换. ...
3d激光SLAM：LIO-SAM框架---位姿融合输出
3d激光SLAM:LIO-SAM框架---位姿融合输出前言 Eigen::Affine3f 位姿融合输出 result 前言 LIO-SAM的全称是:Tightly-coupled Lidar In ...
3D激光SLAM:ALOAM:异常点剔除机制代码解析
3D激光SLAM:ALOAM:异常点剔除机制代码解析前言 LOAM中的异常点 Code 前言在LOAM的论文中,作者提到了要剔除两种异常点. 平行点遮挡点但是在ALOAM的代码中并未写相关的功 ...
32线镭神雷达跑LeGO-LOAM：3D 激光SLAM
32线镭神雷达跑LeGO-LOAM:3D 激光SLAM 安装LeGO-LOAM 镭神雷达的相关修改 LeGO-LOAM的修改修改utility.h 修改imageproject.cpp Enjoy ...

3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析