PCL RMSAC算法拟合（拟合平面为例）（SampleConsensus_RMSAC）

PCL专栏目录及须知

PCL拟合模型方法效果对比

拟合方法	用时	样本点数	拟合得出内点数	拟合百分比	平面方程参数
LMedS	6.59863s	34262	15880	46.3487%	A:-0.245971,B:0.328976,C:-0.911742,D:273.254
RMSAC	40.2117s	34262	2418	7.05738%	A:-0.14948,B:-0.570804,C:0.807365,D:-191.15
MSAC	0.732149s	34262	14551	42.4698%	A:-0.198437,B:0.323444,C:-0.925206,D:275.461
PROSAC	0.0931069s	34262	13627	39.7729%	A:0.256624,B:-0.288087,C:0.922578,D:-283.215
Ransac	0.093296s	34262	14870	43.4009%	A:-0.226923,B:0.322948,C:-0.918809,D:276.486
MLESAC	4.8623s	34262	13580	39.487%	A:-0.245971,B:0.328976,C:-0.911742,D:273.254

 本文以拟合平面为例子，如果使用其他模型：修改该处代码，按照下文其他例程使用即可。

其他模型拟合例程：

PCL 拟合二维圆（以RANSAC为例）

PCL 拟合三维圆（以RANSAC为例）

PCL 拟合直线（以RANSAC为例）

PCL 拟合球体（以RANSAC为例）

PCL 拟合圆柱（以RANSAC为例）

PCL 拟合圆锥（以RANSAC为例）

1.RMSAC介绍

一种LMeds算法的改进，用于提升计算速度。类似于RRANSAC，在算法的2,3歩中间插入了一个预判断 ，进行该判断时会从当前点集中随机抽取一定数量d的点，用当前模型去判断，如果为内点，则继续下面的步骤，否则直接放弃当前模型重新抽样。

2.代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/RMSAC.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>

int main()
{
    /****************RMSAC拟合（以拟合平面为例）********************/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);			// 源点云
    pcl::io::loadPCDFile("D:/code/csdn/data/samp11-TIN.pcd", *cloud);
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr modelPlane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud)); // 待拟合平面点云对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudInliers(new pcl::PointCloud<pcl::PointXYZ>);			// 拟合出平面的内点

    // RMSAC
    pcl::console::TicToc time; time.tic();
    pcl::RandomizedMEstimatorSampleConsensus<pcl::PointXYZ> RMSAC(modelPlane);                     // RMSAC算法模型
    RMSAC.setDistanceThreshold(3.0);                                                       // 距离阈值
    RMSAC.setMaxIterations(500);                                                           // 最大迭代次数
    RMSAC.computeModel();                                                                  // 拟合平面
    Eigen::VectorXf RMSACCof;
    RMSAC.getModelCoefficients(RMSACCof);                                                   // 获取模型参数
    std::vector<int> RMSACInliers;                                                         // 获取属于拟合出平面的内点
    RMSAC.getInliers(RMSACInliers);
    pcl::copyPointCloud(*cloud, RMSACInliers, *cloudInliers);

    std::cout << "RMSAC计算时间:" << time.toc() / 1000 << "s" << std::endl;
    std::cout << "RMSAC原始点数:" << cloud->size() << std::endl;
    std::cout << "RMSAC拟合点数:" << RMSACInliers.size() << std::endl;
    std::cout << "RMSAC拟合百分比:" << ((double)RMSACInliers.size() / (double)cloud->size()) * 100 << "%" << std::endl;
    std::cout << "RMSAC平面参数:" << "A:" << RMSACCof[0] << ",B:" << RMSACCof[1] << ",C:" << RMSACCof[2] << ",D:" << RMSACCof[3] << std::endl;

    /****************展示********************/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("RMSAC"));
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");
    viewer->addPointCloud<pcl::PointXYZ>(cloudInliers, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudInliers, 255, 0, 0), "cloudInliers");

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

    return (0);
}

3.结果展示

打印：

可视化结果：