PCL提取建筑物的平面

//ComputeBuildingNormals
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/mls.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/ModelCoefficients.h>//模型系数头文件
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>//基于采样一致性分割的类的头文件


#include <pcl/filters/extract_indices.h>
//pcl::ModelCoefficients::Ptr cofficients (new pcl::ModelCoefficients);//模型系数
//pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

using namespace std;

typedef pcl::PointXYZ PointT;

void SegBuilding(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,string filename);//定义一个函数，对输入点云进行分割

int
main (int argc, char** argv)
{
    if(argc<2){
        cout<<"Error!"<<endl;
        return -1;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1],*cloud_in);
    std::cout<<"cloud_in: "<<cloud_in->size()<<endl;
    std::cout<<"fileds: "<<pcl::getFieldsList(*cloud_in)<<std::endl;


    // 对点云重采样
    pcl::search::KdTree<PointT>::Ptr treeSampling (new pcl::search::KdTree<PointT>);
    pcl::PointCloud<PointT> mls_point;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_smoothed;
    pcl::MovingLeastSquares<PointT,PointT> mls;
    mls.setComputeNormals(false);
    mls.setInputCloud(cloud_in);
    mls.setPolynomialOrder(2);
    mls.setPolynomialFit(false);
    mls.setSearchMethod(treeSampling);
    mls.setSearchRadius(0.05);
    mls.process(mls_point);

    // 输出重采样结果
    cloud_smoothed = mls_point.makeShared();

    ostringstream oss;
    oss<<"Smoothed_"<<argv[1];
    std::cout<<oss.str()<<"  |size: "<<cloud_smoothed->size() <<std::endl;
    std::cout<<oss.str()<<"  |fields: "<<pcl::getFieldsList(mls_point)<<std::endl;

    //save cloud_smoothed
    pcl::io::savePCDFileASCII(oss.str(),*cloud_smoothed);



    // 法线估计
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud(cloud_smoothed);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    normalEstimation.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    normalEstimation.setKSearch(10);
    normalEstimation.compute(*normals);
    std::cout<<"normals: "<<normals->size()<<"\n"<<"normals fields: "<<pcl::getFieldsList(*normals)<<std::endl;
    oss.str("");
    oss<<"NormalsOnly_"<<argv[1];
    pcl::io::savePCDFileASCII(oss.str(),*normals);
//    //Triangle Projection
//    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
//    pcl::concatenateFields(*cloud_smoothed,*normals,*cloud_with_normals);
//    std::cout<<"cloud_with_normals fields: "<<pcl::getFieldsList(*cloud_with_normals)<<std::endl;
//    oss.str("");
//    oss<<"Normals_"<<argv[1];
//    pcl::io::savePCDFileASCII(oss.str(),*cloud_with_normals);
//
//    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
//    tree2->setInputCloud(cloud_with_normals);
//
//    pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
//    pcl::PolygonMesh triangles;
//
//    gp3.setSearchRadius(0.1);
//    gp3.setMu(2.5);
//    gp3.setMaximumNearestNeighbors(52);
//
//    gp3.setMaximumAngle(2*M_PI/3);
//    gp3.setMinimumAngle(M_PI/18);
//
//    gp3.setMaximumSurfaceAngle(M_PI/4);
//    gp3.setNormalConsistency(false);
//
//    gp3.setInputCloud(cloud_with_normals);
//    gp3.setSearchMethod(tree2);
//    gp3.reconstruct(triangles);



    //可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer);
    int v4(0);
    viewer->setBackgroundColor(0,0,0,v4);
//    viewer->addPolylineFromPolygonMesh(triangles,"GPTriangle");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(cloud_smoothed, 0,200,0);

    //平面分割
    SegBuilding(cloud_smoothed,argv[1]);//
    cout<<"Final cloud_smoothed: "<<cloud_smoothed->size()<<endl;

    //保存最后的剩下的点云
    oss.str("");
    oss<<"Remain_"<<argv[1];

    pcl::io::savePCDFileASCII(oss.str(),*cloud_smoothed);
    cout<<oss.str()<<" Saved!"<<endl;
//    viewer->addText("cloud_filtered_out",0,0,"cloud_filtered_out",v4);

    //viewer->addCoordinateSystem();
    viewer->spin();


    return 0;
}

void SegBuilding(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, string filename) {

    cout<<"\nSegBuilding\n------------------------------------------------------"<<endl;
    pcl::PCDWriter writer;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());  //创建一个PointIndices结构体指针
    // 创建分割对象
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // 可选
    seg.setOptimizeCoefficients(true); //设置对估计的模型做优化处理
    // 必选
    seg.setModelType(pcl::SACMODEL_PLANE);//设置分割模型类别
    seg.setMethodType(pcl::SAC_RANSAC);//设置使用那个随机参数估计方法
    seg.setMaxIterations(1000);//迭代次数
    seg.setDistanceThreshold(0.045);//设置是否为模型内点的距离阈值
    // 创建滤波器对象
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    int i = 0, nr_points = (int) cloud_in->points.size();
    cout << "All Point: " << cloud_in->size() <<"\n--------------------"<< endl;
    double PointNumber = cloud_in->size();
    // 当还多于30%原始点云数据时
    while (cloud_in->points.size() > 0.1 * nr_points) {
        // 从余下的点云中分割最大平面组成部分
        seg.setInputCloud(cloud_in);
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0) {
            std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        // 分离内层
        extract.setInputCloud(cloud_in);
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*cloud_p);//将分割点云根据inliers提取到cloud_p中
        PointNumber = PointNumber - cloud_p->size();
        std::cerr << "after cloud_filtered: " << PointNumber << std::endl;//剩余的点云

        //保存
        std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height
                  << " data points." << std::endl;
        std::stringstream ss;
        ss << "Plane_" << i <<"_"<< filename; //对每一次的提取都进行了文件保存
        writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);
        cout<<ss.str()<<" saved!"<<endl;
        std::cerr << "----------------------------------" << std::endl;

        // 再次创建滤波器对象
        extract.setNegative(true);//提取外层
        extract.filter(*cloud_f);//将外层的提取结果保存到cloud_f
        cloud_in.swap(cloud_f);//经cloud_filtered与cloud_f交换


        i++;


    }
}

效果如图：