PCL点云随机采样一致性分割算法

最近在学习点云的分割算法，目前了解到有两种分割算法，一种是聚类分割算法，一种是随机采样一致性算法，聚类算法上篇文章已经有所提及。今天分析下随机采样一致性算法，然后代码理解；

RANSAC随机采样一致性算法介绍

RANSAC是”RANdom SAmple Consensus（随机抽样一致）”的缩写，随机采样一致性算法是一种概率性的算法，有时为了提高有效数据的概率，需要增加迭代次数，

数据一般分为两种：有效数据（inliers）和无效数据（outliers），和目标数据相差不大的数据为有效数据，相差比较大的数据为无效数据。

如果有效数据占据大多数的话，无效数据只有一少部分，我们可以通过最下二乘法或者类似的方法进行确定模型的参数和误差，如果无效数据很多，最下二乘法就失效了，需要新的算法进行。

RANSAC算法的概述

RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点。通过下述的方法进行验证：

有一个模型适用于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
最后，通过估计局内点与模型的错误率来评估模型。
算法伪代码

输入：

data —— 一组观测数据
model —— 适应于数据的模型
n —— 适用于模型的最少数据个数
k —— 算法的迭代次数
t —— 用于决定数据是否适应于模型的阀值
d —— 判定模型是否适用于数据集的数据数目
输出：

best_model —— 跟数据最匹配的模型参数（如果没有找到好的模型，返回null）
best_consensus_set —— 估计出模型的数据点
best_error —— 跟数据相关的估计出的模型错误
开始：

iterations = 0
best_model = null
best_consensus_set = null
best_error = 无穷大
while ( iterations < k )
maybe_inliers = 从数据集中随机选择n个点
maybe_model = 适合于maybe_inliers的模型参数
consensus_set = maybe_inliers
for ( 每个数据集中不属于maybe_inliers的点）
if ( 如果点适合于maybe_model，且错误小于t ）
将点添加到consensus_set
if （ consensus_set中的元素数目大于d ）
已经找到了好的模型，现在测试该模型到底有多好
better_model = 适合于consensus_set中所有点的模型参数
this_error = better_model究竟如何适合这些点的度量
if ( this_error < best_error )
我们发现了比以前好的模型，保存该模型直到更好的模型出现
best_model = better_model
best_consensus_set = consensus_set
best_error = this_error
增加迭代次数
返回 best_model, best_consensus_set, best_error
最小中值法（LMedS）

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

pcl::SampleConsensus< T > 是采样一致性算法的基类

1. SampleConsensus (const SampleConsensusModelPtr &model, double threshold, bool random=false)
其中model设置随机采样性算法使用的模型，threshold 阀值
2.设置模型 void setSampleConsensusModel (const SampleConsensusModelPtr &model)
3.设置距离阈值 void setDistanceThreshold (double threshold)
4.获取距离阈值 double getDistanceThreshold ()
5.设置最大迭代次数 void setMaxIterations (int max_iterations)
6.获取最大迭代次数 int getMaxIterations ()
随机采样一致性算法平面模型代码实现：

//创建随机采样一致性对象
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));
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
//内点索引
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

// pcl::PointIndices::Ptr outliers (new pcl::PointIndices);//外点索引
// 创建一个点云分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 是否优化模型系数
seg.setOptimizeCoefficients (true);
// 设置模型　和　采样方法
seg.setModelType (pcl::SACMODEL_PLANE);//　平面模型
seg.setMethodType (pcl::SAC_RANSAC);// 随机采样一致性算法
seg.setDistanceThreshold (0.01);//是否在平面上的阈值

seg.setInputCloud (cloud_filtered);//输入点云
seg.segment (*inliers, *coefficients);//分割　得到平面系数　已经在平面上的点的　索引

if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return (-1) ;
}

//按照索引提取点云　　内点
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;//索引提取器
extract_indices.setIndices (boost::make_shared<const pcl::PointIndices> (*inliers));//设置索引
extract_indices.setInputCloud (cloud_filtered);//设置输入点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);
extract_indices.filter (*output);//提取对于索引的点云内点
std::cerr << "output point size : " << output->points.size () << std::endl;