LOAM

LOAM是一套非常有价值的LIDAR ODOMOTRY算法（它是一个历程计算法，没有回环检测和全局优化的部分）。

LEGO LOAM

LeGO LOAM

它含有四个主要线程

image projection：相对于LOAM增加的模块，对每一帧的激光数据的预处理。包括了地面的提取（没有统一平面作为地面的假设），点云的实时分割。
feature optimization ：特征点的提取（和LOAM一样的方式），但是匹配的时候增加了预处理中分割标签的匹配。
map optimization ：地图的优化，这里LEGO提出了另一种地图的存储方式，使用图优化模型来表示。
transform fusion ：这个和LOAM一致，将各个坐标系统一的线程。

1. Image Projection

主要步骤在cloudHandler这个callback函数里面。里面计算点云出于哪一条扫描线的第几个点的计算方法和LOAM中的原理一致（findStartEndAngle()函数）。 groundRemoval 和 cloudSegmentation 是最重要的两个函数，分别提取了地面点和实现了快速的点云分割。

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){// 1. Convert ros message to pcl point cloudcopyPointCloud(laserCloudMsg);// 2. Start and end angle of a scanfindStartEndAngle();// 3. Range image projectionprojectPointCloud();// 4. Mark ground pointsgroundRemoval();// 5. Point cloud segmentationcloudSegmentation();// 6. Publish all cloudspublishCloud();// 7. Reset parameters for next iterationresetParameters();}

1.1 projectPointCloud

这里将激光点云投影到了一个二维矩阵上（实际还存储在了一个向量数组中）。横坐标代表激光点云属于的扫描线（总共是16或者32或者64）.纵坐标则表示它是这条线上的第几个数据。每一个矩阵元素的值是它距离激光设备的距离，在函数中变量名为range。

range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);

另外可以发现在后面的点云循环大多是对这样的二维矩阵循环。比如：

for (size_t j = 0; j < Horizon_SCAN; ++j){for (size_t i = 0; i < groundScanInd; ++i){// whatever}
}

1.2 groundRemoval

这一块做的工作可以很简单的从名字中理解，是提取地面点（不一定是标准的平面）。
这里考虑的是平面点应该是较为平滑的，所以相邻点的俯仰角度不会太大。另外同一条扫描线上的点可能属于同一个平面（比如墙），对他们筛选没有意义。所以考虑对相邻两条扫描线上的临近点筛选。

对每一个点，取当前的点和下一个扫描线的同样位置的点（在二维矩阵图像看来就是处在下一个row同样colum的点）。

lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;

下一步判断当前的点是不是有效点。

if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){// no info to check, invalid pointsgroundMat.at<int8_t>(i,j) = -1;continue;
}

然后计算选取的两个点之间的角度（相对于激光扫描器XY平面的角度）。

 diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

如果角度小于某一个阈值，则判断它是一个平面。

if (abs(angle - sensorMountAngle) <= 10){groundMat.at<int8_t>(i,j) = 1;groundMat.at<int8_t>(i+1,j) = 1;
}

这样得到的地面更类似于一个平面，但是由于它地面的提取筛选的角度是相对于扫描设备的xy平面，这其实是假设扫描设备一直是相对水平的。如果扫描设备没有保持相对水平，这一步就会失效，影响到每一个点的标签，会影响之后的特征点匹配，进而影响到之后的模块（这个我在实验中测试过，如果太大得偏转扫描器，整个系统了就失效了）。

1.3 cloudSegmentation

这里是对提取完成地面的点云进行分割。使用的方法来源于文章 “Fast image-based segmentation of sparse 3D laser scans for online ooperation”。其主要的原理是，假设同一个团簇的点云之间的连线和于扫描设备的连线的夹角应该小于一个阈值。详细可以见下图。

图中的A，B是两个团簇类，角度beta代表上面描述的参考值。同一类点的beta会比较大，不同类则会较小。另外需要注意的是：计算beta的两个点是相邻点（二维矩阵的colum或者row上的相邻点）。

在代码中上面的步骤是通过 labelComponents(i, j) 实现的。

d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY));
d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY));
if ((*iter).first == 0)alpha = segmentAlphaX;
elsealpha = segmentAlphaY;angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));if (angle > segmentTheta){// whatever
}

代码中的segmentTheta设置为10°，在论文中有讨论关于参数选取的部分，通过多组对比实验说明了10°是最优的取值。

2. Feature Association

下面是特征提取的线程。最主要的部分是 calculateSmoothness，提取每一个点的平滑程度值，再根据这个值使用 extractFeatures 提取特征点。

        /**1. Feature Extraction*/adjustDistortion();calculateSmoothness();markOccludedPoints();extractFeatures();publishCloud(); // cloud for visualization/**2. Feature Association*/if (!systemInitedLM) {checkSystemInitialization();return;}updateInitialGuess();updateTransformation();integrateTransformation();publishOdometry();publishCloudsLast(); // cloud to mapOptimization

2.1 calculateSmoothness

在这里使用的方法基本和LOAM一致。提取每个点的平滑度都是通过一个1*11的卷积，kernel是[1,1,1,1,1,-10,1,1,1,1,1]。就是取左右各五个点然后计算这11个点的平滑趋势。

float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]+ segInfo.segmentedCloudRange[i+5];

除此以外还有很多工程方面的特征点匹配的代码，这里更合适的是阅读LOAM的代码之后，再联系寻找LEGO LOAM做了哪些修改。

2.2 extractFeatures

特征点提取的算法和LOAM基本一致，可以参加其他很棒的博主的分析LOAM的文章。

3. Map Optimization

最后是地图优化的线程。最重要的部分是 scan2MapOptimization 。

if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {timeLastProcessing = timeLaserOdometry;// transform to the same coordinate systemtransformAssociateToMap();// extract surrounding key frames based on the map formatextractSurroundingKeyFrames();// down sampledownsampleCurrentScan();// optimization scan2MapOptimization();saveKeyFramesAndFactor();correctPoses();// publisherspublishTF();publishKeyPosesAndFrames();clearCloud();
}

see compare for the comparsion of hdl_graph_slam, A_LOAM and LeGO LOAM

LeGO LOAM学习相关推荐

LEGO LOAM 学习理解总结
参考资料 LOAM等内容的博客需要好好研读 LeGO-LOAM 源码阅读笔记(imageProjecion.cpp) LeGO-LOAM 源码阅读笔记(featureAssociation.cpp) ...
lego loam 安装过程与问题处理
lego loam安装与问题处理: https://blog.csdn.net/weixin_44156680/article/details/118070387 ubuntu 20 安装lego l ...
LOAM学习-代码解析（三）特征点运动估计 laserOdometry
LOAM学习-代码解析(三)特征点运动估计 laserOdometry 前言一.初始化二.去除位移畸变 TransformToStart TransformToEnd 三.去除角度畸变 Plugi ...
LOAM学习-LM方法
LOAM学习-LM方法前言一.基本原理二.计算步骤结语前言在LOAM学习-代码解析(四)特征点运动估计 laserOdometry的3.3中,使用LM方法进行点云配准. 本部分对LM方法进 ...
LOAM学习-安装与运行
LOAM-安装与运行引言 LOAM安装 LOAM运行结语引言由于最近想学习SLAM,发现LOAM是较为经典的入门算法. 于是就开始了LOAM的学习. LOAM安装 LOAM的安装真的是一言难尽 ...
运行lego—loam遇到的问题以及解决方案
问题 1 : The imported target "vtkRenderingPythonTkWidgets" references the file "/usr ...
lego-loam学习笔记(一)
前言: 主要记录配置编译lego-loam源码时遇到的问题和解决的方法. 系统:ubuntu18.04 一.安装gtsam 因为系统是18.04所以不需要作很大的更改,按照官网的doc一步一步的来就行 ...
LOAM, ALOAM, LegoLOAM, hdl graph slam比较
LOAM LOAM: LOAM使用了作者定义的特征点提取和匹配方法,主要去边角点和平面点.LOAM use a new defined feature system (corner and flat ...
LeGO-LOAM学习
前言在学习了LOAM之后,了解到LeGO-LOAM(面向复杂情况的轻量级优化地面的雷达里程计),进行了一个学习整理. Github:https://github.com/RobustFieldAut ...

LeGO LOAM学习