PCL 注册 API
将各种 3D 点云数据视图一致地对齐到完整模型的过程称为注册。其目标是找到在全局坐标框架中单独获取的视图的相对位置和方向,以便它们之间的交叉区域完美重叠。因此,对于从不同视图获取的每个点云数据集,我们需要一个能够将它们对齐到单个点云模型的系统,以便可以应用后续处理步骤,如分割和对象重建。
代码首先包含了一系列pcl库的头文件,用于工作流中的各种点云处理步骤:输入输出、常用计算、点类型定义、特征计算、配准等。使用了pcl命名空间下的多个模块。定义了几种类型别名,例如PointNormal
被定义为PointT
。
在findCorrespondences
函数中,代码创建了一个CorrespondenceEstimationBackProjection
的对象,用来估计两个点云之间的对应关系。该函数依赖于点云的表面法线信息,使用带法线的点云来估计源点云src和目标点云tgt之间的对应关系。
rejectBadCorrespondences
函数负责拒绝/过滤掉坏的对应关系,它首先使用CorrespondenceRejectorMedianDistance
根据中位数因子来拒绝离群值,然后根据点法线被CorrespondenceRejectorSurfaceNormal
作为标准进一步拒绝不满足阈值的对应关系。
findTransformation
函数利用点到平面的最小二乘法估计(TransformationEstimationPointToPlaneLLS
)来找到最佳变换,帮助将src点云对齐到tgt点云。
view
函数是用于可视化结果的辅助函数。如果设置了可视化标志,它会显示源点云、目标点云以及它们之间的对应关系。
在icp
函数中,执行了迭代最近点(ICP)算法,基于以上定义的函数完成对两个点云的配准。这个过程会循环查找对应关系,拒绝坏的对应关系,估计变换,并不断更新源点云的位置直到收敛。最终,将变换矩阵返回。
saveTransform
函数负责将最终的变换矩阵写入一个外部文件中,以便于其他应用或者分析使用。
main
函数是程序的入口点,解析命令行参数来获取输入输出文件,设置调试和可视化选项,加载点云文件,调用ICP算法配准点云,保存变换矩阵,并在标准输出中打印变换矩阵以供查看。
该程序的总体目标是,加载两个PCD文件,利用ICP算法寻找一个变换矩阵,从而可以最好地使两个点云对齐。在这一过程中,它还提供了错误拒绝策略,以提高配准的准确性,并且可选地支持结果的可视化。最后,将最佳变换矩阵保存到文件,并打印到控制台。
#include <fstream> // 包含C++文件操作库
#include <pcl/common/angles.h> // 包含PCL库中的角度转换相关功能
#include <pcl/console/parse.h> // 包含PCL命令行解析功能
#include <pcl/point_types.h> // 包含PCL点类型定义
#include <pcl/point_cloud.h> // 包含PCL点云数据结构
#include <pcl/point_representation.h> // 包含PCL点表示方法
#include <pcl/io/pcd_io.h> // 包含PCL的PCD文件输入输出操作
#include <pcl/console/time.h> // 包含PCL计时功能
#include <pcl/features/normal_3d.h> // 包含PCL表面法线估计相关功能
#include <pcl/features/fpfh.h> // 包含PCL的FPFH特征描述子计算功能
#include <pcl/registration/correspondence_estimation.h> // 包含PCL点云配准中的对应点估计功能
#include <pcl/registration/correspondence_estimation_normal_shooting.h> // 包含PCL取法线方向作为对应点搜索方向的对应点估计方法
#include <pcl/registration/correspondence_estimation_backprojection.h> // 包含PCL基于反投影的对应点估计方法
#include <pcl/registration/correspondence_rejection_median_distance.h> // 包含PCL基于中值距离的对应点剔除方法
#include <pcl/registration/correspondence_rejection_surface_normal.h> // 包含PCL基于表面法线一致性的对应点剔除方法
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h> // 包含PCL的点到平面最小二乘法变换估计方法
#include <pcl/registration/default_convergence_criteria.h> // 包含PCL的默认收敛判断条件定义
#include <pcl/visualization/pcl_visualizer.h> // 包含PCL的点云可视化工具
using namespace pcl; // 使用PCL命名空间
using namespace pcl::io; // 使用PCL输入输出命名空间
using namespace pcl::console; // 使用PCL控制台命名空间
using namespace pcl::registration; // 使用PCL注册命名空间
using namespace pcl::visualization; // 使用PCL可视化命名空间
typedef PointNormal PointT; // 定义点类型别名,这里使用包含法线的点类型
typedef PointCloud<PointT> Cloud; // 定义点云类型别名
typedef Cloud::Ptr CloudPtr; // 定义点云智能指针别名
typedef Cloud::ConstPtr CloudConstPtr; // 定义点云常量智能指针别名
CloudPtr src, tgt; // 声明源点云和目标点云的智能指针
bool rejection = true; // 是否执行对应点剔除
bool visualize = false; // 是否执行可视化
PCLVisualizer::Ptr vis; // PCL可视化实例的智能指针
// 以下是一系列函数的定义
// 对应估计(correspondence estimation)
void
findCorrespondences (const CloudPtr &src,
const CloudPtr &tgt,
Correspondences &all_correspondences)
{
// CorrespondenceEstimationNormalShooting<PointT, PointT, PointT> est; // 创建基于法线射击的对应点估计实例
// CorrespondenceEstimation<PointT, PointT> est; // 创建基于最近点的对应点估计实例
CorrespondenceEstimationBackProjection<PointT, PointT, PointT> est; // 创建基于反投影的对应点估计实例
est.setInputSource (src); // 设置源点云
est.setInputTarget (tgt); // 设置目标点云
est.setSourceNormals (src); // 设置源点云的法线
est.setTargetNormals (tgt); // 设置目标点云的法线
est.setKSearch (10); // 设置在目标点云中搜索最近点的数量上限
est.determineCorrespondences (all_correspondences); // 计算对应点
// est.determineReciprocalCorrespondences (all_correspondences); // 计算双向最近邻对应点(注释掉了)
}
//拒绝/过滤掉坏的对应关系
void
rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
const CloudPtr &src,
const CloudPtr &tgt,
Correspondences &remaining_correspondences)
{
CorrespondenceRejectorMedianDistance rej; // 创建基于中值距离的对应点剔除实例
rej.setMedianFactor (8.79241104); // 设置中值因子
rej.setInputCorrespondences (all_correspondences); // 设置输入的对应点
rej.getCorrespondences (remaining_correspondences); // 执行剔除操作
return; // 返回,以下代码被注释
CorrespondencesPtr remaining_correspondences_temp (new Correspondences); // 创建剩余对应点的智能指针
rej.getCorrespondences (*remaining_correspondences_temp); // 执行剔除并保存结果
PCL_DEBUG ("[rejectBadCorrespondences] Number of correspondences remaining after rejection: %d\n", remaining_correspondences_temp->size ());
// Reject if the angle between the normals is really off
CorrespondenceRejectorSurfaceNormal rej_normals; // 创建基于表面法线一致性的对应点剔除实例
rej_normals.setThreshold (std::acos (deg2rad (45.0))); // 设定法线夹角的阈值
rej_normals.initializeDataContainer<PointT, PointT> (); // 初始化数据容器
rej_normals.setInputCloud<PointT> (src); // 设置源点云
rej_normals.setInputNormals<PointT, PointT> (src); // 设置源点云的法线
rej_normals.setInputTarget<PointT> (tgt); // 设置目标点云
rej_normals.setTargetNormals<PointT, PointT> (tgt); // 设置目标点云的法线
rej_normals.setInputCorrespondences (remaining_correspondences_temp); // 设置输入的对应点
rej_normals.getCorrespondences (remaining_correspondences); // 执行剔除操作
}
void
findTransformation (const CloudPtr &src,
const CloudPtr &tgt,
const CorrespondencesPtr &correspondences,
Eigen::Matrix4d &transform)
{
TransformationEstimationPointToPlaneLLS<PointT, PointT, double> trans_est; // 创建点到平面最小二乘变换估计实例
trans_est.estimateRigidTransformation (*src, *tgt, *correspondences, transform); // 估计刚性变换
}
void
view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
{
if (!visualize || !vis) return; // 如果不需要可视化或可视化实例未创建,直接返回
PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0); // 创建绿色的点云颜色处理器
if (!vis->updatePointCloud<PointT> (src, "source"))
{
vis->addPointCloud<PointT> (src, "source"); // 添加或更新源点云到可视化中
vis->resetCameraViewpoint ("source"); // 重置相机视角到源点云
}
if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target"); // 添加或更新目标点云到可视化中
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source"); // 设置源点云的透明度
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target"); // 设置目标点云的透明度
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source"); // 设置源点云的点大小
pcl::console::TicToc tt; // 创建计时器
tt.tic (); // 开始计时
if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1))
vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences"); // 添加或更新对应点到可视化中
tt.toc_print (); // 打印计时结果
vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences"); // 设置对应点连接线宽度
// vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences"); // 设置对应点连接线颜色(注释掉了)
vis->spin (); // 开始交互式可视化
}
void
icp (const PointCloud<PointT>::Ptr &src, // ICP迭代过程的函数定义
const PointCloud<PointT>::Ptr &tgt,
Eigen::Matrix4d &transform)
{
CorrespondencesPtr all_correspondences (new Correspondences),
good_correspondences (new Correspondences); // 创建所有对应点和好的对应点的智能指针
PointCloud<PointT>::Ptr output (new PointCloud<PointT>); // 创建输出的点云智能指针
*output = *src; // 将源点云内容拷贝到输出点云
//初始化从源点云到目标点云的变换矩阵
Eigen::Matrix4d final_transform (Eigen::Matrix4d::Identity ()); // 初始化最终变换矩阵为单位矩阵
int iterations = 0; // 初始化迭代次数
DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences); // 创建默认的收敛判断准则实例
// ICP循环
do
{
// Find correspondences
findCorrespondences (output, tgt, *all_correspondences); // 查找对应点
PCL_DEBUG ("Number of correspondences found: %d\n", all_correspondences->size ()); // 打印找到的对应点数量
if (rejection)
{
// Reject correspondences
rejectBadCorrespondences (all_correspondences, output, tgt, *good_correspondences); // 剔除对应点
PCL_DEBUG ("Number of correspondences remaining after rejection: %d\n", good_correspondences->size ()); // 打印剔除后剩余的对应点数量
}
else
*good_correspondences = *all_correspondences; // 如果不执行剔除,则所有找到的对应点都是好的
// Find transformation
findTransformation (output, tgt, good_correspondences, transform); // 查找变换矩阵
// Obtain the final transformation
final_transform = transform * final_transform; // 更新最终变换矩阵
// Transform the data
transformPointCloudWithNormals (*src, *output, final_transform.cast<float> ()); // 将源点云根据最终变换矩阵进行变换
// Check if convergence has been reached
++iterations; // 迭代次数加一
// Visualize the results
view (output, tgt, good_correspondences); // 可视化结果
}
while (!converged); // 检查是否收敛
transform = final_transform; // 结束时更新传出变换矩阵
}
void
saveTransform (const std::string &file, const Eigen::Matrix4d &transform)
{
ofstream ofs; // 定义一个文件输出流对象
ofs.open (file.c_str (), std::ios::trunc | std::ios::binary); // 以截断和二进制模式打开文件
for (int i = 0; i < 4; ++i) // 遍历变换矩阵的行
for (int j = 0; j < 4; ++j) // 遍历变换矩阵的列
ofs.write (reinterpret_cast<const char*>(&transform (i, j)), sizeof (double)); // 将矩阵的每个元素写入文件
ofs.close (); // 关闭文件
}
/* ---[ */
int
main (int argc, char** argv)
{
// Check whether we want to enable debug mode
bool debug = false; // 定义一个标志,指示是否启用调试模式
parse_argument (argc, argv, "-debug", debug); // 解析命令行参数,检查是否启用调试模式
if (debug)
setVerbosityLevel (L_DEBUG); // 如果启用调试模式,则设置日志级别为调试
parse_argument (argc, argv, "-rejection", rejection); // 解析命令行参数,检查是否启用拒绝策略
parse_argument (argc, argv, "-visualization", visualize); // 解析命令行参数,检查是否启用可视化
if (visualize)
vis.reset (new PCLVisualizer ("Registration example")); // 如果启用可视化,则初始化可视化窗口
// Parse the command line arguments for .pcd and .transform files
std::vector<int> p_pcd_file_indices, p_tr_file_indices; // 定义两个向量,用于存储.pcd和.transform文件的索引
p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); // 解析命令行参数,获取.pcd文件的索引
if (p_pcd_file_indices.size () != 2) // 如果不是两个.pcd文件,打印错误并退出
{
print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
return (-1);
}
p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform"); // 解析命令行参数,获取.transform文件的索引
if (p_tr_file_indices.size () != 1) // 如果没有一个.transform文件,打印错误并退出
{
print_error ("Need one output transform file to continue.\n");
print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
return (-1);
}
// Load the files
print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]); // 打印加载文件的信息
src.reset (new PointCloud<PointT>); // 初始化源点云
tgt.reset (new PointCloud<PointT>); // 初始化目标点云
if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1) // 如果读取文件失败,打印错误并退出
{
print_error ("Error reading the input files!\n");
return (-1);
}
// Compute the best transformtion
Eigen::Matrix4d transform; // 定义变换矩阵
icp (src, tgt, transform); // 调用ICP算法计算最佳变换
saveTransform (argv[p_tr_file_indices[0]], transform); // 保存变换到文件
std::cerr.precision (15); // 设置输出精度
std::cerr << transform << std::endl; // 打印变换矩阵
}
/* ]--- */
实现点云间的迭代最近点(ICP)配准,并保存计算得到的变换矩阵到文件中。程序首先解析命令行参数,包括是否启用调试模式、是否进行拒绝策略检查和是否启用可视化。接着,它加载源点云和目标点云文件,并执行ICP算法来计算两个点云间的最佳变换。计算完成后,将变换矩阵保存到文件,并打印出变换矩阵。
点云智能指针 与 点云常量智能指针区别
CorrespondenceEstimationBackProjection<PointT, PointT, PointT>
CorrespondenceRejectorMedianDistance 与
CorrespondenceRejectorSurfaceNormal
TransformationEstimationPointToPlaneLLS<PointT, PointT, double>
resetCameraViewpoint
vis->updateCorrespondences<PointT>(src, tgt, *correspondences, 1)
ofs.open(file.c_str(),
std::ios::trunc | std::ios::binary);
ofs.write(reinterpret_cast<const char*>(&transform(i, j)),
sizeof(double));
DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences);