利用PCL(Point Cloud Library)进行点云拼接

本文翻译自官网教程

近期在做一个关于三维扫描的项目，需要用到点云拼接，从而接触到PCL。PCL是一个类似于OpenCV的开源库，只是OpenCV提供的是对二维图像的处理方法，而PCL提供了很多三维点云的处理功能，其中就包括点云拼接。在三维扫描项目中，需要利用点云拼接方法将多次扫描得到的点云数据拼合成一个整体，因此官网的这篇教程对我的项目很有帮助。这里翻译出来与大家共享。

翻译的过程中在保留原教程核心内容的基础上，也加入了博主实际操作的一些细节。红字是原文翻译，蓝字是博主注解。

下面正式开始

----------------------------------------

将物体对齐到点云

本教程关于如何将官方教程其他部分提到的一些功能结合起来，以解决一个比较高端的问题——将先前捕获的物体点云模型与其后捕捉的点云模型进行拼接。在本例中，我们将试图用一幅包含人脸的深度图像去匹配之前获得的人脸模板，这将使我们能够确定人脸在场景中的位置和方向。

代码

首先，从github.com/PointCloudLibrary/data/tree/master/tutorials/template_alignment/下载数据并解压。

进入该网址能够看到如下文件，虽然教程中说的好像可以一键下载一样，但博主没有找到该功能，所以就一一点进去粘到记事本里了……

接下来，复制并粘贴以下代码到您的编辑器并将其保存为 template_alignment.cpp

由于代码比较长，这里就不放了，可以到原教程获取。

解释

现在，让我们逐步分析代码。

我们从测试 FeatureCloud 类开始。这个类的定义是为了提供一个方便的计算和存储点云中每点的局部特征描述算子的方法。以下构造函数创建了一个 KdTreeFLANN 对象并初始化了半径变量，从而用于计算表面法线和局部特征。

    FeatureCloud () :      search_method_xyz_ (new SearchMethod),      normal_radius_ (0.02f),      feature_radius_ (0.02f)    {}

接下来我们定义输入点云的方法，既可以传递一个共享指针到 PointCloud，也可以提供PCD文件的文件名用来加载，不论哪种方法，在设置了输入后，都将调用 processInput，并按照接下来介绍的方法计算局部特征。

    // Process the given cloud    void    setInputCloud (PointCloud::Ptr xyz)    {      xyz_ = xyz;      processInput ();    }    // Load and process the cloud in the given PCD file    void    loadInputCloud (const std::string &pcd_file)    {      xyz_ = PointCloud::Ptr (new PointCloud);      pcl::io::loadPCDFile (pcd_file, *xyz_);      processInput ();    }

我们还定义了一些公共访问器方法用来获取指向点云、表面法相、局部特征描述符的共享指针。

    // Get a pointer to the cloud 3D points    PointCloud::Ptr    getPointCloud () const    {      return (xyz_);    }    // Get a pointer to the cloud of 3D surface normals    SurfaceNormals::Ptr    getSurfaceNormals () const    {      return (normals_);    }    // Get a pointer to the cloud of feature descriptors    LocalFeatures::Ptr    getLocalFeatures () const    {      return (features_);    }

接下来我们定义了用于处理输入的点云的方法，首先计算点云的表面法线，然后计算其局部特征。

    // Compute the surface normals and local features    void    processInput ()    {      computeSurfaceNormals ();      computeLocalFeatures ();    }

我们利用PCL的 NormalEstimation 类来计算表面法线。为做到这一点，我们必须指定输入点云。KdTree 用来搜索相邻点，而“半径”定义了每点的邻域大小，然后我们计算表面法线并在一个成员变量内存储它们以便后续使用。

    // Compute the surface normals    void    computeSurfaceNormals ()    {      normals_ = SurfaceNormals::Ptr (new SurfaceNormals);      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;      norm_est.setInputCloud (xyz_);      norm_est.setSearchMethod (search_method_xyz_);      norm_est.setRadiusSearch (normal_radius_);      norm_est.compute (*normals_);    }

同样地，我们利用PCL的 FPFHEstimation 类通过输入点云及其表面法线来计算“快速点特征直方图”（“Fast Point Feature Histogram”）描述算子。

    // Compute the local feature descriptors    void    computeLocalFeatures ()    {      features_ = LocalFeatures::Ptr (new LocalFeatures);      pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;      fpfh_est.setInputCloud (xyz_);      fpfh_est.setInputNormals (normals_);      fpfh_est.setSearchMethod (search_method_xyz_);      fpfh_est.setRadiusSearch (feature_radius_);      fpfh_est.compute (*features_);    }

上面描述的这一方法封装了计算特征描述算子以及将这些算子与对应的三维点云存储在一起所需要的操作。

现在我们分析 TemplateAlignment 类，就像名称所显示的，其被用来进行模板对齐（template alignment ），也被称作模板匹配（template fitting/matching/registration）。模板通常是指一组规模较小的像素点或三维点，来源于更大的已知物体或场景。通过将模板与新获得的图像或点云进行匹配，就可以获得模板所代表物体的位置及方向。

我们首先定义一个结构体来存储对齐结果。它包括一个表示匹配度(fitness)的浮点数（该数值越低则匹配效果越好），以及一个变换矩阵，用来描述模板上的点通过怎样的旋转和平移才能尽可能好地与目标点云匹配。

注释：

因为我们在这个结构体中包含了 Eigen::Matrix4f，我们需要包含 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 宏，这将会重载结构体的“新建操作”从而生成16字节对齐的指针。有兴趣的话可以在官方教程内了解更多。

    // A struct for storing alignment results    struct Result    {      float fitness_score;      Eigen::Matrix4f final_transformation;      EIGEN_MAKE_ALIGNED_OPERATOR_NEW    };

在构造函数中，我们初始化了 SampleConsensusInitialAlignment (SAC-IA) 对象用来进行对齐操作，为其每一个参量提供了初始值。注：最大通信距离(correspondence distance)实际上被指定为距离值的平方，我们决定限制误差上限为1厘米，所以实际传递的值为0.01平方厘米。

    TemplateAlignment () :      min_sample_distance_ (0.05f),      max_correspondence_distance_ (0.01f*0.01f),      nr_iterations_ (500)    {      // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm      sac_ia_.setMinSampleDistance (min_sample_distance_);      sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);      sac_ia_.setMaximumIterations (nr_iterations_);    }

接下来我们定义一种方法来设定目标点云（即与模板对齐的点云），按照 SAC-IA 对齐方法设置输入。

    // Set the given cloud as the target to which the templates will be aligned    void    setTargetCloud (FeatureCloud &target_cloud)    {      target_ = target_cloud;      sac_ia_.setInputTarget (target_cloud.getPointCloud ());      sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());    }

然后我们定义一种方法以指定哪个或哪些模板被用来匹配，每次调用这个方法都会把给定的模板添加到 FeatureClouds 的一个内部向量中存储起来，以便后续使用。

    // Add the given cloud to the list of template clouds    void    addTemplateCloud (FeatureCloud &template_cloud)    {      templates_.push_back (template_cloud);    }

接下来我们定义对齐方法，这一方法将模板作为输入并将其与通过调用 setInputTarget() 方法指定的目标点云进行匹配。其工作过程为，设置给定模板为 SAC-IA 算法的源点云(source cloud)，接下来调用匹配算法将源与目标进行匹配。需要注意这一匹配算法需要我们传入一个点云用来存储新对齐的点云，但是我们的应用可以忽略这个输出。作为代替，我们调用 SAC-IA 的存取器方法来获得对齐的匹配度以及最终的变换矩阵（从源点云到目标点云的刚性变换），并且以 Result 结构体的形式输出。

    // Align the given template cloud to the target specified by setTargetCloud ()    void    align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)    {      sac_ia_.setInputCloud (template_cloud.getPointCloud ());      sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());      pcl::PointCloud<pcl::PointXYZ> registration_output;      sac_ia_.align (registration_output);      result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);      result.final_transformation = sac_ia_.getFinalTransformation ();    }

由于这个类被设计用来处理多个模板，我们也定义了一个 Result 型向量用于储存所有模板的对齐结果。

    // Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()    void    alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)    {      results.resize (templates_.size ());      for (size_t i = 0; i < templates_.size (); ++i)      {        align (templates_[i], results[i]);      }    }

最后，我们定义了能够将所有模板与点云对齐的方法并返回最佳对齐的序号及其 Result 结构体。

    // Align all of template clouds to the target cloud to find the one with best alignment score    int    findBestAlignment (TemplateAlignment::Result &result)    {      // Align all of the templates to the target cloud      std::vector<Result, Eigen::aligned_allocator<Result> > results;      alignAll (results);      // Find the template with the best (lowest) fitness score      float lowest_score = std::numeric_limits<float>::infinity ();      int best_template = 0;      for (size_t i = 0; i < results.size (); ++i)      {        const Result &r = results[i];        if (r.fitness_score < lowest_score)        {          lowest_score = r.fitness_score;          best_template = (int) i;        }      }      // Output the best alignment      result = results[best_template];      return (best_template);    }

现在进行模板对齐的类已经完成，我们要将其应用到面部匹配问题。在提供的数据文件中，包括了6组从不同角度获得的人脸点云模板，每组都被降采样至5mm点距并人为修建使其仅含有来自面部的点。在下面的代码中，展示如何利用 TemplateAlignment 类在新的点云中确定人脸的位置及方向。
首先，我们加载物体的模板点云。我们已经将其存储为.PCD文件，也已经将这些文件的名称写入名为 object_templates.txt 的文件中。这里，我们读入每个文件名，加载到 FeatureCloud 中，并存入向量。

  // Load the object templates specified in the object_templates.txt file  std::vector<FeatureCloud> object_templates;  std::ifstream input_stream (argv[1]);  object_templates.resize (0);  std::string pcd_filename;  while (input_stream.good ())  {    std::getline (input_stream, pcd_filename);    if (pcd_filename.empty () || pcd_filename.at (0) == '#') // Skip blank lines or comments      continue;    FeatureCloud template_cloud;    template_cloud.loadInputCloud (pcd_filename);    object_templates.push_back (template_cloud);  }  input_stream.close ();

接下来我们加载目标点云（文件名由命令行提供）。

  // Load the target cloud PCD file  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  pcl::io::loadPCDFile (argv[2], *cloud);

接下来我们对点云做一些小的预处理以便对齐。第一步是过滤所有背景点。在本例中假定我们与所要对齐的人的距离小于1米，所以我们使用通道滤波，保留Z向（即深度方向）0至1米的范围。

  // Preprocess the cloud by...  // ...removing distant points  const float depth_limit = 1.0;  pcl::PassThrough<pcl::PointXYZ> pass;  pass.setInputCloud (cloud);  pass.setFilterFieldName ("z");  pass.setFilterLimits (0, depth_limit);  pass.filter (*cloud);

我们也将点云降采样到点距5mm，从而降低所需的计算量。

  // ... and downsampling the point cloud  const float voxel_grid_size = 0.005f;  pcl::VoxelGrid<pcl::PointXYZ> vox_grid;  vox_grid.setInputCloud (cloud);  vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);  //vox_grid.filter (*cloud); // Please see this http://www.pcl-developers.org/Possible-problem-in-new-VoxelGrid-implementation-from-PCL-1-5-0-td5490361.html

完成预处理后，我们生成目标的 FeatureCloud。

  vox_grid.filter (*tempCloud);  cloud = tempCloud; 

接下来，初始化 TemplateAlignment 物体，对此，我们要加入所有模板点云并设定目标点云。

  FeatureCloud target_cloud;  target_cloud.setInputCloud (cloud);  // Set the TemplateAlignment inputs  TemplateAlignment template_align;  for (size_t i = 0; i < object_templates.size (); ++i)  {

现在 TemplateAlignment 物体已经初始化，我们将利用 findBestAlignment方法确定哪个模板与目标点云配合得最好。我们将对齐结果保存到 best_alignment。

  }  template_align.setTargetCloud (target_cloud);  // Find the best template alignment

接下来我们输出结果。通过匹配分数(best_alignment.fitness_score)我们了解到匹配的效果，而变换矩阵(best_alignment.final_transformation)告诉我们对齐的模板相对于目标点云的位置及方向。特别地，由于这是一个刚性变换，其可以被分解为三维平移向量和3X3的旋转矩阵。

  int best_index = template_align.findBestAlignment (best_alignment);  const FeatureCloud &best_template = object_templates[best_index];  // Print the alignment fitness score (values less than 0.00002 are good)  printf ("Best fitness score: %f\n", best_alignment.fitness_score);  // Print the rotation matrix and translation vector  Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);  Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);  printf ("\n");  printf ("    | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));  printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));

最后，我们选取最佳匹配模板，应用其到目标点云的变换，并将对齐结果输出至.PCD文件以便我们将其可视化并观察对齐结果。

  printf ("\n");  printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));  // Save the aligned template for visualization

编译及运行程序

添加以下内容到 CMakeLists.txt 文件。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(template_alignment)find_package(PCL 1.2 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})link_directories(${PCL_LIBRARY_DIRS})add_definitions(${PCL_DEFINITIONS})add_executable (template_alignment template_alignment.cpp)target_link_libraries (template_alignment ${PCL_LIBRARIES})

生成可执行文件后，可以这样运行：

$ ./template_alignment data/object_templates.txt data/person.pcd

几秒钟后，您将看到类似于这样的结果：

Best fitness score: 0.000009    |  0.834  0.295  0.466 |R = | -0.336  0.942  0.006 |    | -0.437 -0.162  0.885 |t = < -0.373, -0.097, 0.087 >

您也可以用 pcl_viewer 工具来可视化对齐模板并通过运行以下代码将其覆盖到目标点云上：

$ pcl_viewer data/person.pcd output.pcd

----------------------------------------------------------------
本教程的测试会在之后的博文中介绍