前言
有时,我们需要将离散点云投影到参数化的模型上(如任意平面),以便于后续的一些特定处理,这里的参数化模型其实就是用一个方程来表示的空间中的平面,高中生都知道,平面方程是ax+by+cz+d=0
其中(a,b,c)即为这个平面的法向量,c=1的时候,法向量就是与z轴平行了,此时平面当然与z轴垂直,为水平面
一、点云投影到平面?
这个平面的方程,可以是我们拟合局部范围内的点云得到的平面(比如RANSAC平面拟合,最小二乘平面拟合之类的),也可以是我们指定三个点组成的平面(显然三点可以确定一个面),也可以是我们直接指定的平面(比如0x+0y+1z+0=0,就是过原点的xoy水平面),
在PCL中,是用专门的容器ModelCoefficients来存储参数的,这里 的[0],[1], [2], [3]分别存储平面方程中的a,b,c,d值
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;
在明确了所要投影的平面参数后,即可将一个个离散的不规则分布的点云投影到该平面上,使其分布相对规则,当然具体投影也很简单,这都是有数学公式可以直接计算的,不调用PCL库也可以直接实现,感兴趣的可以直接看:
点云投影到一般式平面
但有时候我们只知道平面上一点和平面的法向量,这时也可以直接看:
点云投影到点法式平面
当然,学会计算点到平面的投影点后,很自然的就可以根据投影前和投影后点的距离得到投影前的原始点到平面的距离,这样点到平面的距离也就会计算了,比如:
点到平面的距离
当然了,比起自己实现,PCL也提供相应的计算模块,下面为具体的代码,直接调用即可
二、算法实现
1.代码
代码如下(示例):
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
// 随机生成点云数据,如果你有自己的测试数据,也是在这读取
cloud->width = 5;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (auto& point: *cloud)
{
point.x = 1024 * rand () / (RAND_MAX + 1.0f);
point.y = 1024 * rand () / (RAND_MAX + 1.0f);
point.z = 1024 * rand () / (RAND_MAX + 1.0f);
}
//这里是输出投影前的点云坐标
std::cerr << "Cloud before projection: " << std::endl;
for (const auto& point: *cloud)
std::cerr << " " << point.x << " "
<< point.y << " "
<< point.z << std::endl;
// 确定要投影的平面的参数方程,上面有介绍
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;
// 具体投影计算就是着一小坨代码了
pcl::ProjectInliers<pcl::PointXYZ> proj;//创建投影器
proj.setModelType (pcl::SACMODEL_PLANE);//选择要投影的是平面
proj.setInputCloud (cloud);//输入投影前的点云
proj.setModelCoefficients (coefficients);//输入平面的参数
proj.filter (*cloud_projected);//输出投影后的点云
//这里是一些投影后点云坐标的输出,可以看看
std::cerr << "Cloud after projection: " << std::endl;
for (const auto& point: *cloud_projected)
std::cerr << " " << point.x << " "
<< point.y << " "
<< point.z << std::endl;
return (0);
}
总结
代码我没跑,但应该没问题哈哈
一切顺利的话,你将会看到如下结果:
Cloud before projection:
0.352222 -0.151883 -0.106395
-0.397406 -0.473106 0.292602
-0.731898 0.667105 0.441304
-0.734766 0.854581 -0.0361733
-0.4607 -0.277468 -0.916762
Cloud after projection:
0.352222 -0.151883 0
-0.397406 -0.473106 0
-0.731898 0.667105 0
-0.734766 0.854581 0
-0.4607 -0.277468 0
文章出处登录后可见!
已经登录?立即刷新