@[TOC]PCL中点云配准模块的学习
学习背景
参考书籍:《点云库PCL从入门到精通》以及官方代码PCL官方代码链接,,PCL版本为1.10.0,CMake版本为3.16,可用点云下载地址
学习内容
使用正态分布算法来确定两个大型点云之间的刚体变换,正态分布变换算法是一个配准算法,它应用于三维点的统计模型,使用标准最优化技术来确定两个点云间的最优匹配,因为其在配准的过程中不利用对应点的特征计算和匹配,所以时间比其他方法比较快,本章点云下载room_scan1.pcd,room_scan2.pcd
源代码及所用函数
源代码
cpp
//使用正态分布变换进行配准的实验 。其中room_scan1.pcd room_scan2.pcd这些点云包含同一房间360不同视角的扫描数据
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include<thread>//C++管理线程头文件
#include<pcl/registration/ndt.h>//用于执行正态分布变换(Normal Distributions Transform, NDT)配准算法的头文件
#include<pcl/filters/approximate_voxel_grid.h>//滤波头文件
using namespace std::chrono_literals;
int main()
{
//********************************************加载房间的第一次扫描点云数据作为目标***************************//
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ>("/home/jojo/PointCloud/PCLB/Registration/03/room_scan1.pcd",*target_cloud) == -1)
{
//std::cout << " 加载文件失败" << std::endl;
PCL_ERROR("加载文件room_scan1.pcd失败\n");
return -1;
}
std::cout << "加载 " << target_cloud->size () << " 个点从room_scan1.pcd中" << std::endl;
//*************************************加载从新视角得到的第二次扫描点云数据作为源点云***************************//
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/jojo/PointCloud/PCLB/Registration/03/room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("加载文件room_scan2.pcd失败\n \n");
return (-1);
}
std::cout << "加载 " << input_cloud->size () << " 个点从room_scan2.pcd中" << std::endl;
//************************将输入扫描内容过滤为原稿大小的 10%左右,以提高套准速度*********************//
//以上的代码加载了两个PCD文件得到共享指针,后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计,得到第二组点云变换到第一组点云坐标系下的变换矩阵
//因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点,而是使用包含在每个体素单元格中的点的统计数据
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->size ()
<< " 个点从room_scan2.pcd中" << std::endl;
//*****************************************************初始化正态分布变换************************//
pcl::NormalDistributionsTransform<pcl::PointXYZ,pcl::PointXYZ> ndt;
//设置与刻度相关的无损检测参数
//为终止条件设置最小变换差。
ndt.setTransformationEpsilon(0.01);
//设置 More-Thuente 线搜索的最大步长
ndt.setStepSize(0.1);
//设置NDT网格网格结构的分辨率
ndt.setResolution (1.0);
//以上参数在使用房间尺寸比例下运算比较好,但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体,需要对参数进行很大程度的缩小
//设置匹配迭代的最大次数,这个参数控制程序运行的最大迭代次数,一般来说这个限制值之前优化程序会在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);
//将平移矩阵init_translation与从四元数init_rotation获得的旋转矩阵相乘,初始化了一个4x4变换矩阵init_guess。
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
// 计算需要的刚体变换以便将输入的源点云匹配到目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud,init_guess);//点云配准
//这个地方的output_cloud不能作为最终的源点云变换,因为上面对点云进行了滤波处理
std::cout << "正态分布变换已收敛:" << ndt.hasConverged ()
<< " 分数为: " << ndt.getFitnessScore () << std::endl;
//使用创建的变换对为过滤的输入点云进行变换
pcl::transformPointCloud(*input_cloud,*output_cloud,ndt.getFinalTransformation());
//保存转换后的源点云作为最终的变换输出
pcl::io::savePCDFileASCII("/home/jojo/PointCloud/PCLB/Registration/03/room_scan2_transformed.pcd",*output_cloud);
//***************************************可视化点云***********************************//
pcl::visualization::PCLVisualizer::Ptr 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,2,"target cloud");
//对转换后的源点云着色 (green)可视化.
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,2,"output cloud");
// 启动可视化
viewer_final->addCoordinateSystem(1.0);//显示XYZ指示轴
viewer_final->initCameraParameters();//使用默认摄像头参数
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
std::this_thread::sleep_for(100ms);
}
return (0);
}
CMakeLists.txt
cpp
cmake_minimum_required(VERSION 3.16 FATAL_ERROR)#指定CMake的最低版本要求为3.16
project(project)#设置项目名称
find_package(PCL 1.10 REQUIRED)#查找PCL库,要求版本为1.10或更高。
include_directories(${PCL_INCLUDE_DIRS})#将PCL库的头文件目录添加到包含路径中
link_directories(${PCL_LIBRARY_DIRS})#将PCL库的库文件目录添加到链接器搜索路径中。
add_definitions(${PCL_DEFINITIONS})#添加PCL库的编译器定义
add_executable (normal_distributions_transform normal_distributions_transform.cpp)
target_link_libraries (normal_distributions_transform ${PCL_LIBRARIES})#将PCL库链接到可执行文件目标。
运行结果
函数
Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
这行代码创建了一个旋转向量init_rotation,表示绕Z轴旋转0.6931弧度(约39.7度)。
Eigen::AngleAxisf是Eigen库中表示旋转的类,它接受一个旋转角度和一个旋转轴作为参数。
Eigen::Vector3f::UnitZ()返回一个表示Z轴单位向量的向量,即(0, 0, 1)。Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
这行代码创建了一个平移向量init_translation,表示在X轴方向平移1.79387,在Y轴方向平移0.720047,在Z轴方向不平移。Eigen::Translation3f
是Eigen库中表示三维平移的类,它接受三个浮点数作为参数,分别表示在X、Y、Z轴方向上的平移量。