PCL MLESAC极大似然采样一致性估计（拟合平面为例）（SampleConsensus_MLESAC）

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.名词解释

（1）似然：描述已经观察到的数据在不同参数值下发生的可能性。

（2）极大似然估计：就是利用观测数据的样本结果信息，去反推最具有可能导致这些观察数据出现的模型参数值。

2.原理

MLESAC使用似然函数评价误差。当我们抽样计算出一个模型后，会使用当前的抽样以及模型生成一个高斯混合模型。之后使用这个混合模型对所有样本求似然估计。最后选取似然估计值最小的模型作为随机抽样的最终模型。

（1）按照输入模型的类型，建立相应的概率模型。

（2）根据数据和模型假设，构建似然参数。似然函数是关于参数的函数，它描述了在给定模型下，观察到当前数据的概率。这个函数是关于参数的函数，因为它考虑了模型参数对数据概率的影响。

（3）寻找最大似然估计：在给定观测数据后，通过寻找使似然函数取得最大值的参数值，来估计模型的参数。最大似然估计的思想是选择能够最大程度解释观测数据的模型参数，使数据出现的可能性最大。

（4）参数估计：找到使似然函数最大化的参数值，这些参数值就是最大似然估计得到的模型参数。

3.完整代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/mlesac.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()
{
    /****************MLESAC拟合（以拟合平面为例）********************/
    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>);			// 拟合出平面的内点

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

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

    /****************展示********************/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("MLESAC"));
    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);
}

4.结果展示