基于RANSAC的激光雷达地面点云分割

如何利用RANSAC算法实现激光雷达的地面与非地面点云分割

利用激光雷达做感知输出首先要分割出地面点云以减少对障碍物聚类的影响。本文首先介绍RANSAC的基本原理，并依据RANSAC在ROS中实现对地面点云的分割。接着，引入PCL点云库，PCL点云库中有标准的RANSAC算法接口，我们可以通过调用它实现更加快速，稳定地滤除地面点云。

random sample consensus

ransac是 random sample consensus的缩写，翻译过来是随机抽样一致的意思。它表示可以从一组包含“局外点”的数据集中，通过迭代的方式估计某个数学模型的参数。这是一种包含概率统计思想的“不确定算法”，它的每次迭代并不能保证获得一个正确合理的结果，这是带有概率性的，要想提高获得这种合理结果的概率，就需要增加迭代次数。

ransac的基本假设

1. 整个数据集中同时包含好的点和不好的点，我们将它们分别称为局内点和局外点；
2. 数据的分布可以通过某个数学模型来描述，而局内点就是可以适应该模型的点，局外点是不可以适应该模型的点；
3. 随意给定一组点（可能是局内点也有可能是局外点），我们假设这组点都是局内点，它们满足某个数学模型，我们利用这个数学模型去估算其他的点，如果有足够多的点通过计算能被归类为假设的局内点，那么这个模型就足够合理。

ransac的算法流程

1. 随机在数据集中选取几个点假设为局内点，点的个数选择要依据模型的特征确定，一般来说选择适应于模型的最小数据个数。比如说，要在一堆点中拟合一条直线，那就选取两个点，因为两点确定一条直线；如果要拟合一个平面，那你至少需要选取三个点且不共线。
2. 计算适合这几个点的数学模型；
3. 根据设定的阈值计算数据集中的其他点是否满足该数学模型，如果满足，记为该模型的局内点；
4. 记录下该模型的局内点数量；
5. 重复迭代多次，每次产生的模型要么因为局内点个数太少而被舍弃，要么就比现存模型更好而被选用；
6. 满足迭代退出条件，退出循环，得到整个迭代过程中最合理的解。

如何确定迭代的退出条件？

用W表示随机抽到局内点的概率，用P表示置信度，这个参数表示RANSAC算法在运行后提供有用结果的期望概率。n表示计算模型参数需要选取的数据个数，k表示迭代次数，则

$$W^n表示随机抽取的n个点都是局内点的概率$$
$$1-W^n表示至少存在一个局外点的概率$$
$$(1-W^n{)}^k表示迭代k次都至少存在一个局外点的概率$$
$$当k足够大时，这个概率会变得很小，意味着模型可信度越高$$
$$那么迭代k次至少可以获得一组正确模型参数的概率为1-(1-W^n{)}^k$$
$$这个概率等于置信度参数P，有P=1-(1-W^n{)}^k$$
$$取对数，反推k的值为$$
$$k=log(1-P)/log(1-W^n{)}^k$$
$$这个k就是我们需要的迭代终止条件，当实际迭代次数大于k时，退出循环$$

ROS代码实现

首先订阅激光雷达的topic，我用的是速腾16线的激光雷达，车体左右侧各一个，此次只用左侧激光雷达的数据，安装高度1.06m。

int main(int argc, char **argv)
{
     
    srand((unsigned int)time(NULL));
    ros::init(argc, argv, "ransac_ground_detect");
    ros::NodeHandle nh;
    ros::Subscriber sub_ori = nh.subscribe("/left/rslidar_points",1,oripointsCallback);
    pub_groundPoint = nh.advertise<sensor_msgs::PointCloud2>("/ransac_groundPoint",1000);
    pub_nogroundPoint = nh.advertise<sensor_msgs::PointCloud2>("/ransac_nogroundPoint",1000);
    ros::spin();
    return 0;
}

然后在回调函数里面设置各项参数

void oripointsCallback(const sensor_msgs::PointCloud2::ConstPtr& oripoints_msg)
{
     
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::fromROSMsg(*oripoints_msg, *point_cloud_ptr);
    
    double BestPlaneEquationCoefficient[4] = {
     0};
    double distance_threshold = 0.2;
    double height_threshold = 0.55;
    double P = 0.999;
    int iter_count = 0;
    int max_point_num = 0;
    int K = 100;


    pcl::PointCloud<pcl::PointXYZI>::Ptr GroundinitPoint_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr GroundPoint_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr GroundPointBest_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr NoGroundPoint_ptr(new pcl::PointCloud<pcl::PointXYZI>);

根据激光雷达安装高度初步筛选地面点云，这样可以减少计算量，提高效率。

#pragma omp for
    for (int i = 0; i < point_cloud_ptr->points.size(); i++)
    {
     
        if (point_cloud_ptr->points[i].z >= -1.16 && point_cloud_ptr->points[i].z <= -0.96)
        {
     
            GroundinitPoint_ptr->points.push_back(point_cloud_ptr->points[i]);
        }
    }

然后是整个迭代过程，通过随机选择的三个点计算出一个平面模型，再判断这个模型是否适合其他点

while (iter_count < K)
    {
     
        GroundPoint_ptr->points.clear();
        int pointIndex[3] = {
      0 };
        for (int i = 0; i < 3; i++)
        {
     
            pointIndex[i] = rand() % GroundinitPoint_ptr->points.size();
        }

        double x1 = GroundinitPoint_ptr->points[pointIndex[0]].x;
        double y1 = GroundinitPoint_ptr->points[pointIndex[0]].y;
        double z1 = GroundinitPoint_ptr->points[pointIndex[0]].z;
        double x2 = GroundinitPoint_ptr->points[pointIndex[1]].x;
        double y2 = GroundinitPoint_ptr->points[pointIndex[1]].y;
        double z2 = GroundinitPoint_ptr->points[pointIndex[1]].z;
        double x3 = GroundinitPoint_ptr->points[pointIndex[2]].x;
        double y3 = GroundinitPoint_ptr->points[pointIndex[2]].y;
        double z3 = GroundinitPoint_ptr->points[pointIndex[2]].z;

        double vectorProduct = (x2 - x1)*(x3 - x1) + (y2 - y1)*(y3 - y1) + (z2 - z1)*(z3 - z1);
        double vectorLength1 = sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) + (z2 - z1)*(z2 - z1));
        double vectorLength2 = sqrt((x3 - x1)*(x3 - x1) + (y3 - y1)*(y3 - y1) + (z3 - z1)*(z3 - z1));
        double vectorAngle = vectorProduct / (vectorLength1*vectorLength2);

        if (abs(vectorAngle) == 1)
        {
     
            continue;
        }

        double planeEquationCoefficient[4] = {
      0 };

        planeEquationCoefficient[0] = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
        planeEquationCoefficient[1] = (z2 - z1)*(x3 - x1) - (x2 - x1)*(z3 - z1);
        planeEquationCoefficient[2] = (x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1);
        planeEquationCoefficient[3] = -(planeEquationCoefficient[0] * x1 + planeEquationCoefficient[1] * y1 + planeEquationCoefficient[2] * z1);
        
        for (int i = 0; i < GroundinitPoint_ptr->points.size(); i++)
        {
     
            double A = planeEquationCoefficient[0];
            double B = planeEquationCoefficient[1];
            double C = planeEquationCoefficient[2];
            double D = planeEquationCoefficient[3];
            double X = GroundinitPoint_ptr->points[i].x;
            double Y = GroundinitPoint_ptr->points[i].y;
            double Z = GroundinitPoint_ptr->points[i].z;
            double distance = abs(A*X + B*Y + C*Z + D) / (sqrt(A*A + B*B + C*C));

            if (distance < distance_threshold)
            {
     
                GroundPoint_ptr->points.push_back(GroundinitPoint_ptr->points[i]);
            } 
        }

        if (GroundPoint_ptr->points.size() > max_point_num)
        {
     
            GroundPointBest_ptr->points.clear();
            GroundPointBest_ptr->points.assign(GroundPoint_ptr->points.begin(),GroundPoint_ptr->points.end());
            max_point_num = GroundPointBest_ptr->points.size();

            for (int i = 0; i < 4; i++)
            {
     
                BestPlaneEquationCoefficient[i] = planeEquationCoefficient[i];
            }
            
            double GroundinitPointPtr_size = GroundinitPoint_ptr->points.size();
            double P_Ground = max_point_num / GroundinitPointPtr_size;
            double P_GroundTr = P_Ground*P_Ground*P_Ground;
            K = log(1 - P) / log(1 - P_GroundTr);
        }

        iter_count++;
        if (iter_count > K)
        {
     
            break;
        }
    }

起一个shell窗口，运行

rosbag play -l test.bag

cd 到对应工作空间，执行

source devel/setup.bash

然后运行节点

rosrun ransac_ground_detect ransac_ground_detect

打开rviz，设置Fixed Frame为 rslidar，添加topic


去除地面点云的效果如下

这个效果不怎么好，点云一直闪，而且非地面点云滤除的也比较多，后来接触了PCL点云库，调用里面的ransac接口来做地面分割发现稳定多了，效果也很好。

运行节点

rosrun lidar_ground_filter pcl_ransac_ground_filter

如下




完整代码地址https://download.csdn.net/download/dengxiong_bright/11928021
RANSAC维基百科https://en.wikipedia.org/wiki/Random_sample_consensus
测试数据下载地址https://pan.baidu.com/s/1MbUjV7sMCBdOLHpOa3pbvg