【PCL】教程 template_alignment.cpp将对象模板对齐到点云中

将对象模板对齐到点云中

本教程展示了如何将其他教程中的一些工具组合起来解决更高级的问题——将之前捕获的对象模型对齐到一些新的捕获数据。在这个特定示例中，我们将采用包含人脸的深度图像，并尝试将之前捕获的人脸模板拟合到其中，以确定场景中人脸的位置和方向。

我们可以使用以下代码将人脸模板（蓝点）拟合到新的点云（绿点）中。

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

终端输出：

Best fitness score: 0.000012


    |  0.830  0.351  0.434 |
R = | -0.414  0.909  0.057 |
    | -0.374 -0.227  0.899 |


t = < -0.355, -0.139, 0.090 >

[done, 748.774 ms : 1419 points]
Available dimensions: x y z

output.pcd

pcl_viewer data/person.pcd output.pcd

这段代码主要用于点云数据的特征提取和模板对齐。它使用了PCL(Point Cloud Library)库来处理3D点云数据，并涉及到多个步骤，包括读取点云文件、计算表面法向量和局部特征、以及使用样本一致性初始对齐（SAC-IA）算法进行模板对齐。

功能分析：

1. 特征云（FeatureCloud）类：专门用于处理特征提取相关任务。它可以加载点云数据，计算点云的表面法向量和局部特征（如FPFH特征）。其中，setInputCloud和loadInputCloud方法用于设定处理对象的点云，getPointCloud、getSurfaceNormals和getLocalFeatures方法分别用于获取处理后的点云、表面法向量和局部特征。

2. 模板对齐（TemplateAlignment）类：负责将一组模板点云与目标点云进行对齐。该类使用了PCL库中的SampleConsensusInitialAlignment方法来寻找最佳对齐。通过设定目标云和添加模板云，以及调整SAC-IA算法的参数（如最小样本距离、最大对应距离和迭代次数），实现对齐操作。align方法用于对单个模板进行对齐，alignAll用于对所有模板进行对齐，并通过findBestAlignment方法找到最佳对齐结果。

3. 主函数（main）：是程序的入口点，它先检查命令行参数，读取模板点云文件和目标点云文件。接着对目标点云进行预处理，如移除远处点和降采样。然后，创建FeatureCloud和TemplateAlignment对象，并将目标云和模板云加载进去。最后，找到最佳对齐的模板，并输出对齐得分、旋转矩阵和平移向量。最佳对齐的点云被保存到文件供可视化。

方法概述：

• 点云的读取和预处理：读取PCD文件中的点云数据，并通过过滤和降采样预处理数据以提高处理效率。

• 表面法向量和局部特征的计算：计算点云的表面法向量和FPFH特征，为后续的点云对齐提供依据。

• 点云对齐：使用SAC-IA算法对给定的模板点云与目标点云进行最佳对齐，寻找模板与目标间具有最低对齐误差的变换矩阵。

整个代码流程展示了一种使用PCL处理3D点云数据的方法，从读取点云数据到特征提取，再到模板对齐，体现了3D点云处理的一般步骤和方法。

#include <limits>                  // 包含标准库的数值极限
#include <fstream>                 // 包含文件输入输出流的功能
#include <vector>                  // 包含向量容器的库
#include <Eigen/Core>              // 包含Eigen库核心部分，进行矩阵操作
#include <pcl/memory.h>            // 包含PCL库的内存处理功能
#include <pcl/pcl_macros.h>        // 包含PCL库的宏定义
#include <pcl/point_types.h>       // 包含PCL库的点类型定义
#include <pcl/point_cloud.h>       // 包含PCL库的点云处理功能
#include <pcl/io/pcd_io.h>         // 包含PCL库的PCD文件输入输出功能
#include <pcl/kdtree/kdtree_flann.h> // 包含PCL库的KD树索引构建功能
#include <pcl/filters/passthrough.h> // 包含PCL库的直通过滤器功能
#include <pcl/filters/voxel_grid.h>  // 包含PCL库的体素网格过滤器功能
#include <pcl/features/normal_3d.h>  // 包含PCL库的3D表面法线估计功能
#include <pcl/features/fpfh.h>       // 包含PCL库的FPFH特征计算功能
#include <pcl/registration/ia_ransac.h> // 包含PCL库的RANSAC配准算法功能


// 定义一个用于处理特征点云的类
class FeatureCloud
{
  public:
    // 为几种对象类型定义别名，简化代码
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; // 点云类型
    typedef pcl::PointCloud<pcl::Normal> SurfaceNormals; // 表面法线类型
    typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures; // 本地特征类型
    typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod; // 搜索方法类型（KD树）


    // 构造函数，初始化搜索方法，法线和特征的半径
    FeatureCloud () :
      search_method_xyz_ (new SearchMethod),
      normal_radius_ (0.02f),
      feature_radius_ (0.02f)
    {}


    // 设置输入的点云
    void
    setInputCloud (PointCloud::Ptr xyz)
{
      xyz_ = xyz;
      processInput (); // 处理输入的点云
    }


    // 从PCD文件加载并处理点云
    void
    loadInputCloud (const std::string &pcd_file)
{
      xyz_ = PointCloud::Ptr (new PointCloud);
      pcl::io::loadPCDFile (pcd_file, *xyz_); // 加载PCD文件
      processInput (); // 处理输入的点云
    }


    // 获取点云的指针
    PointCloud::Ptr
    getPointCloud () const
{
      return (xyz_);
    }


    // 获取表面法线的指针
    SurfaceNormals::Ptr
    getSurfaceNormals () const
{
      return (normals_);
    }


    // 获取本地特征的指针
    LocalFeatures::Ptr
    getLocalFeatures () const
{
      return (features_);
    }


  protected:
    // 计算表面法线和本地特征
    void
    processInput ()
{
      computeSurfaceNormals (); // 计算表面法线
      computeLocalFeatures (); // 计算本地特征
    }


    // 计算表面法线
    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_); // 计算法线
    }


    // 计算本地特征描述符
    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_); // 计算特征
    }


  private:
    // 点云数据
    PointCloud::Ptr xyz_;
    SurfaceNormals::Ptr normals_;
    LocalFeatures::Ptr features_;
    SearchMethod::Ptr search_method_xyz_;


    // 参数
    float normal_radius_;
    float feature_radius_;
};

代码定义了一个FeatureCloud类，用于处理点云数据。它可以加载点云数据（从内存或PCD文件）、计算点云的表面法线和本地特征。这是进行3D点云处理和分析中常见的一个步骤，特别适用于机器视觉和机器人技术领域，比如点云的配准、目标识别、环境映射等。通过计算点云的特征来提高其在这些应用中的有效性和准确性。

class TemplateAlignment
{
  public:


    // 用于存储对齐结果的结构
    struct Result
    {
      float fitness_score;                    // 对齐后的适应值得分
      Eigen::Matrix4f final_transformation;   // 对齐后的最终变换矩阵
      PCL_MAKE_ALIGNED_OPERATOR_NEW          // 确保Eigen类型正确对齐
    };


    // 构造函数，初始化样本一致性初始对齐(SAC-IA)算法的参数
    TemplateAlignment () :
      min_sample_distance_ (0.05f),            // 最小样本距离
      max_correspondence_distance_ (0.01f*0.01f), // 最大对应点距离
      nr_iterations_ (500)                    // 最大迭代次数
    {
      sac_ia_.setMinSampleDistance (min_sample_distance_); // 设置最小样本距离
      sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_); // 设置最大对应点距离
      sac_ia_.setMaximumIterations (nr_iterations_); // 设置最大迭代次数
    }


    // 设置目标点云，模板点云将会被对准到这个目标点云上
    void
    setTargetCloud (FeatureCloud &target_cloud)
{
      target_ = target_cloud;
      sac_ia_.setInputTarget (target_cloud.getPointCloud ()); // 设置目标点云
      sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ()); // 设置目标本地特征
    }


    // 向模板列表中添加新的模板点云
    void
    addTemplateCloud (FeatureCloud &template_cloud)
{
      templates_.push_back (template_cloud); // 添加到模板列表
    }


    // 对指定的模板点云执行对齐操作，以setTargetCloud指定的目标为基准
    void
    align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
{
      sac_ia_.setInputSource (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 (); // 获取最终的变换矩阵
    }


    // 对所有通过addTemplateCloud添加的模板点云执行批量对齐操作
    void
    alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)
{
      results.resize (templates_.size ()); // 调整结果向量的大小
      for (std::size_t i = 0; i < templates_.size (); ++i)
      {
        align (templates_[i], results[i]); // 逐个对齐所有模板
      }
    }


    // 对所有模板点云执行对齐，找到最佳的对齐结果
    int
    findBestAlignment (TemplateAlignment::Result &result)
{
      // 对所有模板进行对齐
      std::vector<Result, Eigen::aligned_allocator<Result> > results; // 创建结果向量
      alignAll (results); // 执行对齐


      // 找到最佳的(最低的)适应值得分
      float lowest_score = std::numeric_limits<float>::infinity (); // 初始化最低得分为无穷大
      int best_template = 0; // 最佳模板索引
      for (std::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; // 更新最佳模板索引
        }
      }


      // 输出最佳对齐结果
      result = results[best_template]; // 设置最终结果
      return (best_template); // 返回最佳模板索引
    }


  private:
    // 模板点云列表和目标点云，这些模板将会被对准到目标上
    std::vector<FeatureCloud> templates_;
    FeatureCloud target_;


    // 样本一致性初始对齐(SAC-IA)注册流程及其参数
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
    float min_sample_distance_;
    float max_correspondence_distance_;
    int nr_iterations_;
};

代码定义了一个TemplateAlignment类，主要用于模板点云与目标点云之间的初始对齐。这个类能够添加多个模板点云，设置一个目标点云，并且使用样本一致性初始对齐（SAC-IA）算法来寻找最佳的对齐方式。该类还存储了对齐结果，包括适应值得分和最终的变换矩阵。通过比较不同模板点云的对齐得分，可以找到与目标点云最匹配的模板，这在许多工业、机器人以及3D重建应用中有着重要的作用。

// 将一组对象模板对齐到一个样本点云上的主函数
int
main (int argc, char **argv)
{
  if (argc < 3)
  {
    printf ("No target PCD file given!\n"); // 如果没有给定目标 PCD 文件
    return (-1);
  }


  // 加载在 object_templates.txt 文件中指定的对象模板
  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) == '#') // 跳过空行或注释行
      continue;


    FeatureCloud template_cloud;
    template_cloud.loadInputCloud (pcd_filename); // 加载 PCD 文件
    object_templates.push_back (template_cloud); // 添加到模板列表
  }
  input_stream.close ();


  // 加载目标点云的 PCD 文件
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile (argv[2], *cloud);


  // 预处理点云，包括...
  // ...移除远处的点
  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);


  // ... 以及对点云进行下采样
  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);
  pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud (new pcl::PointCloud<pcl::PointXYZ>); 
  vox_grid.filter (*tempCloud);
  cloud = tempCloud; // 更新处理后的点云


  // 将处理后的点云设置为目标 FeatureCloud
  FeatureCloud target_cloud;
  target_cloud.setInputCloud (cloud);


  // 设置 TemplateAlignment 的输入
  TemplateAlignment template_align;
  for (std::size_t i = 0; i < object_templates.size (); ++i)
  {
    template_align.addTemplateCloud (object_templates[i]); // 添加模板点云
  }
  template_align.setTargetCloud (target_cloud); // 设置目标点云


  // 查找最佳的模板对齐
  TemplateAlignment::Result best_alignment; // 最佳对齐结果
  int best_index = template_align.findBestAlignment (best_alignment); // 最佳索引
  const FeatureCloud &best_template = object_templates[best_index];


  // 打印对齐的适应度分数（值小于0.00002是好的）
  printf ("Best fitness score: %f\n", best_alignment.fitness_score);


  // 打印旋转矩阵和平移向量
  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));
  printf ("    | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
  printf ("\n");
  printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));


  // 保存对齐的模板以供可视化
  pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
  pcl::transformPointCloud (*best_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
  pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud); // 保存转换后的点云到文件


  return (0);
}

代码主要实现了将一组预定义的物体模板（通过PCD文件给出）对齐到一个给定的样本点云上的过程。首先，它从argv[1]指定的文件中加载这些模板，并将它们添加到一个FeatureCloud对象的列表中。接着，它加载目标点云并对其进行预处理，包括PassThrough移除远处的点和下采样处理VoxelGrid以减小点云的密度。之后，这些模板通过template_align对象的各种方法被对齐到目标点云上。最终，程序找到并打印出了最佳对齐模板的适应度分数、旋转矩阵、平移向量，并将对齐后的点云保存到文件中供后续使用或可视化。

std::vector<Result, Eigen::aligned_allocator<Result> > results;

pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;