LeGOLOAM学习

LOAM

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

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 cloud
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
findStartEndAngle();
// 3. Range image projection
projectPointCloud();
// 4. Mark ground points
groundRemoval();
// 5. Point cloud segmentation
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();
}


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 for (size_t i = 0; 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 points
groundMat.at(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(i,j) = 1;
groundMat.at(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(fromIndX, fromIndY),
rangeMat.at(thisIndX, thisIndY));
d2 = std::min(rangeMat.at(fromIndX, fromIndY),
rangeMat.at(thisIndX, thisIndY));
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = 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 system
transformAssociateToMap();

// extract surrounding key frames based on the map format
extractSurroundingKeyFrames();

// down sample
downsampleCurrentScan();

// optimization
scan2MapOptimization();

saveKeyFramesAndFactor();
correctPoses();

// publishers
publishTF();
publishKeyPosesAndFrames();
clearCloud();
}


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