点云多平面分割--实操（源码供参考）

思路

分割出多个平面，并赋予不同颜色进行显示，将点云坐标与颜色保存为pcd点云。。。

源码

//****************************************************************
//   平面分割
//   source /home/robot/catkin_ws/devel/setup.bash && rosrun chapter10_tutorials  plane
//
//****************************************************************

#include <iostream>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree.h>
//分割类
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/filters/passthrough.h>  //直通滤波器头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#define DisThre 0.03//平面分割阈值

using namespace std;
using PointT = pcl::PointXYZRGB;
uint8_t R;
uint8_t G;
uint8_t B;

FILE *fpWrite=fopen("cloud_all.txt","w");//a续写，w清除后写入
pcl::PointCloud<PointT>::Ptr cloud_all(new pcl::PointCloud<PointT>);

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("planar segment"));
void Plane_fitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input);
void output_plane(pcl::PointCloud<PointT>::Ptr cloud_plane,int begin);
void  cloudPassThrough(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const char *zhou,int min,int max);
int main(int argc, char** argv)
{
    // -------------------------------------------加载点云---------------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile <pcl::PointXYZ>("cloud1.pcd", *cloud) == -1)
    {
        PCL_ERROR("Cloud reading failed.");
        return (-1);
    }

    viewer->setBackgroundColor(0, 0, 0);
    viewer->setWindowName("平面分割");

    //直通滤波
    cloudPassThrough(cloud,"y",-20,20);
    cloudPassThrough(cloud,"x",5,50);
    cloudPassThrough(cloud,"z",-5,15);

    Plane_fitting(cloud);

    pcl::io::savePCDFileBinary("cloud_all.pcd", *cloud_all);
    cout << "cloud_all.pcd 保存完毕！！！" << endl;
    for(std::size_t i=0; i< cloud_all->size(); i++)
        fprintf(fpWrite,"%2.3f %2.3f %2.3f %d %d %d \n",cloud_all->points[i].x,cloud_all->points[i].y,cloud_all->points[i].z,cloud_all->points[i].r,cloud_all->points[i].g,cloud_all->points[i].b);
    fclose(fpWrite);
    cout << "cloud_all.txt 保存完毕！！！" << endl;

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    return (0);
}


void Plane_fitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input)
{

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(300);
    seg.setDistanceThreshold(DisThre);

    int m=0;
    while (cloud_input->size() > 100)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
        seg.setInputCloud(cloud_input);
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0)
        {
            break;
        }
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_input);
        extract.setIndices(inliers);
        extract.filter(*cloud_plane);//输出平面

        if (cloud_plane->size()>40)
        {
            m++;
            // 可视化相关的代码
            R = rand() % (256) + 0;
            G = rand() % (256) + 0;
            B = rand() % (256) + 0;
            cout<<"cloud_cluster->size()="<<cloud_plane->size()<<endl;
            pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);
            for(std::size_t k=0; k<cloud_plane->size(); k++ )
            {
                PointT thispoint;
                thispoint.x=cloud_plane->points[k].x;
                thispoint.y=cloud_plane->points[k].y;
                thispoint.z=cloud_plane->points[k].z;
                thispoint.r=R;
                thispoint.g=G;
                thispoint.b=B;
                cloud_cluster->push_back(thispoint);
                cloud_all->push_back(thispoint);
            }
            output_plane(cloud_cluster,m);
        }
        // 移除plane
        extract.setNegative(true);
        extract.filter(*cloud_p);
        *cloud_input = *cloud_p;
    }

}
void output_plane(pcl::PointCloud<PointT>::Ptr cloud_plane,int begin)
{
    std::stringstream ss;
    ss << "plane_" << begin<< ".pcd";
    pcl::io::savePCDFileBinary(ss.str(), *cloud_plane);
    cout << ss.str() << "保存完毕！！！" << endl;

    string str;
    ss >> str;
    pcl::visualization::PointCloudColorHandlerCustom<PointT> color(cloud_plane, R, G, B);
    viewer->addPointCloud<PointT>(cloud_plane, color, str);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, str);
}

//直通滤波器对点云进行处理
void  cloudPassThrough(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const char *zhou,int min,int max)
{
     pcl::PassThrough<pcl::PointXYZ> passthrough;
     passthrough.setInputCloud(cloud);//输入点云
     passthrough.setFilterFieldName(zhou);//对z轴进行操作
     passthrough.setFilterLimits(min,max);//设置直通滤波器操作范围
     passthrough.filter(*cloud);//);//执行滤波

}

数据

配套数据：
https://download.csdn.net/download/xx970829/85038137