保存点云数据_3Ｄ点云配准（二多幅点云配准）

本文首发于微信公众号「3D视觉工坊」：3Ｄ点云配准（二多幅点云配准）

在上一篇文章点云配准（一两两配准）中我们介绍了两两点云之间的配准原理。本篇文章，我们主要介绍一下PCL中对于多幅点云连续配准的实现过程，重点请关注代码行的注释。

对于多幅点云的配准，它的主要思想是对所有点云进行变换，使得都与第一个点云在统一坐标系中。在每个连贯的、有重叠的点云之间找到最佳的变换，并累积这些变换到全部的点云。能够进行ICP算法的点云需要进行粗略的预匹配，并且一个点云与另一个点云需要有重叠部分。

此处我们以郭浩主编的《点云库PCL从入门到精通》提供的示例demo来介绍一下多幅点云进行配准的过程。

/* ---[ */
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);}
}

对于上述过程中的核心函数pairAlign()，我们重点介绍如下：

/**匹配一对点云数据集并且返还结果*参数 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;}
效果图如下，此为动态图，请耐心等待几秒钟，注意迭代过程。