PCL学习3：平面拟合

PCL平面拟合功能位于模块sample_consensus中：PCL sample_consensus

该模块基于“随机抽样一致算法”（RANdom SAmple Consensus），不仅可以用于平面拟合，也可以拟合柱面、球面等，对RANSAC的简单解释：

RANSAC可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。该算法最早由Fischler和Bolles于1981年提出。图像检索中，RANSAC可以作为检索后的后续处理，对图像的中目标进行空间一致验证。

具体原理：PCL 采样一致性算法

下面直接给出代码：

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <Eigen/Core>


pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>());//待拟合点云指针
//省略ground的io操作，向ground读入数据
vector<int> inliers;//用于存放合群点的vector
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model(
new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(ground));//定义待拟合平面的model，并使用待拟合点云初始化
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model);//定义RANSAC算法模型
ransac.setDistanceThreshold(0.05);//设定阈值
ransac.computeModel();//拟合
ransac.getInliers(inliers);//获取合群点
vector<int> tmp;
Eigen::VectorXf coeff;
ransac.getModelCoefficients(coeff);//获取拟合平面参数，对于平面ax+by_cz_d=0，coeff分别按顺序保存a,b,c,d
cout<<"coeff "<<coeff[0]<<" "<<coeff[1]<<" "<<coeff[2]<<" "<<coeff[3]<<endl;
ransac.getModel(tmp);

pcl::copyPointCloud(*ground, inliers, *ground_inliers);
//    for(int i=0; i<inliers.size(); i++)
//    {
//        ground_inliers->points.push_back(ground->points[inliers[i]]);
//    }