PCL学习笔记（二十）-- 随机采样一致性模型

一、简介

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

        RANSAC随机采样一致性算法简介：

        RANSAC是一种随机参数估计算法。RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点（inliers），简称为局内点或内点，否则为模型外样本点（outliers），简称为局外点或外点，记录下当前的inliers的个数，然后重复这一过程。每一次重复，都记录当前最佳的模型参数，所谓最佳，即inliers的个数最多，次数对应的inliers个数为best_ninliers。每次迭代的末尾，都会根据期望的误差率、best_ninliers、总样本个数，当前迭代次数，计算一个迭代结束评判因子，据此决定是否迭代结束。迭代结束后，最佳模型参数就是最终的模型参数估计值。

        RANSAC理论上可以剔除outliers的影响，并得到全局最优的参数估计。但是RANSAC有两个问题，首先在每次迭代中都要区分inliers和outliers，因此需要事先设定阈值，当模型具有明显的物理意义时，这个阈值还比较容易设定，但是若模型比较抽象时，这个阈值就不那么容易设定了，而且固定阈值不适用于样本动态变化的应用；第二个问题是，RANSAC的迭代次数是运行期决定的，不能阈值迭代的确切次数（当然迭代次数的范围是可以预测的）。除此之外，RANSAC只能从一个特定数据集中估计一个模型，当两个（或者更多个）模型存在时，RANSAC不能找到别的模型。

        LMedS最小中值方差估计算法：

        LMedS也是一种随机参数估计算法。LMedS也从样本中随机抽选出一个样本子集，使用最小方差估计算法对子集计算模型参数，然后计算所有样本于该模型的偏差。但是与RANSAC不同的是，LMedS记录的是所有样本中，偏差值居中的那个样本的偏差--称之为Med偏差（这也是LMedS中Med的由来），以及本次计算得到的模型参数。由于这一变化，LMedS不需要预先设定阈值来区分inliers和outliers。重复前面的过程N次，从N个Med偏差中挑选出最小的一个，其对应的模型参数就是最终的模型参数估计值。其中迭代次数N是由样本子集中的样本的个数、期望的模型误差、事先估计的样本中outliers的比例所决定的。

        LMedS理论上也可以剔除outliers的影响，并得到全局最优的参数估计，而且克服了RANSAC的两个缺点（虽然LMedS也需要实现设定样本中outliers的比例，但这个数字比较容易设定）。但是当outliers在样本中所占比例达到或超过50%时，LMedS就无能为力了！这与LMedS每次迭代记录的是Med偏差值有关。

二、代码分析

    1）首先对两个点云进行定义初始化，并对其中一个点云填充点云数据，作为处理前的原始点云，其中，大部分点云数据是基于设定的圆球和平面模型计算而得到的坐标值，作为局内点；有五分之一 的点云数据是被随机放置的，作为局外点。

	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);
  //生成数据，由于测试的Linux和Windows下rand函数的不同，根据自己的系统调整下面的随机数
  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)
    {//根据命令行参数，用球心为0，球半径为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 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);
    }

    2）接下来，创建一个用于存储点云中局内点的位置的整数型向量，作为点的索引的向量，这里我们使用一个平面或者球面随机估计模型建立随机采样的一致性对象，并根据命令行参数分别以点云作为输入，来进行随机参数估计，存储局内点：

  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);//与球面距离小于0.01的点作为局内点考虑
    ransac.computeModel();//执行随机参数估计
    ransac.getInliers(inliers);//存储估计所得的局内点
  }

    3）最后，复制合适模型的局内点到final点云中，然后根据命令行参数在三维窗口中显示原始点云或者估计得到的局内点组成的点云：

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

    4）整体代码

#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->initCameraParameters();
	return (viewer);
}

int main(int argc, char** argv)
{
	srand(time(NULL));
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);														//智能指针初始化点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 5000;
	cloud->height = 1;
	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
		{
			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;																											//初始化动态数组保存内点
	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);																								//与球面距离小于0.01的点作为局内点
		ransac.computeModel();																											//执行随机参数估计
		ransac.getInliers(inliers);																										//存储局内点
	}
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);																		//将模型估算出的局内点复制到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);
}

三、编译结果

    1）无参数时原始点云平面内点与立方体外点

    2）参数为-f 时提取到的平面上的内点

    3）参数为-s 时圆球内点与立方体外点

    4）参数为-sf时提取圆球上的点为内点

    2\4）的报错让人感觉有些迷茫