PCL-基于RANSAC的平面拟合方法

一、简介

随机采样一致性（Random sample consensus，RANSAC）：RANSAC是一种鲁棒的模型拟合方法，它可以处理存在大量噪声和异常值的数据。在进行平面拟合时，RANSAC会随机选择三个点，然后计算这三个点确定的平面模型。然后，RANSAC会计算其他所有点到这个平面的距离，并根据一个预设的阈值来判断这些点是否符合这个平面模型。这个过程会重复多次，最后选择符合点最多的平面模型作为最终的结果。
随机采样一致性（RANSAC）是一种迭代的模型估计方法，它的主要目标是从一组包含大量异常值的观测数据中估计出数学模型的参数。在进行平面拟合时，RANSAC的工作原理如下：
1.随机选择最小样本集：在进行平面拟合时，RANSAC首先会随机选择三个点作为最小样本集。这是因为在三维空间中，三个非共线的点可以确定一个平面。
2.构建模型：然后，RANSAC会根据这三个点计算出一个平面模型。这个模型就是通过这三个点的平面。
3.计算误差：接着，RANSAC会计算其他所有点到这个平面的距离，这个距离就是每个点的误差。
4.确定内点：然后，RANSAC会根据一个预设的阈值来判断每个点是否为内点，也就是说，如果一个点的误差小于这个阈值，那么就认为这个点符合平面模型，将其标记为内点。
5.更新模型：如果内点的数量超过了之前的最大内点数量，那么就用所有的内点来更新平面模型。
6.迭代：以上的过程会重复多次。在每次迭代中，RANSAC都会随机选择一个新的最小样本集，然后构建模型，计算误差，确定内点，更新模型。这个过程会一直进行，直到达到预设的迭代次数。
7.计算最佳模型：最后，RANSAC会选择内点数量最多的样本，再用这些内点进行求解最终的方程，作为最终的结果。通过这种方式，RANSAC可以有效地处理存在大量噪声和异常值的数据，从而得到鲁棒的模型估计结果。

二、实验代码

#pragma warning(disable:4996)
#include <iostream>
#include <pcl/io/pcd_io.h>
#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 <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace std;

int main(int argc, char** argv)
{
	// ------------------------------------------读取点云-----------------------------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("E:****\\0_point_cloud_00000.pcd", *cloud) == -1)
	{
		PCL_ERROR("点云读取失败 \n");
		return (-1);
	}
	//------------------------------------------RANSAC框架---------------------------------------------- 
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);// 定义RANSAC算法模型
	ransac.setDistanceThreshold(0.02);// 设定距离阈值
	ransac.setMaxIterations(500);     // 设置最大迭代次数
	ransac.setProbability(0.99);      // 设置从离群值中选择至少一个样本的期望概率
	ransac.computeModel();            // 拟合平面
	vector<int> inliers;              // 用于存放内点索引的vector
	ransac.getInliers(inliers);       // 获取内点索引
	Eigen::VectorXf coeff;
	ransac.getModelCoefficients(coeff);  //获取拟合平面参数，coeff分别按顺序保存a,b,c,d
	cout << "平面模型系数coeff(a,b,c,d): " << coeff[0] << " \t" << coeff[1] << "\t " << coeff[2] << "\t " << coeff[3] << endl;
	
	// --------------------------------根据内点索引提取拟合的平面点云-----------------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr sac_plane(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *sac_plane);
	pcl::io::savePCDFileBinary("E:\\****\\0_point_cloud_00000拟合.pcd", *sac_plane);
	
	// -------------------------------------------可视化-------------------------------------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud show"));
	int v1 = 0;
	int v2 = 1;

	viewer->createViewPort(0, 0, 0.5, 1, v1);
	viewer->createViewPort(0.5, 0.5, 1, 1, v2);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->setBackgroundColor(0, 0, 0, v2);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cloud, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> after_sac(sac_plane, 0, 0, 255);

	viewer->addPointCloud(cloud, color, "cloud", v1);
	viewer->addPointCloud(sac_plane, after_sac, "plane cloud", v2);
	
	

	// 显示拟合出来的平面
	pcl::ModelCoefficients plane;
	plane.values.push_back(coeff[0]);
	plane.values.push_back(coeff[1]);
	plane.values.push_back(coeff[2]);
	plane.values.push_back(coeff[3]);

	viewer->addPlane(plane, "plane",v2);
	

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

三、实验结果

拟合前：

拟合后：

平面方程：