sample consensus 采样一致性算法

PCL的几种采样方法：

1. 下采样 Downsampling
   一般下采样是通过构造一个三维体素栅格，然后在每个体素内用体素内的所有点的重心近似显示体素中的其他点，这样体素内所有点就用一个重心点来表示，进行下采样的来达到滤波的效果，这样就大大的减少了数据量，特别是在配准，曲面重建等工作之前作为预处理，可以很好的提高程序的运行速度

#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>

int
main(int argc, char** argv)
{
    // 创建点云对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 读取PCD文件
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
    {
        return -1;
    }

    // 创建滤波对象
    pcl::VoxelGrid<pcl::PointXYZ> filter;
    filter.setInputCloud(cloud);
    // 设置体素栅格的大小为 1x1x1cm
    filter.setLeafSize(0.01f, 0.01f, 0.01f);
    filter.filter(*filteredCloud);
}

2. 均匀采样：这个类基本上是相同的，但它输出的点云索引是选择的关键点在计算描述子的常见方式。

#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/uniform_sampling.h>

int
main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
    {
        return -1;
    }
    // Uniform sampling object.
    pcl::UniformSampling<pcl::PointXYZ> filter;
    filter.setInputCloud(cloud);
    filter.setRadiusSearch(0.01f);
    // We need an additional object to store the indices of surviving points.
    pcl::PointCloud<int> keypointIndices;

    filter.compute(keypointIndices);
    pcl::copyPointCloud(*cloud, keypointIndices.points, *filteredCloud);
}

3. 增采样 ：增采样是一种表面重建方法，当你有比你想象的要少的点云数据时，增采样可以帮你恢复原有的表面（S）

#include <pcl/io/pcd_io.h>
#include <pcl/surface/mls.h>

int main(int argc,char** argv)
{
// 新建点云存储对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 读取文件
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
    {
        return -1;
    }
    // 滤波对象
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> filter;
    filter.setInputCloud(cloud);
    //建立搜索对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree;
    filter.setSearchMethod(kdtree);
    //设置搜索邻域的半径为3cm
    filter.setSearchRadius(0.03);
    // Upsampling 采样的方法有 DISTINCT_CLOUD, RANDOM_UNIFORM_DENSITY
    filter.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ>::SAMPLE_LOCAL_PLANE);
    // 采样的半径是
    filter.setUpsamplingRadius(0.03);
    // 采样步数的大小
    filter.setUpsamplingStepSize(0.02);

    filter.process(*filteredCloud);
}

4. 表面重建：深度传感器的测量是不准确的，和由此产生的点云也是存在的测量误差，比如离群点，孔等表面，可以用一个算法重建表面，遍历所有的点云和插值数据，试图重建原来的表面。比如增采样，PCL使用MLS算法和类。执行这一步是很重要的，因为由此产生的点云的法线将更准确。

#include <pcl/io/pcd_io.h>
#include <pcl/surface/mls.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
int
main(int argc, char** argv)
{
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);    
    pcl::PointCloud<pcl::PointNormal>::Ptr smoothedCloud(new pcl::PointCloud<pcl::PointNormal>);

    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0)
    {
        return -1;
    }

    // Smoothing object (we choose what point types we want as input and output).
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> filter;
    filter.setInputCloud(cloud);
    // Use all neighbors in a radius of 3cm.
    filter.setSearchRadius(0.03);
    // If true, the surface and normal are approximated using a polynomial estimation
    // (if false, only a tangent one).
    filter.setPolynomialFit(true);
    // We can tell the algorithm to also compute smoothed normals (optional).
    filter.setComputeNormals(true);
    // kd-tree object for performing searches.
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree;
    filter.setSearchMethod(kdtree);

    filter.process(*smoothedCloud);


  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("smooth"));
viewer->addPointCloud<pcl::PointNormal>(smoothedCloud,"smoothed");

while(!viewer->wasStopped())
  {
 viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(1000000));
 }
}

基础理论

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

PCL 中以随机采样一致性算法( RANSAC) 为核心，实现了五种类似于RANSAC的随机参数估计算法，例如随机采样一致性估计（RANSAC ) 、最大似然一致性估计 （MLESAC ) 、最小中值方差一致性估计 ( LMEDS )等，所有的估计参数算法都符合一致性准则。
● 利用RANSAC可以实现点云分割，目前 PCL 中支持的几何模型分割有 空间平面、直线、二维或三维圆、圆球、锥体等 。 RANSAC的另一应用就是点云的配准对的剔除。
在PCL中设计的采样一致性算法的应用主要就是对点云进行分割，根据设定的不同的几个模型，估计对应的几何参数模型的参数，在一定容许的范围内分割出在模型上的点云。

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

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

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

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

包含很多局外点的数据集，

RANSAC找到的直线（局外点并不影响结果）

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

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

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

2 最小中值法（LMedS）

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

支持以下模型：

● SACMODEL_PLANE-用于确定平面模型。平面的四个系数是其Hessian范式：[ normal_x normal_y normal_z d ]
● SACMODEL_LINE-用于确定线模型。线的六个系数由线上的一个点和线的方向给出：[ point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z ]
● SACMODEL_CIRCLE2D-用于确定平面中的2D圆。圆的三个系数由其中心和半径给出：[ center.x center.y radius ]
● SACMODEL_CIRCLE3D-用于确定平面中的3D圆。圆的七个系数由其中心，半径和法线给出：[ center.x，center.y，center.z，半径，normal.x，normal.y，normal.z ]
● SACMODEL_SPHERE-用于确定球体模型。球面的四个系数由其3D中心和半径给出：[ center.x center.y center.z radius ]
● SACMODEL_CYLINDER-用于确定圆柱模型。圆柱体的七个系数由其轴线上的一个点，轴线方向和半径给出，例如：[ point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius ]
● SACMODEL_CONE-用于确定圆锥模型。圆锥的七个系数由其顶点，轴方向和张开角给出，例如：[ apex.x，apex.y，apex.z，axis_direction.x，axis_direction.y，axis_direction.z， opening_angle ]
● SACMODEL_TORUS-尚未实现
● SACMODEL_PARALLEL_LINE-用于在最大指定角度偏差内确定与给定轴平行的线的模型。线系数类似于SACMODEL_LINE。
● SACMODEL_PERPENDICULAR_PLANE-用于在最大指定角度偏差内确定垂直于用户指定轴的平面的模型。平面系数类似于SACMODEL_PLANE。
● SACMODEL_PARALLEL_LINES-尚未实施
● SACMODEL_NORMAL_PLANE-用于使用附加约束确定平面模型的模型：每个内点的表面法线必须在最大指定角度偏差内平行于输出平面的表面法线。平面系数类似于SACMODEL_PLANE。
● SACMODEL_NORMAL_SPHERE -类似SACMODEL_SPHERE，但是具有附加的表面法线的约束。
● SACMODEL_PARALLEL_PLANE-用于在最大指定角度偏差内确定与用户指定的轴平行的平面的模型。平面系数类似于SACMODEL_PLANE。
● SACMODEL_NORMAL_PARALLEL_PLANE使用附加的曲面法向约束定义3D平面分割的模型。平面法线必须平行于用户指定的轴。因此，SACMODEL_NORMAL_PARALLEL_PLANE等效于SACMODEL_NORMAL_PLANE + SACMODEL_PERPENDICULAR_PLANE。平面系数类似于SACMODEL_PLANE。
● SACMODEL_STICK -3D杆分割模型。摇杆是用户指定最小/最大宽度的线。

以下列表描述了已实现的鲁棒样本共识估计量：
● SAC_RANSAC - RANdom SAmple Consensus
● SAC_LMEDS - Least Median of Squares
● SAC_MSAC - M-Estimator SAmple Consensus
● SAC_RRANSAC - Randomized RANSAC
● SAC_RMSAC - Randomized MSAC
● SAC_MLESAC - Maximum LikeLihood Estimation SAmple Consensus
● SAC_PROSAC - PROgressive SAmple Consensus
默认情况下，如果您不熟悉上述大多数估计量及其操作方式，请使用RANSAC检验您的假设

随机采样一致性算法

/*
 * @Description: 1随机采样一致性算法           https://www.cnblogs.com/li-yao7758258/p/6477007.html
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-19 15:04:50
 * @LastEditTime: 2020-10-19 18:34:17
 * @FilePath: /pcl-learning/05sampleconsensus抽样一致性模块/1随机采样一致性算法/random_sample_consensus.cpp
 */

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