（编程）点云地面分割——RANSAC平面拟合

1.介绍

依据传感器高度筛选地面高度附近点，拟合平面即为地面

只适用于局部平坦的地面提取

2.代码

/*依赖文件*/
#include <string>
#include<iostream>
#include <cstdlib>
#include<fstream>
#include <sstream>	
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <pcl/io/ply_io.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/registration/icp.h>
#include <pcl/common/common.h> 
#include <pcl/filters/passthrough.h> 
#include <boost/thread/thread.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/progressive_morphological_filter.h>

int main()
{
    /*读取点云文件*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/subway.pcd", *cloud) == -1) 
    {
        PCL_ERROR("Couldn't read file\n");
        return(-1);
    }
    /*ransac平面拟合*/
    /*根据传感器安装高度筛选地面可能点*/
    pcl::PointCloud<pcl::PointXYZ> ground_cloud_selected;
    double sensor_z = -1.75;
    for (size_t i = 0; i < cloud->points.size(); ++i) 
    {
        if(cloud->points[i].z <= sensor_z + 0.05 && cloud->points[i].z >= sensor_z - 0.05)
        {
            ground_cloud_selected.push_back(cloud->points[i]);
        }
    }
    /*拟合平面*/
    std::vector<int> ground_inliers;/*plane simulation by RANSAC*/
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(ground_cloud_selected.makeShared()));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);
    ransac.setDistanceThreshold(0.1);
    ransac.computeModel();
    ransac.getInliers(ground_inliers);
    pcl::PointCloud<pcl::PointXYZ> ground_cloud, not_ground_cloud;
    pcl::copyPointCloud<pcl::PointXYZ>(ground_cloud_selected, ground_inliers, ground_cloud);
    for (size_t i = 0; i < cloud->points.size(); ++i) 
    {
        int flag = 0;
        for (size_t j = 0; j < ground_cloud.points.size(); ++j) 
        {
            if(cloud->points[i].x == ground_cloud.points[j].x && cloud->points[i].y == ground_cloud.points[j].y && cloud->points[i].z == ground_cloud.points[j].z)
            {
                flag = 1;
                break;
            }
        }
        if(flag == 0)
        {
            not_ground_cloud.push_back(cloud->points[i]);
        }
    }
    /*存储与显示*/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud show"));
    int v1 = 0;
    viewer->createViewPort(0, 0, 1.0, 1.0, v1);
    viewer->setBackgroundColor(255,255, 255, v1);
    viewer->setBackgroundColor(255,255, 255);
    pcl::PointCloud<pcl::PointXYZ>::Ptr g_cloud = ground_cloud.makeShared();
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> raw(cloud, 0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> g_pt(g_cloud, 255,0, 0);
    viewer->addPointCloud(cloud, raw, "source cloud", v1);
    viewer->addPointCloud(g_cloud, g_pt, "select ground point", v1);
    std::string path = "../ground.pcd";
    pcl::io::savePCDFile(path, *g_cloud);
    while (!viewer->wasStopped())
    {
	viewer->spinOnce(100);
	boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
    system("pause");
    return 0;
}

3.示例

(图中红色为地面点云)