文章目录
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=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[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
显然，投影点云拟合得出的球面参数，与我们给定的投影球面一致，验证了方法的正确性。