PCL经典代码赏析五：随机一致性采样算法

· 说明

• 以下均为 Being_young 前辈所写，现转载过来，再加上自己的理解，重新写了一遍，方便自己日后使用
• 博客地址：http://blog.csdn.net/u013019296/article/

---

· 目录索引

1. PCL 采样一致性算法
2. 随机采样一致性算法 (RANSAC)
3. 最小中值法 (LMeds)
4. PCL中 Sample_consensus 模块及类的介绍

---

· PCL 采样一致性算法

在计算机视觉领域广泛的使用各种不同的采样一致性参数估计算法用于排除错误的样本，样本不同对应的应用不同，例如剔除错误的配准点对，分割出处在模型上的点集、

PCL中以随机采样一致性算法（RANSAC）为核心，同时实现了五种类似与随机采样一致形算法的随机参数估计算法：

• 随机采样一致性算法（RANSAC）
• 最大似然一致性算法（MLESAC）
• 最小中值方差一致性算法（LMEDS）
• **
• **

所有的这些估计参数算法都符合一致性原则。在PCL中设计的采样一致性算法的应用主要就是对点云进行分割，根据设定的不同的模型，估计对应的几何参数模型的参数，在一定容许的范围内分割出在模型上的点云。

---

· 随机采样一致性算法 (RANSAC)

RANSAC是“Random Sample Consensus（随机抽样一致）”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。

想进一步理解，请参考：关于RANSAC的理解，网上的参考教程很多

#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, "global");
  viewer->initCameraParameters ();
  return (viewer);
}
/******************************************************************************************************************
 对点云进行初始化，并对其中一个点云填充点云数据作为处理前的的原始点云，其中大部分点云数据是基于设定的圆球和平面模型计算
  而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的组委局外点。
 *****************************************************************************************************************/
int
main(int argc, char** argv)
{
  // 初始化点云对象
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点

  // 填充点云数据
  cloud->width    = 500;                 //填充点云数目
   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)
    {
//根据命令行参数用x^2+y^2+Z^2=1设置一部分点云数据，此时点云组成1/4个球体作为内点
      cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if (i % 5 == 0)
        cloud->points[i].z = 1024 * 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 = 1024 * rand () / (RAND_MAX + 1.0);
      cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if( i % 2 == 0)
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);   //对应的局外点
      else
        cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
    }
  }

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

  //创建随机采样一致性对象
  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);
  }

  // 复制估算模型的所有的局内点到final中
  pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

  // 创建可视化对象并加入原始点云或者所有的局内点

  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;
 }

---

· 最小中值法 (LMeds)

LMedS 就是从样本中随机抽出N个样本子集，使用最大似然（通常是最小二乘）对每个子集计算模型参数和该模型的偏差，记录该模型参数及子集中所有样本中偏差居中的那个样本的偏差（即Med偏差），最后选取N个样本子集中Med偏差最小的所对应的模型参数作为我们要估计的模型参数

//代码暂无

---

· PCL中Sample_consensus模块及类的介绍

PCL中Sample_consensus库实现了随机采样一致性及其泛化估计算法，例如平面，柱面，等各种常见的几何模型，用不同的估计算法和不同的几何模型自由的结合估算点云中隐含的具体几何模型的系数，实现对点云中所处的几何模型的分割，线，平面，柱面 ，和球面都可以在PCL 库中实现，平面模型经常被用到常见的室内平面的分割提取中， 比如墙，地板，桌面，其他模型常应用到根据几何结构检测识别和分割物体中，一共可以分为两类：一类是针对采样一致性及其泛化函数的实现，一类是几个不同模型的具体实现，例如：平面，直线，圆球等。