在上一篇文章 点云配准（一 两两配准）中我们介绍了两两点云之间的配准原理。本篇文章，我们主要介绍一下PCL中对于多幅点云连续配准的实现过程，重点请关注代码行的注释。
对于多幅点云的配准，它的主要思想是对所有点云进行变换，使得都与第一个点云在统一坐标系中。在每个连贯的、有重叠的点云之间找到最佳的变换，并累积这些变换到全部的点云。能够进行ICP算法的点云需要进行粗略的预匹配，并且一个点云与另一个点云需要有重叠部分。
image
此处我们以郭浩主编的《点云库PCL从入门到精通》提供的示例demo来介绍一下多幅点云进行配准的过程。
<pre style="background-color: rgb(255, 255, 255); color: rgb(0, 0, 0); font-size: 11.3pt; font-family: "DejaVu Sans Mono";">/* ---[ / int main (int argc, char* argv)
{
// 加载数据
std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //存储管理所有打开的点云
loadData (argc, argv, data); //加载所有点云文件到data
//检查用户输入
if (data.empty ())
{
PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> []", argv[0]);
PCL_ERROR ("[] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
PCL_INFO ("Example: %s rospack find pcl/test/bun0.pcd rospack find pcl/test/bun4.pcd", argv[0]);
return (-1);
}
PCL_INFO ("Loaded %d datasets.", (int)data.size ());
//创建一个PCL可视化对象
p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example"); //创建一个PCL可视化对象
p->createViewPort (0.0, 0, 0.5, 1.0, vp_1); //用左半窗口创建视口vp_1
p->createViewPort (0.5, 0, 1.0, 1.0, vp_2); //用右半窗口创建视口vp_2 PointCloud::Ptr result (new PointCloud), source, target;
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
for (size_t i = 1; i < data.size (); ++i) //循环处理所有点云
{
source = data[i-1].cloud; //连续配准
target = data[i].cloud; //相邻两组点云
//添加可视化数据
showCloudsLeft(source, target); //可视化为配准的源和目标点云
PointCloud::Ptr temp (new PointCloud);
PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
//调用子函数完成一组点云的配准，temp返回配准后两组点云在第一组点云坐标下的点云，pairTransform返回从目标点云target到源点云source的变换矩阵。
//现在我们开始进行实际的匹配，由子函数pairAlign具体实现，
//其中参数有输入一组需要配准的点云，以及是否进行下采样的设置项，其他参数输出配准后的点云及变换矩阵。
pairAlign (source, target, temp, pairTransform, true);
//把当前的两两配对转换到全局变换
//把当前的两两配对后的点云temp转换到全局坐标系下（第一个输入点云的坐标系）返回result
pcl::transformPointCloud (*temp, *result, GlobalTransform);
//update the global transform更新全局变换
//用当前的两组点云之间的变换更新全局变换
GlobalTransform = pairTransform * GlobalTransform;
//保存转换到第一个点云坐标下的当前配准后的两组点云result到文件i.pcd
std::stringstream ss;
ss << i << ".pcd";
pcl::io::savePCDFile (ss.str (), *result, true);
}
}</pre>
对于上述过程中的核心函数pairAlign()，我们重点介绍如下：
<pre style="background-color: rgb(255, 255, 255); color: rgb(0, 0, 0); font-size: 11.3pt; font-family: "DejaVu Sans Mono";">/**匹配一对点云数据集并且返还结果 参数 cloud_src 是源点云 参数 cloud_src 是目标点云 参数output输出的配准结果的源点云 参数final_transform是在来源和目标之间的转换 / void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
//下采样
//为了一致性和高速的下采样
//注意：为了大数据集需要允许这项
PointCloud::Ptr src (new PointCloud); //存储滤波后的源点云
PointCloud::Ptr tgt (new PointCloud); //存储滤波后的目标点云
pcl::VoxelGrid<PointT> grid; //滤波处理对象
if (downsample)
{
grid.setLeafSize (0.05, 0.05, 0.05); //设置滤波处理时采用的体素大小
grid.setInputCloud (cloud_src);
grid.filter (src);
grid.setInputCloud (cloud_tgt);
grid.filter (tgt);
}
else
{
src = cloud_src;
tgt = cloud_tgt;
}
//计算曲面法线和曲率
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est; //点云法线估计对象
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
norm_est.setSearchMethod (tree); //设置估计对象采用的搜索对象
norm_est.setKSearch (30); //设置估计时进行搜索用的k数
norm_est.setInputCloud (src);
norm_est.compute (points_with_normals_src); //下面分别估计源和目标点云法线
pcl::copyPointCloud (src, points_with_normals_src);
norm_est.setInputCloud (tgt);
norm_est.compute (points_with_normals_tgt);
pcl::copyPointCloud (tgt, *points_with_normals_tgt);
//一切准备好之后，可以开始配准了，创建ICP对象，设置它的参数
//以需要匹配的两个点云作为输入，使用时，参数的设置需要根据自己的数据集进行调整。
//
//举例说明我们自定义点的表示（以上定义）
MyPointRepresentation point_representation;
//调整'curvature'尺寸权重以便使它和x, y, z平衡
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);
//
// 配准
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; //配准对象
reg.setTransformationEpsilon (1e-6); //设置收敛判断条件，越小精度越大，收敛也越慢
//将两个对应关系之间的(src<->tgt)最大距离设置为10厘米
//注意：根据你的数据集大小来调整
reg.setMaxCorrespondenceDistance (0.1);
//设置点表示
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputCloud (points_with_normals_src); //设置源点云
reg.setInputTarget (points_with_normals_tgt); //设置目标点云
//
//在一个循环中运行相同的最优化并且使结果可视化
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
//由于这是一个demo，因而希望显示匹配过程的迭代过程，为达到该目的，ICP在内部进行计算时，限制其最大的迭代次数为2，即每迭代两次就认为收敛，停止内部迭代
reg.setMaximumIterations (2); //设置最大迭代次数
//手动迭代，此处设置为30次，每手动迭代一次，在配准结果视口对迭代的最新的结果进行刷新显示
for (int i = 0; i < 30; ++i)
{
PCL_INFO ("Iteration Nr. %d.\n", i);
//为了可视化的目的保存点云
points_with_normals_src = reg_result;
reg.setInputCloud (points_with_normals_src);
reg.align (*reg_result);
//在每一个迭代之间累积转换
Ti = reg.getFinalTransformation () * Ti;
//如果这次转换和之前转换之间的差异小于阈值
//则通过减小最大对应距离来改善程序
//也就是说，如果迭代N次找到的变换和迭代N-1次中找到的变换之间的差异小于传给ICP的变换收敛阈值，我们选择源和目标之间更靠近的对应点距离阈值来改善配准过程
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
prev = reg.getLastIncrementalTransformation ();
//可视化当前状态
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
//
//一旦找到最优的变换，ICP返回的变换是从源点云到目标点云的变换矩阵，我们求逆变换得到从目标点云到源点云的变换矩阵，并应用到目标点云，变换后的目标点云然后添加到源点云中。
// 得到目标点云到源点云的变换
targetToSource = Ti.inverse();
//
//把目标点云转换回源框架
pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);
p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");
//添加源点云到转换目标
*output += *cloud_src;
final_transform = targetToSource;
}
效果图如下，此为动态图，请耐心等待几秒钟，注意迭代过程。
</pre>
image
思考：
对于小型或者中型数量的点云数据（点个数<100，000），我们选择ICP来进行迭代配准，可是利用对应点的特征计算和匹配较为耗时，如果对于两个大型点云（都超过100000）之间的刚体变换的确定，有什么好办法可以解决呢？
主要参考:郭浩主编的<点云库PCL从入门到精通>
上述内容，如有侵犯版权，请联系作者，会自行删文。
星球成员，可免费提问，并邀进讨论群
image
网友评论