A-LOAM(前端-2)异常点的剔除-算法流程+代码注释

异常点剔除的原始出处见LOAM论文的 LIDAR ODOMETRY章节的B. Finding Feature Point Correspondence
最重要的理解见下图

算法流程

1. 先看 (b) ，取A点的距离（距离为雷达到A点），取B点的距离，计算两个点的列id之差
2. 根据 列id之差判断两点是否邻近，只有邻近才有意义去判断剔除
3. 先判断 depthA-depthB>0.3 满足的话则将A点的左边5个点设置为无效点置1
4. 然后判断 depthB-depthA>0.3 满足的话则将B点的右边5个点设置为无效点置1
5. 然后看 (a) ，计算点与其邻近点的距离，距离过大则很可能为平行点
6. 先看平行点的情况：取B点的距离与B点旁边一个点的距离，如果是平行点则这两点接近于平行激光线束，则两点各自的距离相差比较大
7. 如果为非平行点：看A点，则A点附近的点离雷达的距离与A点离雷达的距离是相近的
8. 代码中的判断是 depthB/depthA - depth(B+1)/depth(A+1) > 0.02

个人学习总结，有错误的地方欢迎指出一起讨论

标记异常点代码注释

    //标记一下遮挡的点
    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];   //第2个点的距离
            int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i])); //计算两个有效点的列id之差

            if (columnDiff < 10){ //只有邻近的才需要判断 ，才有意义
                // 10 pixel diff in range image
                if (depth1 - depth2 > 0.3){  //depth太靠近容易被遮挡，因此depth1的左5个点设为无效点
                    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]));

            if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
                cloudNeighborPicked[i] = 1;
        }
    }

A-LOAM系列讲解
A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释
A-LOAM(前端-2)异常点的剔除-算法流程+代码注释
A-LOAM(前端-3)的雷达畸变及运动补偿-算法流程+代码注释
A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释
A-LOAM总结-(前端+后端)算法流程分析
关于ROS中map、odom、base_link三个坐标系的理解