PCL：RANSAC球面拟合

Posted 2022-01-06 没事就要敲代码

文章目录

• • 1 球面方程
  • 2 算法原理
  • 3 代码实现
  • 4 结果展示

1 球面方程

球面方程 是指空间中所有处于同一球面的点所对应的方程，已知球心 $p_0(x_0,y_0,z_0)$ 和 半径 $r$，则球面方程的一般式为 $$(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2=r^2$$

2 算法原理

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

RANSAC球面拟合的过程如下：

（1）在初始点云中随机选择4个点，计算其对应球面方程 $(x-x_0{)}^2+(y-y_0{)}^2+(z-z_0{)}^2=r^2$.

（2）计算所有点至该球面的距离 $d_i$ .选取阈值 $d_{threshold}$，若 $di\le d_{threshold}$ ，则该点被认为是模型内样本点（$inliers$，内点），否则为模型外样本点（$outliers$，外点），记录当前内点的个数。

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

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

3 代码实现

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

SACMODEL_SPHERE 模型：定义为球面模型，共设置4个参数[center.x,center.y,center.z,radius]，前三个参数为球心，第四个参数为半径。

下面，对含有噪声的球面点云进行RANSAC球面拟合，该球面球心为$(0,0,0)$，半径为 $1$，球面方程为
$$x^2+y^2+z^2=1$$
实现代码：

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>	//根据索引提取内点--方法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("test.pcd", *cloud) < 0)
	
		PCL_ERROR("\\a->点云文件不存在！\\n");
		system("pause");
		return -1;
	
	cout << "->加载点云点数：" << cloud->points.size() << endl;
	//========================== 加载点云 ==========================

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

	PointCloudT::Ptr cloud_sphere(new PointCloudT);

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

	///方法2,需要pcl/filters/extract_indices.h头文件，较为繁琐。
	//pcl::PointIndices pi;
	//ransac.getInliers(pi.indices);
	//pcl::IndicesPtr index_ptr(new vector<int>(pi.indices));/// 将自定义的点云索引数组pi进行智能指针的转换
	//pcl::ExtractIndices<PointT> 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"
		<< "(x - " << coefficient[0]
		<< ") ^ 2 + (y - " << coefficient[1]
		<< ") ^ 2 + (z - " << coefficient[2]
		<< ")^2 = " << coefficient[3]
		<< " ^2"
		<< 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_sphere, "sphere");												//添加平面点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "sphere");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sphere");	//点的大小

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

	return 0;

输出结果：

->正在加载点云...
->加载点云点数：4426
->正在估计球面...
->球面方程为：
(x - -1.96535e-06) ^ 2 + (y - 0) ^ 2 + (z - 3.14691e-07)^2 = 1 ^2

4 结果展示