【点云处理技术之PCL】随机采样一致算法（Random sample consensus，RANSAC）

1. RANSAC介绍

应用范围：曲线拟合、地面拟合、图像拼接。

RANSAC是“RANdom SAmple Consensus（随机抽样一致）”的缩写，这个算法于1981年由Fischler and Bolles首次提出。它是一种迭代方法，用于从一组包含局内点Inliers和局外点outliers的数据中，通过迭代的方式估计出数学模型的参数。Inliers可以解释为适合模型参数的一组点，outliers则是不适合模型的点。它是一种不确定的算法——它有一定的概率得出一个合理的结果，为了提高概率必须提高迭代次数。

RANSAC的输入为观测数据、一个参数模型和一些置信度参数，需要说明的是该参数化模型能够解释或者适用于局内点。

RANSAC的实现方式是迭代选择原始数据的随机子集。这些子集假设是局内点，假设检验步骤如下：

1. 首先选取一部分点作为初始数据，也就是局内点，根据这些点拟合出一个模型，
2. 根据拟合的模型对所有其他数据进行测试，如果某个点与估计模型吻合良好，也可将其视为假设的局内点。
3. 如果有足够多的点被加入到了局内点，则认为该模型足够合理。
4. 加入局内点之后，重新拟合模型。
5. 估计局内点与模型之间的误差来评估模型。

以上步骤只是一次迭代，根据设定的迭代次数，会重复进行以上操作。经过迭代之后有以下两种情况。

1. 要么因为局内点太少，还不如上一次的模型，而被舍弃
2. 要么因为比现有的模型更好而被选用。

这个算法有以下几个输入和输出：

输入：

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

输出：

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

2. RANSAC与最小二乘法

• 普通最小二乘是保守派：在现有数据下，如何实现最优。是从一个整体误差最小的角度去考虑，尽量谁也不得罪。

• RANSAC是改革派：首先假设数据具有某种特性（目的），为了达到目的，适当割舍一些现有的数据。

给出最小二乘拟合（红线）、RANSAC（绿线）对于一阶直线、二阶曲线的拟合对比：

3. RANSAC思路流程

• 第一步：假定模型（如直线方程），并随机抽取Nums个（以2个为例）样本点，对模型进行拟合：

• 第二步：由于不是严格线性，数据点都有一定波动，假设容差范围为：sigma，找出距离拟合曲线容差范围内的点，并统计点的个数：

• 第三步：重新随机选取Nums个点，重复第一步~第二步的操作，直到结束迭代：

• 第四步：每一次拟合后，容差范围内都有对应的数据点数，找出数据点个数最多的情况，就是最终的拟合结果：

需要说明给定几点：

• 每一次随机样本数Nums的选取：如二次曲线最少需要3个点确定，一般来说，Nums少一些易得出较优结果；
• 抽样迭代次数Iter的选取：即重复多少次抽取，就认为是符合要求从而停止运算？太多计算量大，太少性能可能不够理想；
• 容差Sigma的选取：sigma取大取小，对最终结果影响较大；

RANSAC的作用有点类似：将数据一切两段，一部分是自己人，一部分是敌人，自己人留下商量事，敌人赶出去。RANSAC开的是家庭会议，不像最小二乘总是开全体会议。

4. 代码举例

下面设置 两种类型，一种拟合球面(参数-sf)，一种是拟合平面(参数-f)。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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

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: 局内点1/5，其余在圆球内
        //f:平面，sf则为球面，所以else在此作用不大。
        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); //局外点：1/5
            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,一般局外点--作用不大，只是为了不报错，因为原始点云也不是下面的方式产生的
        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)); //针对平面模型的对象
    //f:平面，sf则为球面
    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;
}

拟合平面的命令为：./random_sample_consensus -f，具体效果如下：

拟合球面的命令为：./random_sample_consensus -sf，具体效果如下：

---

参考：
https://pcl.readthedocs.io/projects/tutorials/en/latest/random_sample_consensus.html#random-sample-consensus