使用PCL提取平面的方式

本文主要总结了几种提取平面的方式,算法的原理就不细说了.

1.RANSAC

循环提取平面,直到小于20个点.

void groundRemoval(pcl::PointCloud::Ptr in,int& pl_num)
{

    pcl::PointCloud::Ptr in_totle(new pcl::PointCloud);
    *in_totle = *in;

    int totle_in_size = in_totle->points.size();
    pl_num = 0;
    std::vector > pl_nor_tot;
    while (in_totle->points.size() > 20)
    {
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointCloud::Ptr outgr_cloud(new pcl::PointCloud);
        pcl::PointCloud::Ptr ingr_cloud(new pcl::PointCloud);

        doRansacPlane(in_totle, inliers, coefficients,0.015);//plane

        ingr_cloud = filterCloud(in_totle, inliers, false);//plane points
        outgr_cloud = filterCloud(in_totle, inliers, true);
        *in_totle = *outgr_cloud;
        if(ingr_cloud->points.size() < 200)
        {
            //cout<<"ingr_cloud size is small,continue"<values[0];
        double B = coefficients->values[1];
        double C = coefficients->values[2];
        double D = coefficients->values[3];

        double Aunit = A / sqrt(A*A+B*B+C*C);
        double Bunit = B / sqrt(A*A+B*B+C*C);
        double Cunit = C / sqrt(A*A+B*B+C*C);

        std::vector pl_nor{Aunit,Bunit,Cunit};
        pl_nor_tot.push_back(pl_nor);

        //attribute:distance
        double ydistance = 0;
        //double ydistance_max = 0;
        double zdistance = 0;
        int ingr_num = ingr_cloud->points.size();
        for(int i = 0 ;i < ingr_num;i++)
        {
            if(dir_choice == 0)
            {
                ydistance += (ingr_cloud->points[i].x);
            }
            else if(dir_choice == 1)
            {
                ydistance += (ingr_cloud->points[i].y);
                zdistance += (ingr_cloud->points[i].z);
            }

            // if((ingr_cloud->points[i].y) > ydistance_max)
            // {
            //      ydistance_max = (ingr_cloud->points[i].y) ;
            // }
        }
        double ydistance_ave = ydistance / double(ingr_num);
        double zdistance_ave = zdistance / double(ingr_num);
        cout<<"-----plane:"<::Ptr PlaneCloud;
};
//ransac提取平面
void doRansacPlane(pcl::PointCloud::Ptr cloud, 
                   pcl::PointIndices::Ptr              inliers, 
                   pcl::ModelCoefficients::Ptr         coefficients,
                   double                              distance_threshold)
{
    int max_iterations = 500;
    pcl::SACSegmentation seg;

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(distance_threshold);
    seg.setMaxIterations(max_iterations);
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
}
//根据索引提取所需点云
    pcl::PointCloud::Ptr filterCloud(pcl::PointCloud::Ptr cloud, 
                                                pcl::PointIndices::Ptr              indices, 
                                                bool                                negative)
{
    pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud);
    pcl::ExtractIndices extract;
    extract.setInputCloud(cloud);
    extract.setIndices(indices);
    extract.setNegative(negative);
    extract.filter(*cloud_filtered);

    return cloud_filtered;
}

黄色为提取到的平面,红色为马路牙子线.

2.区域增长法

void GroundRemoval(pcl::PointCloud::Ptr in)
{

pcl::PointCloud::Ptr cloud_normals (new pcl::PointCloud);
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
pcl::NormalEstimation ne; 
pcl::PointCloud::Ptr outgr_cloud(new pcl::PointCloud);
//pcl::PointCloud::Ptr ingr_cloud(new pcl::PointCloud);

ne.setInputCloud (in);
ne.setSearchMethod (tree);
ne.setKSearch (15);
ne.setViewPoint(std::numeric_limits::max (),std::numeric_limits::max (), std::numeric_limits::max ());
ne.compute (*cloud_normals);
//cout<<"cloud_normals size is "<< cloud_normals->size()< reg;
reg.setMinClusterSize (100);  
reg.setMaxClusterSize (10000); 
reg.setSearchMethod (tree);   
reg.setNumberOfNeighbours (30);  //ini =30
reg.setInputCloud (in);        
//reg.setIndices (indices);
reg.setInputNormals (cloud_normals);    
reg.setSmoothnessThreshold (4.0 / 180.0 * M_PI);  //ini =3.
reg.setCurvatureThreshold (1.0); 

std::vector  clusters;
reg.extract (clusters);

// cout << "Number of clusters is equal to " << clusters.size () << endl;
// cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;

for(int i = 0 ;i < clusters.size();i++)
{
    double distance = 0;
    double distance_min = 100;
    pcl::PointCloud::Ptr ingr_cloud(new pcl::PointCloud);

    for(int m = 0;m < clusters[i].indices.size ();m++)
    {
        ingr_cloud->push_back(in->points[clusters[i].indices[m]]);
    }

    int ingr_num = ingr_cloud->points.size();
    //cout<<"ingr_num=="<points[j].z);//todo y
        if(fabs(ingr_cloud->points[j].z) < distance_min)
        {
            distance_min = fabs(ingr_cloud->points[j].z);
        }
    }
    double distance_ave = distance / double(ingr_num);
    planeout[i].distance = distance_min;
    *planeout[i].PlaneCloud = *ingr_cloud;
    // cout<<"distance_min=="<::Ptr colored_cloud = reg.getColoredCloud ();
    pcl::visualization::CloudViewer viewer ("Cluster viewer");
    viewer.showCloud(colored_cloud);
    while (!viewer.wasStopped ())
    {
    }
}

3.Normals + RANSAC

4.DoN + RANSAC

ps:后续两种因时间问题,暂时先不整理代码上传了.