1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
| #include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>//DNT（正态分布）标配类头文件
#include <pcl/filters/approximate_voxel_grid.h>//滤波类头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
int main(int argc,char** argv) {
//加载房间的第一次扫描点云数据作为目标
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;
}
//后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，得到第二组点云变换到第一组点云坐标系下的变换矩阵
//将输入的扫描点云数据过滤到原始尺寸的10%以提高匹配的速度。只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理
//因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据
std::cout<<"Loaded "<<input_cloud->size()<<" data points from room_scan2.pcd"<<std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_grid;
approximate_voxel_grid.setLeafSize(0.2,0.2,0.2);
approximate_voxel_grid.setInputCloud(input_cloud);
approximate_voxel_grid.filter(*filtered_cloud);
std::cout<<"Filtered cloud contains "<<filtered_cloud->size()<<" data points from room_scan2.pcd"<<std::endl;
//实例化DNT对象
pcl::NormalDistributionsTransform<pcl::PointXYZ,pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon(0.01);//为终止条件设置最小转换差异
ndt.setStepSize(0.1);//为more-thuente线搜索设置最大步长
ndt.setResolution(1.0);//设置NDT网格结构的分辨率
//以上参数在对象较大时比较适用，而如果对象很小，比如一个杯子，则参数要进行很大程度的缩小
//设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon边下阈值下终止，
//添加最大迭代次数限制，能够增加程序的鲁棒性，阻止程序在错误的方向上运行时间过长
ndt.setMaximumIterations(35);
//设置源点云
ndt.setInputSource(filtered_cloud);
//设置目标点云
ndt.setInputTarget(target_cloud);
//设置机器人测距法得到的粗略的矩阵结果
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();
//输出结果
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//这里的output不能做为最后的结果，因为上面对点云做了滤波处理，并非原始的点云数据
ndt.align(*output_cloud,init_guess);
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);
//将两个点云进行可视化，其中源点云为绿色，目标点云为红色
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> 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");
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");
viewer_final->addCoordinateSystem(1.0);//显示坐标轴
viewer_final->initCameraParameters();//初始化摄像头
while (!viewer_final->wasStopped()){
viewer_final->spinOnce();
}
return 0;
}
|
