PCL：RANSAC 平面拟合

1 平面方程

平面方程 是指空间中所有处于同一平面的点所对应的方程，其一般式形如 $$Ax+By+Cz+D=0$$已知平面法向量 $(A,B,C)$ 和平面上一点 $p_0(x_0,y_0,z_0)$，则平面的点法式方程为$$A(x-x_0)+B(y-y_0)+C(z-z_0)=0$$将点法式展开，其常数项 $-(A{x}_0+B{y}_0+C{z}_0)$即为一般式方程中的 $D$，即 $$D=-(A{x}_0+B{y}_0+C{z}_0)$$

2 算法原理

与最小二乘算法不同，随机采样一致性算法（RANSAC）不是用所有初始数据去获取一个初始解，然后剔除无效数据，而是使用满足可行条件的尽量少的初始数据，并使用一致性数据集去扩大它，这是一种寻找模型去拟合数据的思想，在计算机视觉领域有较广泛的应用。

RANSAC平面拟合的过程如下：

（1）在初始点云中随机选择三个点，计算其对应平面方程 $Ax+By+Zz+D=0$.

（2）计算所有点至该平面的代数距离 $d_i=|A{x}_i+B{y}_i+C{z}_i+D|$ .选取阈值 $d_{threshold}$，若 $di\le d_{threshold}$ ，则该点被认为是模型内样本点（$inliers$，内点），否则为模型外样本点（$outliers$，外点），记录当前内点的个数。

（3）重复以上步骤，选取最佳拟合参数，即内点数量最多的平面对应的模型参数；每次迭代末尾都会根据期望的误差率、最佳内点个数、总样本个数、当前迭代次数计算一个迭代结束评判因子，根据次决定是否停止迭代。

（4）迭代结束后，最佳模型参数就是最终的参数估计值。

3 代码实现

PCL中Sample——consensus模块提供了RANSAC平面拟合模块。

SACMODEL_PLANE 模型：定义为平面模型，共设置四个参数 [normal_x，normal_y，normal_z，d]。其中，（normal_x，normal_y，normal_z）为平面法向量 $(A,B,C)$，$d$ 为常数项 $D$。

实现代码

#include 
#include 
#include 
#include 
#include 	//根据索引提取内点--方法2所需头文件

using namespace std;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main()
{
	//-------------------------- 加载点云 --------------------------
	cout << "->正在加载点云..." << endl;
	PointCloudT::Ptr cloud(new PointCloudT);
	if (pcl::io::loadPCDFile("desk.pcd", *cloud) < 0)
	{
		PCL_ERROR("\a->点云文件不存在！\n");
		system("pause");
		return -1;
	}
	cout << "->加载点云点数：" << cloud->points.size() << endl;
	//========================== 加载点云 ==========================

	//-------------------------- 模型估计 --------------------------
	cout << "->正在估计平面..." << endl;
	pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(new pcl::SampleConsensusModelPlane<PointT>(cloud));	//选择拟合点云与几何模型
	pcl::RandomSampleConsensus<PointT> ransac(model_plane);	//创建随机采样一致性对象
	ransac.setDistanceThreshold(0.01);	//设置距离阈值，与平面距离小于0.01的点作为内点
	ransac.computeModel();				//执行模型估计

	PointCloudT::Ptr cloud_plane(new PointCloudT);	

	//---------- 根据索引提取内点 ----------
	///方法1
	vector<int> inliers;				//存储内点索引的向量
	ransac.getInliers(inliers);			//提取内点对应的索引
	pcl::copyPointCloud<PointT>(*cloud, inliers, *cloud_plane);

	///方法2,需要pcl/filters/extract_indices.h头文件，较为繁琐。
	//pcl::PointIndices pi;
	//ransac.getInliers(pi.indices);
	//pcl::IndicesPtr index_ptr(new vector(pi.indices));/// 将自定义的点云索引数组pi进行智能指针的转换
	//pcl::ExtractIndices extract;
	//extract.setInputCloud(cloud);
	//extract.setIndices(index_ptr);
	//extract.setNegative(false);	/// 提取索引外的点云，若设置为true,则与copyPointCloud提取结果相同
	//extract.filter(*cloud_plane);
	//========== 根据索引提取内点 ==========

	/// 输出模型参数Ax+By+Cz+D=0
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);
	cout << "平面方程为：\n"
		<< coefficient[0] << "x + "
		<< coefficient[1] << "y + "
		<< coefficient[2] << "z + "
		<< coefficient[3] << " = 0"
		<< endl;
	//========================== 模型估计 ==========================

	//----------------------- 可视化拟合结果 -----------------------
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));

	viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");													//添加原始点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");	//点的大小

	viewer->addPointCloud<pcl::PointXYZ>(cloud_plane, "plane");												//添加平面点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "plane");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");	//点的大小

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	//======================= 可视化拟合结果 =======================

	return 0;
}

输出结果：

->正在加载点云...
->加载点云点数：41049
->正在估计平面...
平面方程为：
-0.00487886x + -0.876555y + -0.481278z + -1.1743 = 0

4 结果展示

5 注意

结论： 当点云中包含多个平面时，以上代码默认拟合包含点数最多的那个平面。

下面是将原始点云中最大的平面下采样之后，再进行平面拟合，得到的是另外一个平面，证实了上面的结论。