激光雷达射线法分割地面

学习记录用，在原博客的基础上增加了一些代码注释和个人理解。

---

前言

过滤地面是激光雷达感知中一步基础的预处理操作，因为我们环境感知通常只对路面上的障碍物感兴趣，且地面的点对于障碍物聚类容易产生影响，所以在做激光雷达障碍物检测之前通常将地面点和非地面点进行分离。在此文中介绍一种Ray Ground Filter的路面过滤方法，并且在ROS中实践。
算法处理主要包含两个部分：点云的裁切和过滤，角度微分化和地面判断。

---

一、预处理

1.裁切过高点云

在分割地面之前，可以滤除掉过高的障碍物，以减少点云的数量，提升整体的处理速率。
具体的裁切高度由launch文件中的clip_height指定。
代码如下：

void clip_above(double clip_height, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,
                             const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
    //cliper用来做提取索引容器
    pcl::ExtractIndices<pcl::PointXYZI> cliper;

    cliper.setInputCloud(in);   //设置容器的输入点云
    pcl::PointIndices indices;      //设置索引
#pragma omp for
    for (size_t i = 0; i < in->points.size(); i++)          //循环遍历所有点
    {
        if (in->points[i].z > clip_height)          //判断点云的高度是否大于裁切高度
        {
            indices.indices.push_back(i);           //将大于裁切高度的点入栈
        }
    }
    cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));          //在提取索引容器cliper中选择indices（索引集合）
    cliper.setNegative(true); //cliper保留之外的点
    cliper.filter(*out);    //cliper过滤后的点保存到 out
}

2.消除雷达近身反射点的影响

具体指标由launch文件中的min_distance参数指定。
也可以在此处去除过远处区域雷达稀疏点云，避免稀疏点云对整体精度的影响。
代码如下（示例）：

void remove_close_pt(double min_distance, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,
                                  const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
    pcl::ExtractIndices<pcl::PointXYZI> cliper;//点云提取对象：pcl库

    cliper.setInputCloud(in);
    pcl::PointIndices indices;
#pragma omp for
    for (size_t i = 0; i < in->points.size(); i++)
    {
        double distance = sqrt(in->points[i].x * in->points[i].x + in->points[i].y * in->points[i].y);

        if (distance < min_distance)//小于此距离的记录点云索引
        {
            indices.indices.push_back(i);
        }
    }
    cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
    cliper.setNegative(true); //设置为true代表提取该索引之外的的点云
    cliper.filter(*out);
}

---

二、角度微分和地面判断(核心部分)

Ray Ground Filter算法的核心是以射线（Ray）的形式来组织点云。我们现在将点云的 (x, y, z)三维空间降到（x,y）平面来看，计算每一个点到车辆x正方向的平面夹角 θ, 我们对360度进行微分，分成若干等份，每一份的角度为0.18度，这个微分的等份近似的可以看作一条射线，如下图所示,图中是一个激光雷达的纵截面的示意图，雷达由下至上分布多个激光器，发出如图所示的放射状激光束，这些激光束在平地上即表现为，图中的水平线即为一条射线：

为了方便地对点进行半径和夹角的表示，作者自定义了数据结构PointXYZIRTColor来代替pcl::PointCloudXYZI：

struct PointXYZIRTColor
{
    pcl::PointXYZI point;
 
    float radius; //xy平面与雷达的欧氏距离
    float theta;  //xy的角度微分
 
    size_t radial_div;     //线圈的索引
    size_t concentric_div; //扫描线的索引
 
    size_t original_index; //与源雷达点云对应的索引
};

其中，radius表示点到lidar的水平距离(半径)，即：

theta是点相对于车头正方向(即x方向)的夹角，计算公式为：
程序中主要的角度线圈划分部分为：

PointXYZIRTColor new_point;
auto radius = (float)sqrt(
    in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y);
auto theta = (float)atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI;
if (theta < 0)
{
    theta += 360;
}
//角度的微分
auto radial_div = (size_t)floor(theta / RADIAL_DIVIDER_ANGLE);      //根据偏转角度theta和单位分割角度，估算出当前点所在的射线是哪一条
//半径的微分
auto concentric_div = (size_t)floor(fabs(radius / concentric_divider_distance_));       //估算出当前点所在的同心圆是哪一个

new_point.point = in_cloud->points[i];
new_point.radius = radius;
new_point.theta = theta;
new_point.radial_div = radial_div;
new_point.concentric_div = concentric_div;
new_point.original_index = i;       //保存原始点云的索引

用radial_div和concentric_div分别描述角度微分和距离微分。对点云进行水平角度微分之后，可得到：360/0.18 = 2000条射线，将这些射线中的点按照距离的远近进行排序。

(最核心的判断逻辑)通过判断射线中前后两点的坡度是否大于我们事先设定的坡度阈值，从而判断点是否为地面点。代码如下：

void classify_pc(std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds,
                              pcl::PointIndices &out_ground_indices,
                              pcl::PointIndices &out_no_ground_indices)
{
    out_ground_indices.indices.clear();
    out_no_ground_indices.indices.clear();
#pragma omp for
    for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) //sweep through each radial division 遍历每一根射线
    {   //实际上in_radial_ordered_clouds的组织形式是以每根射线为最小单位的，可以想象为一个数组下，每个单元是一根线，每根线有数个点
        float prev_radius = 0.f;    //预设半径
        float prev_height = -SENSOR_HEIGHT;     //预设高度为传感器的负值
        bool prev_ground = false;       //这两个prev_ground     current_ground是标志位，用以判断前一个点 与当前点是否为地面点
        bool current_ground = false;
        for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) //loop through each point in the radial div     即遍历每根线下的所有点
        {
            float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius;        //此刻点的半径与前一点相减
            float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;      //根据半径差反求出高度阈值（判断带坡度的地面？）
            float current_height = in_radial_ordered_clouds[i][j].point.z;      //该点此刻的z值
            float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;

            //for points which are very close causing the height threshold to be tiny, set a minimum value
            if (points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_)
            {
                height_threshold = min_height_threshold_;
            }

            //check current point height against the LOCAL threshold (previous point)
            if (current_height <= (prev_height + height_threshold) && current_height >= (prev_height - height_threshold))
            {
                //满足条件后检查前一点是否是地面点
                //Check again using general geometry (radius from origin) if previous points wasn't ground
                if (!prev_ground)
                {
                    //如果前一点不是地面点
                    if (current_height <= (-SENSOR_HEIGHT + general_height_threshold) && current_height >= (-SENSOR_HEIGHT - general_height_threshold))
                    {
                        current_ground = true;
                    }
                    else
                    {
                        current_ground = false;
                    }
                }
                else
                {
                    current_ground = true;
                }
            }
            else
            {
                //check if previous point is too far from previous one, if so classify again
                if (points_distance > reclass_distance_threshold_ &&
                    (current_height <= (-SENSOR_HEIGHT + height_threshold) && current_height >= (-SENSOR_HEIGHT - height_threshold)))
                {
                    current_ground = true;
                }
                else
                {
                    current_ground = false;
                }
            }

            if (current_ground)
            {   //如果当前点是地面点，将点的原始点云索引插入地面点索引
                out_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index);
                prev_ground = true;
            }
            else
            {
                out_no_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index);
                prev_ground = false;
            }
            //插入完成后，再将当前点云设置为前一点云，以便进行下一次判断
            prev_radius = in_radial_ordered_clouds[i][j].radius;
            prev_height = in_radial_ordered_clouds[i][j].point.z;
        }
    }
}

这里有两个重要参数，一个是local_max_slope_，是我们设定的同条射线上邻近两点的坡度阈值，一个是general_max_slope_，表示整个地面的坡度阈值，这两个坡度阈值的单位为度（degree），我们通过这两个坡度阈值以及当前点的半径（到lidar的水平距离）求得高度阈值，通过判断当前点的高度（即点的z值）是否在地面加减高度阈值范围内来判断当前点是为地面。

三、launch文件说明及效果

最后附上launch文件的参数说明。
预处理参数选择：
param name=“SENSOR_HEIGHT” 传感器架设的高度 ，现实中与地面的相对高度

param name=“CLIP_HEIGHT” 裁切高度，将传感器高度 + CLIP_HEIGHT 以上的点云裁切掉
举例， SENSOR_HEIGHT = 0.75, CLIP_HEIGHT = 0.2,则 0.75 + 0.2 = 0.95 以上的点云裁切

param name=“MIN_DISTANCE” 移除自身较近处的反射点，根据雷达在车体上的位置确定，一般0.1-0.2即可

param name=“RADIAL_DIVIDER_ANGLE” 表示雷达水平方向偏转角度，即射线方法分割射线的角度值，十六线雷达一般设为0.18，三十二线雷达一般设为0.09,可以多次调试，选择较好的值
其余参数需根据实际应用场景调整。
实际效果：

总结

个人理解：中心思想与上一文章中的栅格法类似，就是将激光雷达扫描到的点云分簇，只不过栅格法中是将感知区域划分为一个个的扇形cell，但是射线法划分后从俯视图来看就是一个一个的线圈，雷达有多少线就有多少个线圈，通过角度找到扫面线的索引，通过与雷达的距离找到线圈的索引。并在这种划分的基础上遍历每个扫描线上前后点之间的高度差，将高度差与局部坡度阈值local_max_slope_或全局坡度阈值general_max_slope_对比进行地面点/非地面点属性的判断。
射线法的主要缺陷有：
存在少数漏检情况，部分非地面点容易被错误分割，造成障碍物点云的部分缺失，对于较远处接近雷达盲区的区域会存在错误分割的情况，将非地面点归为地面点。

参考自https://adamshan.blog.csdn.net/article/details/82901295#t6