code:
#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;
}
Notice:
Here, the plane is first divided, and then the point cloud belonging to the plane is eliminated in the point cloud data, and then the cylinder is divided.
Don't think that the step of splitting the plane is redundant, and the effect of directly splitting the cylinder is poor.
reference: