『分割』分割圆柱

代码：

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h> // 用于提取指定索引的数据
#include <pcl/filters/passthrough.h>

#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointXYZ PointT;

int main(){
    pcl::PCDReader reader;
    pcl::PassThrough<PointT> pass;
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
    pcl::PCDWriter writer;
    pcl::ExtractIndices<PointT> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);


    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>);

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);

    pcl::ModelCoefficients::Ptr coefficient_plane(new pcl::ModelCoefficients), coefficient_cylinder(new pcl::ModelCoefficients);

    pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices);


    // 读取点云数据
    reader.read("/home/jason/file/pcl-learning/12segmentation分割/2cylinder_segmentation/table_scene_mug_stereo_textured.pcd", *cloud);
    std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl;

    // 直通滤波
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0, 1.5);
    pass.filter(*cloud_filtered);
    std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;

    // 过滤后的点云进行法线估计，为后续进行基于法线分割准备数据
    ne.setSearchMethod(tree);
    ne.setInputCloud(cloud_filtered);
    ne.setKSearch(50);
    ne.compute(*cloud_normals);

    // 分割平面模型
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight(0.1);
    seg.setMaxIterations(100);
    seg.setDistanceThreshold(0.03);
    seg.setInputCloud(cloud_filtered);
    seg.setInputNormals(cloud_normals);

    // 获取平面模型的系数和处在平面的内点
    seg.segment(*inliers_plane, *coefficient_plane);
    std::cerr << "plane coefficients: " << *coefficient_plane << std::endl;

    // 从点云中抽取分割的处在平面上的点集
    extract.setInputCloud(cloud_filtered);
    extract.setIndices(inliers_plane);
    extract.setNegative(false); // 不提取索引的补集，仅提取与索引匹配的点，即提取属于平面的点


    // 存储分割得到的平面上的点到点云文件
    pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>);
    extract.filter(*cloud_plane);
//    std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
//    writer.write("result.pcd", *cloud_plane, false); // 不以二进制写入点云数据，即ASCII编码的文本形式写入点云数据到文件中

//    // Remove the planar inliers, extract the rest
    extract.setNegative(true); // 索引的补集
    extract.filter(*cloud_filtered2); // 提取点云数据

    extract_normals.setNegative(true); // 索引的补集
    extract_normals.setInputCloud(cloud_normals); // 输入法线数据
    extract_normals.setIndices(inliers_plane); // 设置索引
    extract_normals.filter(*cloud_normals2); // 提取法线数据

    // Create the segmentation object for cylinder segmentation and set the parameters
    seg.setOptimizeCoefficients(true);          // 设置对估计模型优化
    seg.setModelType(pcl::SACMODEL_CYLINDER);   // 设置分割模型为圆柱形
    seg.setMethodType(pcl::SAC_RANSAC);         // 参数估计方法
    seg.setNormalDistanceWeight(0.1);           // 设置表面法线权重系数
    seg.setMaxIterations(10000);                // 设置迭代的最大次数
    seg.setDistanceThreshold(0.05);             // 设置内点到模型的距离允许最大值
    seg.setRadiusLimits(0, 0.1);               // 设置估计出的圆柱模型的半径的范围， 这个蛮重要的
    seg.setInputCloud(cloud_filtered2);
    seg.setInputNormals(cloud_normals2);

    // Obtain the cylinder inliers and coefficients
    seg.segment(*inliers_cylinder, *coefficient_cylinder);
    std::cerr << "Cylinder coefficients: " << *coefficient_cylinder << std::endl;

    // write the cylinder inliers to disk
    extract.setInputCloud(cloud_filtered2);
    extract.setIndices(inliers_cylinder);
    extract.setNegative(false);
    pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>);
    extract.filter(*cloud_cylinder);
    if (cloud_cylinder->points.empty())
        std::cerr << "Can't find the cylindercal companent." << std::endl;
    else
    {
        std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl;
        writer.write("result2.pcd", *cloud_cylinder, false); //
    }

    // 可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));



//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_red(cloud_filtered2, 255, 0, 0);
//    viewer->addPointCloud(cloud_filtered2, cloud_out_red, "cloud_out1");

//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_green(cloud_plane, 0, 255, 0);
//    viewer->addPointCloud(cloud_plane, cloud_out_green, "cloud_out2");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_blue(cloud_cylinder, 0, 255, 0);
    viewer->addPointCloud(cloud_cylinder, cloud_out_blue, "cloud_out3");

    viewer->spin();
    return 0;

}
注意：
这里是先分割平面，然后在点云数据中剔除属于平面的点云，然后才分割圆柱形。
不要觉得分割平面这一步多余，直接分割圆柱形的效果差的一批
参考：
PCL学习笔记（二十五）-- 圆柱体模型分割_pcl 分割圆柱_看到我请叫我学C++的博客-CSDN博客
『分割』 分割圆柱

代码：

猜你喜欢

『分割』分割圆柱
