欢迎点云相关产学研的学者和团体加入我们。
本小节演示了在代码中使用ICP迭代最近点算法,程序随机生成一个点云作为源点云,并将其沿x轴平移后作为目标点云,然后利用ICP估计源到目标的刚体变换矩阵,中间对所有的信息打印到标准输入输出设备上。
首先,在PCL(Point Cloud Learning)中国协助发行的书[1]提供光盘的第13章例1文件夹中,打开名为iterative_closest_point.cpp的代码文件。
下面来分析程序中的关键语句。
#include// 标准输入输出头文件
#include//IO 操作头文件
#include// 点类型定义头文件
#include//ICP 配准类相关的头文件
这里我们引用的这些头文件包含我们将要使用的所有类和数据结构的定义。
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
创建两个pcl::PointCloud
structPointXYZ
{
float x;
float y;
float z;
};
以下代码:
// 随机数据填充点云数据
cloud_in->width=5; //设置点云宽度
cloud_in->height=1; //设置点云为无序点云
cloud_in->is_dense=false;
cloud_in->points.resize(cloud_in->width*cloud_in->height);
for(size_t i=0;i<cloud_in->points.size();++i)
{
cloud_in->points[i].x=1024*rand()/(RAND_MAX+1.0f);
cloud_in->points[i].y=1024*rand()/(RAND_MAX+1.0f);
cloud_in->points[i].z=1024*rand()/(RAND_MAX+1.0f);
}
std::cout<<"Saved "<<cloud_in->points.size()<<" data points to input:"
<<std::endl;
for(size_t i=0;i<cloud_in->points.size();++i)std::cout<<" "<<
cloud_in->points[i].x<<" "<<cloud_in->points[i].y<<" "<<
cloud_in->points[i].z<<std::endl;
*cloud_out=*cloud_in;
std::cout<<"size:"<<cloud_out->points.size()<<std::endl;
用产生的随机点值构造源点云cloud_in,并设置合适的参数(width, height, is_dense),同时,打印出保存的点数量和它们的实际坐标值。
for(size_t i=0;i<cloud_in->points.size();++i)
cloud_out->points[i].x=cloud_in->points[i].x+0.7f;
std::cout<<"Transformed "<<cloud_in->points.size()<<" data points:"
<<std::endl;
for(size_t i=0;i<cloud_out->points.size();++i)
std::cout<<" "<<cloud_out->points[i].x<<" "<<
cloud_out->points[i].y<<" "<<cloud_out->points[i].z<<std::endl;
以上代码实现一个简单的点云刚体变换,以构造目标点云,并再次打印出数据值。
pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
这部分代码创建了一个IterativeClosestPoint的对象,并设置了对应的目标点云和源点云,其中语句“icp.setInputCloud(cloud_in);”把cloud_in设置为点云的源点云,“icp.setInputTarget(cloud_out);”把cloud_out设置为与cloud_in对应的匹配目标。
pcl::PointCloud<pcl::PointXYZ> Final; //存储经过配准变换源点云后的点云
icp.align(Final); //执行配准存储变换后的源点云到Final
std::cout<<"has converged:"<<icp.hasConverged()<<" score: "<<
icp.getFitnessScore()<<std::endl; //打印配准相关输入信息
std::cout<<icp.getFinalTransformation()<<std::endl; //打印输出最终估计的变换矩阵
创建一个pcl::PointCloud
利用光盘提供的CMakeLists.txt文件,在cmake中建立工程文件,并生成相应的可执行文件。创建可执行程序后,你就可以运行它了,只需要执行:
...>iterative_closest_point.exe
你将看到类似于图1所示的输出,打印出利用ICP算法估计的变换矩阵等信息。
图1例1运行结果
敬请关注PCL(Point Cloud Learning)中国更多的点云库PCL(Point Cloud Library)相关官方教程。
参考文献:
1.朱德海、郭浩、苏伟.点云库PCL学习教程(ISBN 978-7-5124-0954-5)北京航空航天出版社 2012-10