使用PCL提取平面的方式

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

1.RANSAC

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

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

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

    int totle_in_size = in_totle->points.size();
    pl_num = 0;
    std::vector<std::vector<double> > 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<pcl::PointXYZ>::Ptr outgr_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr ingr_cloud(new pcl::PointCloud<pcl::PointXYZ>);

        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"<<endl;
            continue;
        }

        //attribute:normals not used
        double A = coefficients->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 / s