代码:
#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博客