PCL点云处理-RANSAC

RANSAC算法简介

RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点 ( inliers），或称内部点、局内点或内点，否则为模型外样本点（outliers），或称外部点、局外点或外点，记录下当前的 inliers 的个数，然后重复这一过程。每一次重复都记录当前最佳的模型参数，所谓最佳即是inliers的个数最多 ，此时对应的inliers个数为 best_ninliers 。 每次迭代的末尾都会根据期望的误差率、 best_ninliers、总样本个数、当前迭代次数，计算一 个迭代结束评判因子，据此决定是否迭代结束。迭代结束后，最佳模型参数就是最终的模型参数估计值 。
RANSAC理论上可以剔除outliers的影响，并得到全局最优的参数估计。但是RANSAC 有两个问题，首先在每次迭代中都要区分 inliers 和 outlieres，因此需要事先设定阈值，当模型具有明显的物理意义时，这个阈值还比较容易设定，但是若模型比较抽象时，阈值就不那么容易设定了。而且固定阈值不适用于样本动态变化的应用；第二个问题是，RANSAC的迭代次数是运行期决定的，不能预知迭代的确切次数（当然迭代次数的范围是可以预测的）。除此之外， RANSAC 只能从一个特定数据集中估计一个模型，当两个（或者更多个）模型存在时，RANSAC 同时可能找到多个模型。

实现代码

#include <iostream>
#include <thread>

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


using namespace std::chrono_literals;

pcl::visualization::PCLVisualizer::Ptr
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
          pcl::PointCloud<pcl::PointXYZ>::ConstPtr final=nullptr){
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    pcl::visualization::PCLVisualizer::Ptr 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,2,"sample cloud");

    if(final!=nullptr){
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler(final, 255, 0, 0);
        viewer->addPointCloud<pcl::PointXYZ>(final,color_handler,"final cloud");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "final cloud");
    }
    viewer->addCoordinateSystem(1.0,"global");
    viewer->initCameraParameters();
    return (viewer);
}
/**
 * 使用方法：
 *
 * random_sample_consensus     创建包含外部点的平面
 * random_sample_consensus -f  创建包含外部点的平面，并计算平面内部点
 *
 * random_sample_consensus -s  创建包含外部点的球体
 * random_sample_consensus -sf 创建包含外部点的球体，并计算球体内部点
 */
int main(int argc,char **argv){
    //initialize PointClouds
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

    //populate our PointCloud with points
    // 生产我们的点云
    cloud->width=500;
    cloud->height=1;
    cloud->is_dense=false;
    cloud->points.resize(cloud->width*cloud->height);
    for (std::size_t i=0;i<cloud->points.size();++i){
    //带 -s的话就会产生圆球类型的目标点
    if(pcl::console::find_argument(argc,argv,"-s")>=0||pcl::console::find_argument(argc, argv, "-sf") >= 0){
       	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 {
        //没有 -s标识符的话，指挥产生平面类型的点云
        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;
    //created RandowSampleConsesus object and compute the appropriated model
    //圆形
    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(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(0.01);
        ransac.computeModel();
        ransac.getInliers(inliers);
    }
    // copies all inliers of the model computed to another PointCloudqq
    // 讲cloud中指定索引的点拷贝到final点云中
    pcl::copyPointCloud(*cloud,inliers,*final);

    // creates the visualization object and adds either our original cloud or all of the inliers
    // depending on the command line arguments specified.
    //根据命令对象，看看是否把我们创建的对象加入到可视化界面中
    pcl::visualization::PCLVisualizer::Ptr viewer;
    if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
        viewer=simpleVis(cloud,final);
    else 
        viewer=simpleVis(cloud);
    while(!viewer->wasStopped()){
        viewer->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }
    return 0;

}