点云平面模型分割

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <iostream>
#include <vector>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/uniform_sampling.h>
//读取txt点云文件
void CreateCloudFromTxt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    std::ifstream file(file_path.c_str());
    std::string line;
    pcl::PointXYZ point;
    while (getline(file, line)) {
        std::stringstream ss(line);
        ss >> point.x;
        ss >> point.y;
        ss >> point.z;
        cloud->push_back(point);
    }
    file.close();
}
//加载点云数据
void loadPointCloud(const std::string& fileName, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    std::string suffix_str = fileName.substr(fileName.find_last_of('.') + 1);//获取文件后缀名

    if(suffix_str.compare("pcd") == 0){       //pcd点云文件
        if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *cloud) == -1) //* load the pcd file
        {
            std::cout <<"Couldn't read file  \n";
            return;
        }
    }
    else if(suffix_str.compare("ply") == 0){  //ply点云文件
        if (pcl::io::loadPLYFile<pcl::PointXYZ>(fileName, *cloud) == -1) //* load the ply file
        {
            std::cout <<"Couldn't read file  \n";
            return;
        }
    }
    else if(suffix_str.compare("txt") == 0){  //txt点云文件
        CreateCloudFromTxt(fileName, cloud);
    }
    else{
        std::cout << "点云文件格式错误" << std::endl;
    }
}

int main(int argc, char *argv[])
{
    //读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);//原始点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::string fileName = "xxx.pcd";

    //读取点云数据
    loadPointCloud(fileName, cloud_in);

    //原始点云点太多了，均匀滤波一下
    pcl::UniformSampling<pcl::PointXYZ> unifm_smp;
    unifm_smp.setRadiusSearch(0.01);
    unifm_smp.setInputCloud(cloud_in);
    unifm_smp.filter(*cloud);


    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inner (new pcl::PointCloud<pcl::PointXYZ>);//平面内的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_outer (new pcl::PointCloud<pcl::PointXYZ>);//平面外的点

    //进行平面分割
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);//创建分割时所需的模型系数对象
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//创建储存内点的点索引集合对象

    pcl::SACSegmentation<pcl::PointXYZ> seg;// 创建分割对象
    seg.setOptimizeCoefficients(true);// 设置模型系数需要优化

    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);//分割的模型类型
    seg.setMethodType(pcl::SAC_RANSAC);//随机参数的估计方法
    seg.setDistanceThreshold(0.01);//阈值距离

    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);//输出

    if (inliers->indices.size() == 0)
    {
        PCL_ERROR("Could not estimate a planar model for the given dataset.");
        return (-1);
    }
    //平面内的点
    cloud_inner->width = inliers->indices.size();
    cloud_inner->height = 1;
    cloud_inner->is_dense = false;
    cloud_inner->resize(cloud_inner->height * cloud_inner->width);
    for(size_t i=0;i<inliers->indices.size();i++)
    {
        cloud_inner->points[i].x = cloud->points[inliers->indices[i]].x;
        cloud_inner->points[i].y = cloud->points[inliers->indices[i]].y;
        cloud_inner->points[i].z = cloud->points[inliers->indices[i]].z;
    }
    //平面外的点
    cloud_outer->width = cloud->points.size() - inliers->indices.size();
    cloud_outer->height = 1;
    cloud_outer->points.resize(cloud_outer->width * cloud_outer->height);

    //创建一个数组，大小为点云总数,初始化为0
    std::vector<int> p_flag(cloud->points.size());

    //将平面内的点标记
    for (size_t i = 0; i < inliers->indices.size(); ++i)
        p_flag[inliers->indices[i]] = 1;

    for (size_t i = 0,j=0 ; i < cloud->points.size(); ++i)
    {
        //遍历，找出平面外的点
        if (p_flag[i] == 0)
        {
            cloud_outer->points[j].x = cloud->points[i].x;
            cloud_outer->points[j].y = cloud->points[i].y;
            cloud_outer->points[j].z = cloud->points[i].z;
            ++j;
        }
    }

    //将点云加入viewer窗口可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

    int v1(1);
    int v2(2);

    //创建视角v1，v2
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0,v1);
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0,v2);


    //设置点云颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inner_rgb(cloud_inner, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outer_rgb(cloud_outer, 255, 255, 0);

//    viewer->addPointCloud(cloud_filtered, "cloud_filtered");
    viewer->addPointCloud(cloud, "cloud", v1);
    viewer->addPointCloud(cloud_inner, inner_rgb, "cloud_inner", v2);
    viewer->addPointCloud(cloud_outer, outer_rgb, "cloud_outer", v2);

//    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "cloud_out");//设置关键点大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_inner");//设置关键点大小

    viewer->setBackgroundColor (0, 0.3, 0.4);//设置视口的背景颜色
    // 阻塞式
    viewer->spin();
}