PCL--采样一致性算法

   计算机视觉领域可以应用不同的采样一一致性参数估计算算法排除错误的样本。样本不同，对应的应用也不同。

1. RANSAC随机采样一致性算法：

  RANSAC从样本中随机抽取一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本该模型参数的偏差，在使用一个设定好的阈值与偏差比较，偏差小于阈值，该样本属于内点，否则属于外点。

例如：个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。简单的最小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概率还足够高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。

RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。
  RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
   1. 有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
   2 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
   3 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
   4. 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
   5 .最后，通过估计局内点与模型的错误率来评估模型。

这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。

算法：

输入：
  data —— 一组观测数据
  model —— 适应于数据的模型
  n —— 适用于模型的最少数据个数
  k —— 算法的迭代次数
  t —— 用于决定数据是否适应于模型的阀值
  d —— 判定模型是否适用于数据集的数据数目
输出：
  best_model —— 跟数据最匹配的模型参数（如果没有找到好的模型，返回null）
  best_consensus_set —— 估计出模型的数据点
  best_error —— 跟数据相关的估计出的模型错误

参数：

我们不得不根据特定的问题和数据集通过实验来确定参数t和d。然而参数k（迭代次数）可以从理论结果推断。当我们从估计模型参数时，用p表示一些迭代过程中从数据集内随机选取出的点均为局内点的概率；此时，结果模型很可能有用，因此p也表征了算法产生有用结果的概率。用w表示每次从数据集中选取一个局内点的概率，如下式所示：

 w = 局内点的数目 / 数据集的数目

通常情况下，我们事先并不知道w的值，但是可以给出一些鲁棒的值。假设估计模型需要选定n个点，wn是所有n个点均为局内点的概率；1 − wn是n个点中至少有一个点为局外点的概率，此时表明我们从数据集中估计出了一个不好的模型。 (1 − wn)k表示算法永远都不会选择到n个点均为局内点的概率，它和1-p相同。因此，

1 − p = (1 − wn)k

我们对上式的两边取对数，得出

值得注意的是，这个结果假设n个点都是独立选择的；也就是说，某个点被选定之后，它可能会被后续的迭代过程重复选定到。这种方法通常都不合理，由此推导出的k值被看作是选取不重复点的上限。例如，要从上图中的数据集寻找适合的直线，RANSAC算法通常在每次迭代时选取2个点，计算通过这两点的直线maybe_model，要求这两点必须唯一。
为了得到更可信的参数，标准偏差或它的乘积可以被加到k上。k的标准偏差定义为：

2. LMedS最小中值方差估计算法

   LMedS是从样本中随机抽选出一个样本子集， 使用最小方差估计算法对子集计算模型参数，然后计算所有样本与该模型的偏差。与RANSAC不同的是，LMedSI记录的是所有样本中偏差值居中的那个样本的偏差，称为Med偏差，以及本次计算所得的模型参数。 LMedS不需要预先设定阈值来区分inliners和outliners。重复以上过程N次，从中N个Med偏差中挑选出最小的一个，其对应的模型参数就是最终模型参数的估计值。

3. PCL中的采样一致性算法

#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>

using namespace std;
using namespace pcl;


//点云显示
boost::shared_ptr<visualization::PCLVisualizer> simpleVis(PointCloud<PointXYZ>::ConstPtr cloud)
{
	//创建可视化窗口
	boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0.4,0.4,0.6);
	viewer->addPointCloud<PointXYZ>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample cloud");

	//添加坐标系
	//viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

	return viewer;
}



int main()
{
	//创建点云
	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>); //初始化点云数据
	PointCloud<PointXYZ>::Ptr final(new PointCloud<PointXYZ>); 

	//填充点云
	cloud->width = 5000;     //设置点云数目
	cloud->height = 1;       //设置为无序点云
	cloud->is_dense = false; //非稠密点云

	cloud->points.resize(cloud->width*cloud->height);   //resize设置容量和大小

	//填充点云数据
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		//使用x^2+y^2+z^2=1设置一部分点云数据，此时点云组成由1/4个球体作为内点
		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));
		}
	}


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

		创建随机采样一致性对象
		三维球体模型
		SampleConsensusModelSphere<PointXYZ>::Ptr model_s(new SampleConsensusModelSphere<PointXYZ>(cloud));

		//平面模型
		/*SampleConsensusModelPlane<PointXYZ>::Ptr model_p(new SampleConsensusModelPlane<PointXYZ>(cloud));*/


		//RANSAC (RAndom SAmple Consensus) 采样一致性算法
		RandomSampleConsensus<PointXYZ> ransac(model_s);
		ransac.setDistanceThreshold(0.01); //与球面距离小于1cm的点作为局内点考虑。
		ransac.computeModel();
		ransac.getInliers(inliers);


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

		//创建可视化对象并加入原始点云或者所有的局内点
		boost::shared_ptr<visualization::PCLVisualizer> viewer;
		viewer = simpleVis(final);
		while (!viewer->wasStopped())
		{
			viewer->spinOnce(100);
			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
		return 0;
}

附加：最小二乘法思想：

   假设用不同厂商生产的设备测量同一物体的长度，得到的结果分别为y1, y2, y3, y4, y5。这五个值可能存在细微的差别，那么到底什么值是物体真正的长度呢？

  可以这样考虑：

 假设物体的长度为y, 那么五次测量所得的误差分别为 |y-y1|， |y-y2|， |y-y3|， |y-y4|和 |y-y5|。由于取绝对值比较麻烦，可以用平方值来代表，也就是误差分别为：
           
那么总的误差的平方就是：
     
因此：可以认为，使误差最小的值即为测量的值。


算数平均数只是最小二乘法中的一个特例，使用范围比交狭窄。最小二乘法用途很广泛。