点云区域增长分割

#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>
#include <pcl/features/normal_3d.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/region_growing.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 = "C:/Users/A/Desktop/pointCloudData/Tutorial_Cloud_Couch_bin_compressed.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);


    //设置默认输入参数
    int KN_normal=30;
    //bool Bool_Cuting=false;
    float SmoothnessThreshold = 2.5, CurvatureThreshold = 20;

    //法线估计
    pcl::search::Search<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);//创建一个指向kd树搜索对象的共享指针
    pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;//创建法线估计对象
    normal_estimator.setSearchMethod (tree);//设置搜索方法
    normal_estimator.setInputCloud(cloud);//设置法线估计对象输入点集
    normal_estimator.setKSearch (KN_normal);// 设置用于法向量估计的k近邻数目
    normal_estimator.compute (*normals);//计算并输出法向量

    // 区域生长算法的5个参数
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;//创建区域生长分割对象
    reg.setMinClusterSize (100);//设置一个聚类需要的最小点数
    reg.setMaxClusterSize (100000);//设置一个聚类需要的最大点数
    reg.setSearchMethod (tree);//设置搜索方法
    reg.setNumberOfNeighbours (15);//设置搜索的临近点数目
    reg.setInputCloud(cloud);//设置输入点云

    reg.setInputNormals (normals);//设置输入点云的法向量
    reg.setSmoothnessThreshold (SmoothnessThreshold / 180.0 * M_PI);//设置平滑阈值
    reg.setCurvatureThreshold (CurvatureThreshold);//设置曲率阈值
    std::vector <pcl::PointIndices> clusters;
    reg.extract (clusters);//获取聚类的结果，分割结果保存在点云索引的向量中。

    std::cout << clusters.size() << std::endl;

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RegionGrow; //用于储存区域增长分割后的点云
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
            cloud_cluster->points.push_back(cloud->points[*pit]);
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;
        RegionGrow.push_back(cloud_cluster);

    }

    //将点云加入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);

//    viewer->addPointCloud(RegionGrow[0], "cloud", v2);
    viewer->addPointCloud(cloud, "cloud", v1);
    for (int i = 0; i < RegionGrow.size(); i++)
    {
        //显示分割得到的各片点云
        // 随机生成颜色
        uint8_t R = rand() % (256) + 0;
        uint8_t G = rand() % (256) + 0;
        uint8_t B = rand() % (256) + 0;
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(RegionGrow[i], R, G, B);
        viewer->addPointCloud(RegionGrow[i], color, std::to_string(i), 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();
}