PCL：投影滤波将点云投影至球面
Posted 没事就要敲代码
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了PCL：投影滤波将点云投影至球面相关的知识，希望对你有一定的参考价值。
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
\\begincases x_s = \\cfracx_i*rd_i+x_0\\\\ y_s = \\cfracy_i*rd_i+y_0\\\\ z_s = \\cfracz_i*rd_i+z_0\\\\ \\endcases
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xs=dixi∗r+x0ys=diyi∗r+y0zs=dizi∗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点云处理技术之PCL滤波器——参数化模型(投影点云，pcl::ProjectInliers)