简介
LIO-SAM 的完整注释代码以及流程图见 LIO-SAM With Chinese Comments
整体思路
featureExtraction
类的大致工作流程见下图:
基本的思路和原始的 LOAM 基本一致,可以参考我之前写的 A-LOAM 中这部分的代码解析:A-LOAM 代码分析(二): Scan Registration
这里值得一提的是代码中实现了 LOAM 论文里面的两个雷达点筛选标准,这是 A-LOAM 没有实现的,可以参考一下:
- 在和激光束几乎平行的平面上的点,这些点通常不太可靠,如下图(a) 中的点 B
- 遮挡区域边缘的点,同样的理由因为该点实际上处于被遮挡的区域,所以激光雷达位姿稍微变化之后就观测不到了,如下图 (b) 中的点 A。
这两个筛选标准的实现思路可以参考流程图以及 markOccludedPoints()
这个函数:
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
// 假如相邻点足够近 -- 筛选标准 2
if (columnDiff < 10){
// 10 pixel diff in range image
// 如果 i + 1 点在 i 点前面,将 i-5 到 i 点标记,否则标记 i+1 到 i+6 点
if (depth1 - depth2 > 0.3){
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
// parallel beam
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
// 假设中心点是 O,两相邻点是 A(在前), B,由于水平分辨率是固定的,两个相邻点距离差越大表示 AB 和 OA 的夹角越接近 180°,即和激光束平行性越好 --- 筛选标准 1
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
}
}