返回

LIO-SAM 代码分析(三):featureExtraction

作为对 featureExtraction.cpp 的补充说明,主要是进行逻辑上的梳理。

简介

LIO-SAM 的完整注释代码以及流程图见 LIO-SAM With Chinese Comments

整体思路

featureExtraction 类的大致工作流程见下图:

featureExtraction 流程图
featureExtraction 流程图

基本的思路和原始的 LOAM 基本一致,可以参考我之前写的 A-LOAM 中这部分的代码解析:A-LOAM 代码分析(二): Scan Registration

这里值得一提的是代码中实现了 LOAM 论文里面的两个雷达点筛选标准,这是 A-LOAM 没有实现的,可以参考一下:

  1. 在和激光束几乎平行的平面上的点,这些点通常不太可靠,如下图(a) 中的点 B
  2. 遮挡区域边缘的点,同样的理由因为该点实际上处于被遮挡的区域,所以激光雷达位姿稍微变化之后就观测不到了,如下图 (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;
    }
}
Built with Hugo
Theme Stack designed by Jimmy