标签：PCL 球面 投影 proj pcl 点云 cloud

文章目录

• 1 PCL投影滤波器实现点云向球面投影
• 2 点云投影至指定球面
• • 2.1 投影原理
  • 2.2 代码实现
  • 2.3 结果验证

1 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靠不住，我们只好自己实现了。

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;
}

结果展示：

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

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

标签：PCL,球面,投影,proj,pcl,点云,cloud
来源： https://blog.csdn.net/weixin_46098577/article/details/121977744

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。