ANSAC算法的输入时一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可行的参数。
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
有一个模型适应于假定的局内点，即所有的未知参数都能从假设的局内点计算得出；
用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为他也是局内点；
如果有足够多的点呗归类为假设的局内点，那么估计的模型就足够合理；
然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过；
最后，通过估计局内点与模型的错误率来评估模型；
这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被抛弃，要么因为比现有的模型更好而被选用。
关于模型好坏算法实现上有两种方式：
规定一个点数，达到这个点数后，算这些点与模型间的误差，找误差最小的模型。 对应下面算法一
规定一个误差，找匹配模型并小于这个误差的所有点，匹配的点最多的模型，就是最好模型。 对应下面算法二
伪代码一：
1 输入： 2 data ---- 一组观测数据 3 model ---- 适应于数据的模型 4 n ---- 适用于模型的最少数据个数 5 k ---- 算法的迭代次数 6 t ---- 用于决定数据是否适应于模型的阈值 7 d ---- 判定模型是否适用于数据集的数据数目 8 9 输出： 10 best_model —— 跟数据最匹配的模型参数（如果没有找到，返回null） 11 best_consensus_set —— 估计出模型的数据点 12 best_error —— 跟数据相关的估计出的模型的错误 13 14 iterations = 0 15 best_model = null 16 best_consensus_set = null 17 best_error = 无穷大 18 while( iterations < k ) 19 maybe_inliers = 从数据集中随机选择n个点 20 maybe_model = 适合于maybe_inliers的模型参数 21 consensus_set = maybe_inliers 22 23 for (每个数据集中不属于maybe_inliers的点) 24 if （如果点适合于maybe_model，并且错误小于t） 25 将该点添加到consensus_set 26 27 if (consensus_set中的点数大于d) 28 已经找到了好的模型， 现在测试该模型到底有多好 29 better_model = 适用于consensus_set中所有点的模型参数 30 this_error = better_model 究竟如何适合这些点的度量 31 32 if （this_error < best_error） 33 发现比以前好的模型，保存该模型直到更好的模型出现 34 best_model = better_model 35 best_consensus_set = consensus_set 36 best_error = this_error 37 38 iterations ++ 39 函数返回best_model, best_consensus_set, best_error
RANSAC算法的可能变化包括以下几种：
如果发现一种足够好的模型（该模型有足够下的错误率）， 则跳出主循环，这样节约不必要的计算；设置一个错误率的阈值，小于这个值就跳出循环；
可以直接从maybe_model计算this_error，而不从consensus_set重新估计模型，这样可能会节约时间，但是可能会对噪音敏感。
伪代码二：
1 输入： 2 data ---- 一组观测数据 3 numForEstimate ----- 初始模型需要的点数 4 delta ------ 判定点符合模型的误差 5 probability ----- 表示迭代过程中从数据集内随机选取出的点均为局内点的概率 6 7 输出： 8 best_model —— 跟数据最匹配的模型参数（如果没有找到，返回null） 9 best_consensus_set —— 估计出模型的数据点 10 11 k = 1000 //设置初始值 12 13 iterations = 0 14 best_model = null 15 best_consensus_set = null 16 17 while( iterations < k ) 18 maybe_inliers = 从数据集中随机选择numForEstimate个点 19 maybe_model = 适合于maybe_inliers的模型参数，比如直线，取两个点，得直线方程 20 21 for (每个数据集中不属于maybe_inliers的点) 22 if （如果点适合于maybe_model，并且错误小于delta） 23 将该点添加到maybe_inliers 24 25 if(maybe_inliers的点数 > best_consensus_set 的点数） //找到更好的模型 26 best_model = maybe_model 27 best_consensus_set = maybe_inliers 28 根据公式k=log(1-p)/log(1-pow(w,n))重新计算k 29 iterations ++ 30 函数返回best_model, best_consensus_set,
#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 = 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)
{
//根据命令行参数用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); //与球面距离小于0.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;
}