点云配准是什么?
点云配准是值将不同位置或视角获取的点云数据进行对齐的过程。当使用传感器(如激光雷达、摄像头等)从不同视角或位置获取多个点云时,这些点云可能存在不同的坐标系和姿态,使得它们直接叠加或进行比较。
点云配准的目的是找到一个坐标变换或刚体变换(如平移、旋转等),使得多个点云之间的对应点能够准确重合。通过配准,可以实现不同位置或视角的点云之间的一致性和对齐,从而提供更全面、准确的三维信息。
点云配准的步骤
- 特征提取:从每个点云中提取特征,例如表面法线、关键点等。这些特征用于识别点云中的重要点和特征。
- 特征匹配:将不同点云中提取的特征进行匹配,找到它们之间的对应关系。这一步骤可以使用最近邻匹配、描述子匹配等方法。
- 初始变换估计:根据特征的匹配结果,估计点云之间的初步变换,以将它们粗略对齐。
- 点对点或点对面配准:使用迭代最近点(Iterative Closest Point,ICP)等算法,在初步变换的基础上,优化配准的精度。这一步骤通过最小化匹配点之间的误差来调整变换参数,使得点云之间的对应点更准确地重合。
- 评估和优化:评估配准结果的准确性和稳定性,并根据需要进行进一步的优化或调整。
PCL库中已实现的点云配准方法
- Iterative Closest Point(ICP):ICP算法是一种最常用的点云配准方法。PCL提供了多个ICP相关的类,包
IterativeClosestPoint
,GeneralizedIterativeClosestPoint
等,用于执行刚性变换的点对点或点对面配准。 - Normal Distributions Transform(NDT):PCL实现了基于正态分布变换的点云配准方法,称为NDT算法。该方法可以对非刚性的点云进行配准,适用于更复杂的场景。
- Sample Consensus Initial Alignment(SAC-IA):PCL实现了Sample Consensus Initial Alignment(SAC-IA)算法,用于从初始变换估计点云配准。它基于采样一致性(Sample Consensus)原理,可以估计出两个点云之间的初始变换。
- Fast Global Registration(FastGICP):FastGICP是一种高效的全局配准算法,PCL提供了其实现。该算法通过压缩点云和通过粗略配准的辅助点云,实现了高效的全局优化。
- Go-ICP:PCL库还实现了Go-ICP算法,它是一个鲁棒的配准算法,对离群点和噪声有良好的适应性。
刚性点云与非刚性点云
拿一个硬盘盒子作为例子,可以将硬盘盒子放在桌子上的不同位置,并使用激光雷达或摄像头从不同视角获取点云数据。
如果我们假设硬盘盒子在不同位置和角度下始终保持不变的形状,那么这些点云数据被认为是刚性点云。即使点云在空间中的位置和方向发生变化,我们可以通过刚性变换(平移和旋转)来将这些点云配准到一个统一的坐标系,并使它们重合。
但是,如果我们考虑一个放在桌子上的软塑料球,当我们压缩或挤压球体时,它的形状会发生变化。点云数据会反映出球体在不同位置和角度下的形变。这种情况下,点云被认为是非刚性点云。由于球体的形状变化,简单的刚性变换可能无法将这些点云完全重合。
因此,对于刚性点云,我们可以使用刚性变换模型(例如平移、旋转等)将它们精确地对齐。而对于非刚性点云,我们需要使用更复杂的配准方法,例如基于变形场的方法来捕捉形变信息,并对点云进行适当的配准。
ICP配准刚性点云代码
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
int main(){
// ------------------------
// 创建输入点云与输出点云对象
// -----------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// ------------
// 点云数据填充
// ------------
srand(time(0));
cloud_in->width = 5;
cloud_in->height = 1;
// cloud_in->is_dense = false; // 可能存在无效点,即点云中存在无效点(例如NaN或无穷大值)
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;
// ----------------------------------------------------------------------------------------
// 对输出点云执行简单的刚性变换,将其在x轴方向上移动0.7米,相当于将输出点云整体沿着x轴正方向移动了一定距离
// 这样一来,输出点云与输入点云之间就有了差异
// ----------------------------------------------------------------------------------------
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->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; // 创建IterativeClosestPoint对象 icp
icp.setInputSource(cloud_in); // 设置源点点云;ICP算法将通过优化变换矩阵,使源点云与目标点云对齐
icp.setInputTarget(cloud_out); // 设置目标点云
pcl::PointCloud<pcl::PointXYZ> Final; // 用于存储配准后的源点云
icp.align(Final); // 点云配准
std::cout << "has converged:" << icp.hasConverged() // 算法是否收敛
<< " score: " << icp.getFitnessScore() << std::endl; // 配对的得分,越小匹配程度越好;这里是0,匹配效果很好
const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation();// ICP算法的最终变换矩阵
std::cout << matrix << std::endl;
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("result"));
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_result(Final.makeShared(), 255, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(Final.makeShared(), color_result, "result");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "result");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_tgt(cloud_out, 0, 255, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud_out, color_tgt, "tgt");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "tgt");
viewer->addCoordinateSystem(1.0);
while (!viewer->wasStopped()) {
viewer->spinOnce();
}
return 0;
}
配准后的源点云与目标单云重合