如何逐步匹配多幅点云(1)

【原文：http://www.pclcn.org/study/shownews.php?lang=cn&id=78】

本章演示了迭代最近点算法的使用，以便逐步地对一系列点云进行两两匹配。它的思想是对所有的点云进行变换，使得都与第一个点云在统一坐标系中。在每个连贯的有重叠的点云之间找到最佳的变换，并累积这些变换到全部的点云。能够进行ICP算法的点云需要粗略的预匹配（如：在一个机器人的量距内或在地图框架内），并且一个点云与另一个点云需要有重叠部分。

代码

首先，在PCL（Point Cloud Learning）中国协助发行的书[1]提供光盘的第13章例2文件夹中，打开名为pairwise_incremental_registration.cpp的代码文件，同文件夹下可以找到相关的测试点云文件capture000[1-5].pcd。

解释说明

首先快速看一下相关类和数据结构的头文件声明声明下面语句包含所有我们使用的类的定义的头文件。

#include                     //boost指针相关头文件

#include                         //点类型定义相关头文件

#include                         //点云类相关头文件

#include               //点表示相关头文件

#include                           //pcd文件打开存储类相关头文件

#include                 //基于体素网格化的滤波类相关头文件

#include                     //滤波相关头文件

#include                 //法线特征相关头文件

#include                   //icp类相关头文件

#include                //非线性icp类相关头文件

#include           //变换矩阵类相关头文件

#include    //可视化类相关头文件

为了可视化目的，方便用户直观的观察到配准前后结果以及配准过程，创建全局可视化对象变量，并定义左右视点分别显示配准前和配准后的结果点云。

pcl::visualization::PCLVisualizer *p;//创建可视化对象

int vp_1, vp_2;                        //定义存储左右视点ID

声明一个结构体，方便对点云以文件名和点云对象进行成对处理管理，在配准过程中，可以同时接受多个点云文件输入，程序从第一个文件开始，连续的两两配准处理，然后存储配准后的点云文件。

struct PCD

{

  PointCloud::Ptr cloud;

  std::string f_name;

PCD() : cloud (new PointCloud) {};

};

定义一个新的点表示。

//以< x, y, z, curvature >形式定义一个新的点表示

classMyPointRepresentation:public pcl::PointRepresentation <PointNormalT>

{

using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:

  MyPointRepresentation ()

  {

    nr_dimensions_ =4;//定义点的维度

  }

//重载copyToFloatArray方法来将点转化为4维数组

virtual void copyToFloatArray (const PointNormalT &p, float* out) const

  {

out[0] = p.x;

out[1] = p.y;

out[2] = p.z;

out[3] = p.curvature;

  }

};

整个配准过程分几步，来看看如何组织我们的函数，主函数检查用户输入，在点云矢量data中加载了用户输入的所有点云数据，建立两个视口的可视化对象，左边显示未配准的源和目标点云，右边显示配准后的源和目标点云。在为一对点云找到变换矩阵后，将目标点云变换到源点云坐标系下，并将源点云与变换后的目标点云存储到一点云文件，同时用此次找到的变换矩阵更新全局变换，用于将后续的点云都变换到与第一个输入点云同一坐标系下。

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 [*]", 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 ());

  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 (source, target, temp, pairTransform, true);

//把当前的两两配对后的点云temp转换到全局坐标系下（第一个输入点云的坐标系）返回result

    pcl::transformPointCloud (*temp, *result, GlobalTransform);

//用当前的两组点云之间的变换更新全局变换

    GlobalTransform = pairTransform * GlobalTransform;

//保存转换到第一个点云坐标下的当前配准后的两组点云result到文件i.pcd

std::stringstream ss;

ss<< i <<".pcd";

pcl::io::savePCDFile (ss.str (), *result, true);

整体的程序框架了解后，我们来分别对不同的子函数实现进行分析。加载数据非常简单，我们迭代其他程序的参数，检查每一个参数是否指向一个pcd文件，如果是，创建一个添加到点云矢量data中的PCD对象。

void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models)

{

std::string extension (".pcd");

for (int i =1; i < argc; i++) //第一个参数为命令本身，所以从第二个参数开始解析

  {

std::string fname = std::string (argv[i]);

if (fname.size () <= extension.size ())    //pcd文件名至少为5个字符大小字符串

continue;

    std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);

if (fname.compare (fname.size () - extension.size (), extension.size (), extension)==0)//检查参数是否为一个pcd后缀的文件

    {

//加载点云并保存在总体的点云列表中

      PCD m;

      m.f_name = argv[i];

pcl::io::loadPCDFile (argv[i], *m.cloud);

//从点云中移除NAN点

      std::vector<int> indices;

      pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);

      models.push_back (m);

...

现在我们开始进行实际匹配，由子函数pairAlign具体实现，其中参数有输入一组需要配准的点云，以及是否进行下采样的设置项，其他参数输出配准后的点云及变换矩阵。

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 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 norm_est; //点云法线估计对象

  pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ());

  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对象，设置它的参数，以需要匹配的两个点云作为输入，用户使用时，参数的设置需要根据自己的数据集进行调整。

// 配准

  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);   //设置目标点云

因为这是一个指导实例，我们希望显示匹配过程的迭代过程，为了这个目的，ICP在内部进行计算时，限制其最大的迭代次数为2，即每迭代两次就认为收敛，停止内部迭代。

  reg.setMaximumIterations (2);                     //设置最大迭代次数

         手动迭代（本例中是30次），每手动迭代一次，在配准结果视口对迭代的最新的结果进行刷新显示。

for (int i =0; i <30; ++i)

  {

[...]

    points_with_normals_src = reg_result;

    reg.setInputCloud (points_with_normals_src);

    reg.align (*reg_result);

[...]

  }

在每次迭代过程中，我们记录并积累由ICP返回的变换：

Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;

[...]

for (int i =0; i <30; ++i)

  {

[...]

    Ti = reg.getFinalTransformation () * Ti;

[...]

  }

如果迭代N次找到的变换和迭代N-1中找到的变换之间的差异小于传给ICP的变换收敛阈值，我们选择源与目标之间更靠近的对应点距离阈值来改善配准过程。

for (int i =0; i <30; ++i)

  {

[...]

if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())

      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () -0.001);

prev= reg.getLastIncrementalTransformation ();

[...]

  }

一旦找到最优的变换，ICP返回的变换是从源点云到目标点云的变换矩阵，我们求逆变换得到从目标点云到源点云的变换矩阵，并应用到目标点云，变换后的目标点云然后添加到源点云中，并且将点云和变换矩阵一起返回到主函数。

  targetToSource = Ti.inverse();// 得到目标点云到源点云的变换

//把目标点云变换到源点云坐标系下

  pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);

[...]

*output +=*cloud_src;

    final_transform = targetToSource;

    未完待续，敬请关注“如何逐步匹配多幅点云”的编译和运行程序和结果。