欢迎点云相关产学研的学者和团体加入我们。
本节我们将介绍如何使用正态分布变换算法来确定两个大型点云(都超过100,000个点)之间的刚体变换。正态分布变换算法是一个配准算法,它应用于三维点的统计模型,使用标准最优化技术来确定两个点云间的最优的匹配,因为其在配准过程中不利用对应点的特征计算和匹配,所以时间比其他方法快,更多关于正态分布变换算法的详细的信息,请看Martin Magnusson博士的博士毕业论文“The Three-Dimensional Normal Distributions Transform – an Efficient Representation for Registration, Surface Analysis, and Loop Detection”。
首先,在PCL(Point Cloud Learning)中国协助发行的书[1]提供光盘的第13章例3文件夹中,打开名为normal_distributions_transform.cpp的代码文件,同文件夹下可以找到相关的测试点云文件room_scan1.pcd和room_scan2.pcd,这些点云文件包含同一房间360度不同视角的扫描数据。
现在,让我们把这个代码分解为一段一段来讲解。
#include//NDT 配准类对应头文件
#include// 滤波类对应头文件
这些是使用正态分布变换算法和用来过滤数据的过滤器对应的头文件,这个过滤器可以用其他过滤器来替换,但是使用体素网格过滤器(approximate voxel filter)处理结果较好。
//加载房间的第一次扫描点云数据作为目标
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd",*target_cloud)==-1)
{
PCL_ERROR("Couldn't read file room_scan1.pcd \n");
return(-1);
}
std::cout<<"Loaded "<<target_cloud->size()<<" data points from room_scan1.pcd"<<std::endl;
//加载从新视角得到的房间的第二次扫描点云数据作为源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan2.pcd",*input_cloud)==-1)
{
PCL_ERROR("Couldn't read file room_scan2.pcd \n");
return(-1);
}
std::cout<<"Loaded "<<input_cloud->size()<<" data points from room_scan2.pcd"<<std::endl;
以上代码加载了两个pcd文件到共享指针,后续配准是完成对源点云到目标点云的参考坐标系变换矩阵的估计,即得到这里的第二组点云变换到第一组点云坐标系下的变换矩阵。
//将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize(0.2,0.2,0.2);
approximate_voxel_filter.setInputCloud(input_cloud);
approximate_voxel_filter.filter(*filtered_cloud);
std::cout<<"Filtered cloud contains "<<filtered_cloud->size()
<<" data points from room_scan2.pcd"<<std::endl;
上面代码块过滤输入点云是为了缩短匹配时间,任何均匀地过滤数据的过滤器都可以完成此部分工作,这里只对源点云进行了滤波处理,减少其数据量到原先的大概10%左右,而目标点云不需要滤波处理,因为NDT算法中,在目标点云对应的体素网格数据结构的统计计算不使用单个点,而是使用包含在每个体素单元格中的点的统计数据。
//初始化正态分布变换(NDT)对象
pcl::NormalDistributionsTransform<pcl::PointXYZ,pcl::PointXYZ> ndt;
这里创建默认参数的NDT,NDT对象内部的数据结构等在后面再一一设置。
//根据输入数据的尺度设置NDT相关参数
ndt.setTransformationEpsilon(0.01);//为终止条件设置最小转换差异
ndt.setStepSize(0.1);//为More-Thuente线搜索设置最大步长
ndt.setResolution(1.0);//设置NDT网格结构的分辨率(VoxelGridCovariance)
这里我们设置一些尺度相关的参数,因为NDT算法使用一个体素化数据结构和More-Thuente线搜索,因此需要缩放一些参数来适应数据集。以上参数看起来似乎在我们使用的房间尺寸比例下运行地很好,但是它们如果需要处理例如一个咖啡杯的扫描之类更小物体,需要对参数进行很大程度的缩小。
在变换中Epsilon参数分别从长度和弧度,定义了变换矢量[x, y, z, roll, pitch, yaw]的最小许可的递增量,一旦递增量减小到这个临界值以下,那么配准算法就将终止。步长参数定义了More-Thuente线搜索允许的最大步长,这个线搜索算法确定了最大值以下的最佳步长,当你靠近最优解时该算法会缩短迭代步长,在更大的最大步长将会在较少的迭代次数下遍历较大的距离,但是却有过度迭代和在不符合要求的局部最小值处结束的风险。最后,分辨率参数定义了内部NDT网格结构的体素分辨率。这个结构非常方便搜索,并且每个体素包含与点有关的统计数据,平均值,协方差等,统计数据作为一组多元高斯分布用来模拟点云,并且允许我们计算和优化体素内任意位置点的存在概率。该参数是与尺度最相关的,对每个体素它需要足够大来容纳至少6个点,但是又要足够小到能够代表唯一的场景,即具有辨别性。
ndt.setMaximumIterations(35); //设置匹配迭代的最大次数
这个参数控制了优化程序运行的最大迭代次数,一般来说,在达到这个限制值之前优化程序就会在epsilon变换阈值下终止。添加此最大迭代次数限制能够增加程序鲁棒性,阻止了它在错误的方向运行过长时间。
ndt.setInputCloud(filtered_cloud); //设置源点云
ndt.setInputTarget(target_cloud); //设置目标点云
这里,我们把点云赋给NDT配准对象,目标点云的坐标系是被匹配的输入点云的参考坐标系,匹配完成后输入点云将被变换到与目标点云统一坐标系下,当加载目标点云后,NDT算法的内部数据结构被初始化。
//设置使用机器人测距法得到的粗略初始变换矩阵结果
Eigen::AngleAxisf init_rotation(0.6931,Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(1.79387,0.720047,0);
Eigen::Matrix4f init_guess=(init_translation*init_rotation).matrix();
在这部分的代码块中,我们创建了一个点云配准变换矩阵的初始估计,虽然算法运行并不需要这样的一个初始变换矩阵,但是有了它,易于得到更好的结果,尤其是当参考坐标系之间有较大差异时,其实此处用的实例测试点云数据之间差距较大,如图13-所示。在机器人应用中,如用于生成数据集,通常使用测程法数据生成初始转换。
//计算需要的刚体变换以便将输入的源点云匹配到目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud,init_guess);
//此处output_cloud不能作为最终的源点云变换,因为上面对源点云进行了滤波处理
std::cout<<"Normal Distributions Transform has converged:"<<ndt.hasConverged()
<<" score: "<<ndt.getFitnessScore()<<std::endl;
最后,我们准备好进行配准点云,生成变换后的源点云保存在输出点云里,此处是将第二组打开的点云变换后保存。然后我们打印出配准结果和欧式适合度评分,其中欧式适合度评分是计算输出点云到最近目标点云对应点对的距离平方和。
//使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud(*input_cloud,*output_cloud,ndt.getFinalTransformation()); //保存转换后的源点云作为最终的变换输出
pcl::io::savePCDFileASCII("room_scan2_transformed.pcd",*output_cloud);
配准程序完成之后,输出点云将包含一个过滤的输入点云的变换版本,因为我们传递给算法的输入是滤波后的输入点云,而非原始的输入点云,为了获得原始点云的配准版本,我们从NDT算法的结果中提取最终变换矩阵,并变换我们的原始输入点云,现在保存这个点云到文件room_scan2_transformed.pcd中以便将来使用。
// 初始化点云可视化对象
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer_final->setBackgroundColor(0,0,0); //设置背景色为黑色
//对目标点云着色(红色)并可视化
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud,255,0,0);
viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud,target_color,"target cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"target cloud");
//对转换后的源点云着色(绿色)并可视化
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color(output_cloud,0,255,0);
viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud,output_color,"output cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"output cloud");
// 启动可视化
viewer_final->addCoordinateSystem(1.0); //显示xyz指示轴
viewer_final->initCameraParameters(); //初始化摄像头参数等
//等待直到可视化窗口关闭。
while(!viewer_final->wasStopped())
{
viewer_final->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
接下来的这部分不是必需的,但是若想看到最终程序的可视化结果,使用点云库的可视化类,可以轻松地完成此部分,首先我们用黑色背景生成一个可视化对象,并加载需要显示的点云到对象中。最后,启动可视化对象,等待直到可视化对象的窗口关闭。
利用光盘提供的CMakeLists.txt文件,在cmake中建立工程文件,并生成相应的可执行文件,将CD中提供的点云文件拷贝到你的工作文件夹中,创建可执行程序后,你就可以运行它了。例如,需要配准capture0001.pcd和capture0002.pcd,只需执行:
...>normal_distributions_transform
在这里,并没有键入要打开的点云文件名作为参数,因为程序是默认从当前工作目录中打开点云文件。如图1所示源点云与目标点云之间变换比较大,运行之后,你将会看到如图2类似的结果,打印出加载的配准源和目标文件名以及最终配准后的详细信息,以及匹配点云的可视化结果如图3所示,很明显,源点云和目标点云之间基本重合,并且使用不同的颜色显示。
图1 例3对应的源和目标点云
图2 例3运行标准输出设备输出
图3 例3程序变换后的源点云和目标点云
敬请关注PCL(Point Cloud Learning)中国更多的点云库PCL(Point Cloud Library)相关官方教程。
参考文献:
1.朱德海、郭浩、苏伟.点云库PCL学习教程(ISBN 978-7-5124-0954-5)北京航空航天出版社 2012-10