PCL:投影滤波(二)将点云投影至球面

文章目录

1 PCL投影滤波器实现点云向球面投影

PCL官方文档上指出,可以使用投影滤波器实现点云向平面、球面、圆锥面的投影。
PCL:投影滤波(二)将点云投影至球面
在这之前,我们已经介绍了如何使用PCL中提供的投影滤波器实现点云向参数平面的投影。下面,参考平面模型投影的方式,实现点云向球面模型的投影。

首先,明确球面方程
( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 = r 2 (x-x_0)^2+(y-y_0)^2+(z-z_0)^2=r^2 (x−x0​)2+(y−y0​)2+(z−z0​)2=r2
式中, ( x 0 , y 0 , z 0 ) (x_0,y_0,z_0) (x0​,y0​,z0​) 为球心, r r r 为球半径。

PCL 提供了SACMODEL_SPHERE模型:定义为三维球体模型,共设置4个参数[center.x,center.y,center.z,radius],前三个参数为球心,第四个参数为半径。

下面将点云投影至球心为 ( 0 , 0 , − 1 ) (0,0,-1) (0,0,−1),半径为 2 2 2 的球面,即
x 2 + y 2 + ( z + 1 ) 2 = 4 x^2+y^2+(z+1)^2=4 x2+y2+(z+1)2=4

代码实现:

#include <pcl/io/pcd_io.h>
#include <pcl/ModelCoefficients.h>			//模型系数定义头文件
#include <pcl/filters/project_inliers.h>	//投影滤波类头文件

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("1.pcd", *cloud) < 0)
	{
		PCL_ERROR("\a->点云文件不存在!\n");
		return -1;
	}
	cout << "->加载点云的点数:" << cloud->points.size() << endl;
	//================================== 加载点云 ==================================

	//--------- 创建球面模型 (x - x0)^2 + (y - y0)^2 + (z - z0)^2 = r^2 -------------
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	coefficients->values.resize(4);
	coefficients->values[0] = 0;
	coefficients->values[1] = 0;
	coefficients->values[2] = -1.0;
	coefficients->values[3] = 2.0;
	//========= 创建球面模型 (x - x0)^2 + (y - y0)^2 + (z - z0)^2 = r^2 ============

	//-------------------------------- 执行投影滤波 --------------------------------
	PointCloudT::Ptr cloud_projected(new PointCloudT);
	pcl::ProjectInliers<pcl::PointXYZ> proj;
	proj.setModelType(pcl::SACMODEL_SPHERE);		//球面模型
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(coefficients);
	proj.filter(*cloud_projected);
	//================================ 执行投影滤波 ================================

	//保存滤波点云
	if (!cloud_projected->empty())
	{
		pcl::io::savePCDFileBinary("sphere_project.pcd", *cloud_projected);
	}
	else
	{
		PCL_ERROR("\a->投影滤波点云为空!\n");
	}

	return 0;
}

运行结果:

Unfortunately,PCL貌似没有实现点云向球面模型的投影,结果如下图。
PCL:投影滤波(二)将点云投影至球面
既然PCL靠不住,我们只好自己实现了。

2 点云投影至指定球面

2.1 投影原理

(1)指定投影球面参数,即球心 p 0 ( x 0 , y 0 , z 0 ) p_0(x_0,y_0,z_0) p0​(x0​,y0​,z0​) 和半径 r r r;
(2)计算点云中每一点与坐标原点 ( 0 , 0 , 0 ) (0,0,0) (0,0,0)的距离 d i d_i di​;
(3)点 p i ( x i , y i , z i ) p_i(x_i,y_i,z_i) pi​(xi​,yi​,zi​) 在球面的投影点 p s ( x s , y s , z s ) p_s(x_s,y_s,z_s) ps​(xs​,ys​,zs​) 为
{ x s = x i ∗ r d i + x 0 y s = y i ∗ r d i + y 0 z s = z i ∗ r d i + z 0 \begin{cases} x_s = \cfrac{x_i*r}{d_i}+x_0\\ y_s = \cfrac{y_i*r}{d_i}+y_0\\ z_s = \cfrac{z_i*r}{d_i}+z_0\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧​xs​=di​xi​∗r​+x0​ys​=di​yi​∗r​+y0​zs​=di​zi​∗r​+z0​​

2.2 代码实现

代码:

#include <pcl/io/pcd_io.h>

using namespace std;

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

//定义球的结构体
struct Sphere
{
	float center_x;
	float center_y;
	float center_z;
	float radius;
};

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

	//---------------------------------- 指定球面 ----------------------------------
	Sphere s;		//x^2+y^2+(z+1)^2=4
	s.center_x = 0;
	s.center_y = 0;
	s.center_z = -1;
	s.radius = 2;
	//================================== 指定球面 ==================================

	//---------------------------------- 执行投影 ----------------------------------
	PointCloudT::Ptr cloud_proj(new PointCloudT);
	size_t pt_num = cloud->points.size();
	for (size_t i = 0; i < pt_num; i++)
	{
		float d;
		d = sqrt(pow(cloud->points[i].x, 2) + pow(cloud->points[i].y, 2) + pow(cloud->points[i].z, 2));

		PointT proj_p;
		proj_p.x = cloud->points[i].x * s.radius / d + s.center_x;
		proj_p.y = cloud->points[i].y * s.radius / d + s.center_y;
		proj_p.z = cloud->points[i].z * s.radius / d + s.center_z;
		cloud_proj->points.push_back(proj_p);
	}
	//================================== 执行投影 ==================================

	//-------------------------------- 保存投影点云 --------------------------------
	pcl::io::savePCDFileBinary("sphere_proj.pcd", *cloud_proj);
	//================================ 保存投影点云 ================================

	return 0;
}

结果展示:
PCL:投影滤波(二)将点云投影至球面

2.3 结果验证

将投影点云进行RANSAC球面拟合,计算其球心和半径,看是否与投影球面参数一致。

代码:

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>

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("sphere_proj.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);
	vector<int> inliers;				//存储内点索引的向量
	ransac.getInliers(inliers);			//提取内点对应的索引
	pcl::copyPointCloud<PointT>(*cloud, inliers, *cloud_sphere);
	//========== 根据索引提取内点 ==========

	/// 输出模型参数
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);
	cout << "->球面方程为:\n"
		<< "(x - " << coefficient[0]
		<< ") ^ 2 + (y - " << coefficient[1]
		<< ") ^ 2 + (z - " << coefficient[2]
		<< ")^2 = " << coefficient[3]
		<< " ^2"
		<< endl;
	//========================== 模型估计 ==========================
	
return 0;

输出结果:

->正在加载点云...
->加载点云点数:41049
->正在估计球面...
->球面方程为:
(x - -4.18503e-07) ^ 2 + (y - -0) ^ 2 + (z - -1)^2 = 2 ^2

显然,投影点云拟合得出的球面参数,与我们给定的投影球面一致,验证了方法的正确性。

上一篇:win10下使用VS2019编译PROJ6


下一篇:Java报表工具FineReport导出EXCEL的四种API