PCL随机采样一致性：RANSAC 平面拟合

Posted 2021-08-08 借我十斤肉

文章目录

• • 1 平面方程
  • 2 算法原理
  • 3 算法实现

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+cz+d=0$.

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

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

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

3 算法实现

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

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

实现代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;

int main()
{
	//---------------------------------------------
	//-------------------加载点云-------------------
	//---------------------------------------------
	cout << "->正在加载点云..." << endl;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile("plane.pcd", *cloud) < 0)
	{
		PCL_ERROR("警告！点云文件不存在！\\a\\n");
		system("pause");
		return -1;
	}

	//---------------------------------------------
	//-------------------模型估计-------------------
	//---------------------------------------------
	cout << "->正在估计平面..." << endl;
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));	//选择拟合点云与几何模型
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);	//创建随机采样一致性对象
	ransac.setDistanceThreshold(0.01);	//设置距离阈值，与平面距离小于0.01的点作为内点
	ransac.computeModel();				//执行模型估计
	vector<int> inliers;				//存储内点索引的向量
	ransac.getInliers(inliers);			//提取内点对应的索引
	
	/// 根据索引提取内点
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *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;
}

输出结果：

->正在加载点云...
->正在估计平面...
平面方程为：
-0.00847471x + -0.879568y + -0.475697z + -1.16966 = 0

拟合结果：