pcl点云库采样一致性算法(11)

注：本例是pcl点云库学习教程中第十一章采样一致性算法中应用实例

首先对两个点云进行定义初始化，并对其中一个点云填充点云数据作为处理前的原始点云，其中大部分点云数据是基于设定的圆球

和平面模型计算而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的作为局外点。

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>


boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  //viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  return (viewer);
}


int
main(int argc, char** argv)
{
srand(time(NULL));
  // initialize PointClouds  初始化点云对象
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);//存储源点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);//存储提取的局内点


  // populate our PointCloud with points
 //填充点云数据
  cloud->width    = 5000;//设置点云数目
  cloud->height   = 1;
  cloud->is_dense = false;
  cloud->points.resize (cloud->width * cloud->height);
  //生成数据
  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    {
      cloud->points[i].x =  rand () / (RAND_MAX + 1.0);
      cloud->points[i].y =  rand () / (RAND_MAX + 1.0);
      if (i % 5 == 0)
        cloud->points[i].z =  rand () / (RAND_MAX + 1.0);
      else if(i % 2 == 0)
        cloud->points[i].z =  sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
                                      - (cloud->points[i].y * cloud->points[i].y));
      else
        cloud->points[i].z =  - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
                                        - (cloud->points[i].y * cloud->points[i].y));
    }
    else//根据命令行参数，用x+y+z=1 设置一部分点云数据，此时点云组成菱形平面作为内点
    {
      cloud->points[i].x =  rand () / (RAND_MAX + 1.0);
      cloud->points[i].y =  rand () / (RAND_MAX + 1.0);
      if( i % 5 == 0)
        cloud->points[i].z = rand () / (RAND_MAX + 1.0);
      else
        cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
    }
  }


  std::vector<int> inliers;//存储局内点集合的点的索引的向量


  //创建随机采样一致性对象
  // created RandomSampleConsensus object and compute the appropriated model
  pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
    model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));//针对球模型的对象
  pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));//针对平面模型的对象
  if(pcl::console::find_argument (argc, argv, "-f") >= 0)
  {//根据命令行参数，来随机估算对应的平面模型，并存储估计的局内点
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
    ransac.setDistanceThreshold (.01);//与平面距离小于0.01的点作为局内点考虑
    ransac.computeModel();//执行随机参数估计
    ransac.getInliers(inliers);//存储估计所得的局内点
  }
  else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
  {//根据命令行参数，来随机估算对应的球面模型，并存储估计的局内点
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }


  // copies all inliers of the model computed to another PointCloud
  //复制估算模型的所有局内点到final中
  pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);


  // creates the visualization object and adds either our orignial cloud or all of the inliers
  // depending on the command line arguments specified.
  //创建可视化对象，并加入原始点云或者所有的局内点
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    viewer = simpleVis(final);
  else
    viewer = simpleVis(cloud);
  while (!viewer->wasStopped ())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }
  return 0;
 }