点云分割

这是官网的一个例子,参考网上一些前辈的注释自己理解了一下.

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.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/filters/passthrough.h>
#include <pcl/visualization/pcl_visualizer.h>

//强制类型转换
typedef pcl::PointXYZ PointT;

int
main (int argc, char** argv)
{
  
  //所属类,数据类型,名称
  pcl::PCDReader reader;       //读取pcd文件
  pcl::PassThrough<PointT> pass;    //直通滤波器
  pcl::NormalEstimation<PointT, pcl::Normal> ne;   //法线估计对象
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;    //分割对象
  pcl::PCDWriter writer;       //写入pcd文件
  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<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

  //创建可视化窗口
  pcl::visualization::PCLVisualizer viewer("滤波");

  int v1(0);   //设置左右窗口
  int v2(1);

  viewer.createViewPort(0.0, 0.0, 0.5, 1, v1);
  viewer.setBackgroundColor(0, 0, 0, v1);
 
  viewer.createViewPort(0.5, 0.0, 1, 1, v2);
  viewer.setBackgroundColor(0.5, 0.5, 0.5, v2);

  // Read in the cloud data
  reader.read ("/home/zqh/table_scene_mug_stereo_textured.pcd", *cloud);
  std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;

   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_green(cloud, 20, 180, 20);      // 显示绿色点云
   viewer.addPointCloud(cloud, cloud_out_green, "cloud_out1", v1); 


  pass.setInputCloud (cloud);      //设置输入点云
  pass.setFilterFieldName ("z");   //设置z轴为过滤字段
  pass.setFilterLimits (0, 1.5);
  pass.filter (*cloud_filtered);   //将过滤之后的点云存入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_NORMAL_PLANE);
  seg.setNormalDistanceWeight (0.1);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.03);
  seg.setInputCloud (cloud_filtered);
  seg.setInputNormals (cloud_normals);
  // Obtain the plane inliers and coefficients
  seg.segment (*inliers_plane, *coefficients_plane);  //分割　得到平面系数　已经在平面上的点的　索引
  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

  // Extract the planar inliers from the input cloud
  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;

  // Remove the planar inliers, extract the rest
  extract.setNegative (true);
  extract.filter (*cloud_filtered2);       //过滤之后保存到cloud_filtered2
  extract_normals.setNegative (true);
  extract_normals.setInputCloud (cloud_normals);
  extract_normals.setIndices (inliers_plane);           //索引平面内的点
  extract_normals.filter (*cloud_normals2);       //法线过滤之后存在cloud_normals2


  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, *coefficients_cylinder);
  std::cerr << "Cylinder coefficients: " << *coefficients_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);
  
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_orage(cloud_cylinder, 250, 128, 10);     //显示橘色点云
  viewer.addPointCloud(cloud_cylinder, cloud_out_orage, "cloud_out2", v2);
//viewer.setSize(960, 780);
  while (!viewer.wasStopped())
  {
  viewer.spinOnce();
  }
  return (0);
}

结果:

这个例子用到的知识点比较多,主要是要熟悉RANSAC算法的特点
后续接着这个例子学习下点云索引提取子集 pcl::ExtractIndices extract;
以及学习下点云法向量估计 pcl::NormalEstimation<PointT, pcl::Normal> ne;