激光雷达检测负障碍物（附大概 C++ 代码)

检测效果如图，红色是正负的障碍物点：

障碍物根据其相对于地面的高度可以分为两类：正向障碍物和负向障碍物。在室外环境中，负障碍物是沟渠、悬崖、洞口或具有陡峭负坡度的地形，可能会造成安全隐患。  不慎通过道路坑洼处时，有颠簸，车辆有很大的损坏甚至掉入坑洼的风险。 因此，负障碍物检测对于自动驾驶感知系统至关重要。

检测使用传统方法，主要根据负障碍物存在的显著局部几何特征。

1 后缘高度下沉
负障碍物明显低于周围的地面，后缘有很多扫描点。 我们可以得出结论，这些扫描点的高度明显小于周围地面的平均高度。

2 在后缘上大的点云密度
单条扫描线得到的扫描点在水平面上呈线性分布。 但由于负障碍物远低于地面，激光扫描仪可以在后缘扫描更多的激光点，导致点云密度较大。

先把点云投影到前视图，一个M*N 的图像结构 fro_map_xydis 上去，用于对比两条激光线束之间的高度和距离变换情况

 fro_map_xydis.at<float>(ir, ic) = laser.distance;

理论上来说，前后2根线束的点之间应该是距离差距比较小的，如果差距比较大，可以把该点作为候选点纳入

if  (fro_map_xydis.at<float>(ir, ic) - fro_map_xydis.at<float>(ir-1, ic)) > dis_thres

同时，做一个俯视图的点云投影，如果上面的候选点每增加一个，就把俯视图每个小格子的点数量加一

                bev_map_jump_num.at<float>(static_cast<int>(cy), static_cast<int>(cx)) += 1;
 

如果每个小格子的异常高差点数量较多，就会被纳入下一步的分析，如果数量少，就会跳过

            if (bev_map_jump_num.at<float>(static_cast<int>(cy), static_cast<int>(cx)) < gConfig.smooth_window_size) {
                continue;
            }

最后，根据每个可疑障碍物区域的长宽和点数量，做一个最后的过滤，即可得到最后的正负障碍物。