PCL LMedS最小中值平方拟合（以拟合平面为例）（SampleConsensus_LMedS）

PCL专栏目录及须知

不适用于局外点大于50%的样本。

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.LMedS 算法

1. 随机选择一定数量的点。
2. 使用这些点来估计一个模型，估计的方法一般为最小二乘法
3. 将所有点带入这个模型，计算出偏差，并记录偏差的中位数。
4. 重复1-4步直至迭代次数上限。
5. 选取中位数偏差最小的模型作为结果

LMedS 算法解释

2.注意事项

不适用于局外点大于50%的样本。

3.完整代码

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

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

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

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

打印：

红色为拟合出的平面点