PCL库源代码阅读—配准模块(Registration)—对应关系模块(Correspondence)

两点云配准需要用到对应点对，那如何建立对应点对呢？pcl库建立对应点对是通过对应关系模块(Correspondence)实现的，对应关系由对应点对的索引和距离组成，即源点云、目标点云索引和距离。pcl库中对应关系模块的文件众多，本文重点阐述与常见的配准算法相关性较高的文件，具体如下：correspondence.h，correspondence_types.h，correspondence_types.hpp，correspondence_estimation.h，correspondence_estimation.hpp。其中，h文件是声明文件，hpp文件是代码的具体实现文件。

文中有任何错误或疑惑的地方可在评论区留言，看到均会回复！！！

 correspondence.h文件

 本文件主要由结构体组成，结构体中定义了对应关系的数据结构形式。Correspondence是一个最基础的对应关系结构体，它定义了查询索引、匹配索引和距离(或权重)。

struct Correspondence
{
  index_t index_query = 0;    定义查询点索引(源点)
  index_t index_match = UNAVAILABLE;    定义匹配点索引(目标点)，-1表示没有找到对应的匹配点
  / 定义了一个联合体 /
  union
  {
    float distance = std::numeric_limits<float>::max();
    float weight;
  };

  inline Correspondence () = default;    构造函数

  / 有参构造及初始化列表/
  inline Correspondence (index_t _index_query, index_t _index_match, float _distance) :
    index_query (_index_query), index_match (_index_match), distance (_distance)
  {}
};
/ 给了别名便于使用 /
using Correspondences = std::vector< pcl::Correspondence, Eigen::aligned_allocator<pcl::Correspondence> >;

 getRejectedQueryIndices()函数用来比较两组对应关系，找出一组对应关系中有而另一组对应关系中没有的查询点索引，比如分析对应关系拒绝过程中哪些查询点被拒绝。

void
getRejectedQueryIndices (const pcl::Correspondences &correspondences_before,
                         const pcl::Correspondences &correspondences_after,
                         Indices& indices,
                         bool presorting_required = true);

 结构体PointCorrespondence3D继承于结构体Correspondence，它用于表示两个不同坐标系中三维点的对应关系。

struct PointCorrespondence3D : public Correspondence
{
  Eigen::Vector3f point1;  //!< The 3D position of the point in the first coordinate frame
  Eigen::Vector3f point2;  //!< The 3D position of the point in the second coordinate frame

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

 结构体PointCorrespondence6D继承于结构体PointCorrespondence3D，它用于表示两个不同坐标系中三维点的对应关系，并且包含了6自由度的变换信息。6自由度的变换信息常见的有欧拉角、旋转矩阵和四元数。

struct PointCorrespondence6D : public PointCorrespondence3D
{
  / 定义了一个三维仿射变换矩阵 /
  Eigen::Affine3f transformation;  //!< The transformation to go from the coordinate system
                                      //!< of point2 to the coordinate system of point1

  PCL_MAKE_ALIGNED_OPERATOR_NEW
};

 通过距离判断哪个对应关系更优。

inline bool
isBetterCorrespondence (const Correspondence &pc1, const Correspondence &pc2)
{
  return (pc1.distance > pc2.distance);
}

correspondence_types.h文件

 命名空间嵌套，pcl::registration，注意与配准模块(Registration)区分，前者是小写。

namespace pcl {
namespace registration {...}    命名空间
}

本文件较为简单，实现了获取对应关系中查询索引、匹配索引和距离的平均值和标准差。

/ 获得对应关系中距离的平均值和标准差 /
inline void
getCorDistMeanStd(const pcl::Correspondences& correspondences,
                  double& mean,
                  double& stddev);

/ 获得对应关系中的查询索引(源点索引) /
inline void
getQueryIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices);

/ 获得对应关系中的匹配索引(目标点索引) /
inline void
getMatchIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices);

correspondence_types.hpp文件

 本文件具体实现了correspondence_types.h文件中函数的功能。

inline void
getCorDistMeanStd(const pcl::Correspondences& correspondences,
                  double& mean,
                  double& stddev)
{
  if (correspondences.empty())    检查是否为空
    return;

  double sum = 0, sq_sum = 0;     sum：距离求和，sq_sum：距离平方求和

  for (const auto& correspondence : correspondences) {
    sum += correspondence.distance;
    sq_sum += correspondence.distance * correspondence.distance;
  }
  mean = sum / static_cast<double>(correspondences.size());距离求平均
  double variance = (sq_sum - sum * sum / static_cast<double>(correspondences.size())) /
                    static_cast<double>(correspondences.size() - 1);
  stddev = sqrt(variance);    距离的标准差
}

inline void
getQueryIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices)
{
  indices.resize(correspondences.size());
  for (std::size_t i = 0; i < correspondences.size(); ++i)    遍历存储所有查询点索引
    indices[i] = correspondences[i].index_query;
}

inline void
getMatchIndices(const pcl::Correspondences& correspondences, pcl::Indices& indices)
{
  indices.resize(correspondences.size());
  for (std::size_t i = 0; i < correspondences.size(); ++i)    遍历存储所有目标点索引
    indices[i] = correspondences[i].index_match;
}

correspondence_estimation.h文件

上述部分是本部分对应关系估计的基础，该部分将在点云配准算法中频繁使用。 

命名空间嵌套，pcl::registration，注意与配准模块(Registration)区分，前者是小写。 

namespace pcl {
namespace registration {...}    命名空间
}

CorrespondenceEstimationBase类继承于PCLBase类，它是对应关系估计的基类，所有对应关系估计的具体方法均继承于该类。PCLBase类的详细信息可以参考以下链接：PCL库源代码阅读—配准模块(Registration)—Registration类

template <typename PointSource, typename PointTarget, typename Scalar = float>
class CorrespondenceEstimationBase : public PCLBase<PointSource> {
public:	
  using ...                             引用各种变量，方便本类使用
  CorrespondenceEstimationBase()：…		构造函数及初始化列表
  ...
}

本部分为了快速阅读，简化了函数的书写方式，不常用的函数本部分将略去。

实际使用时一般只需关注源点云和目标点云的设置。

setInputSource(&);						设置输入点云
setInputTarget(&);						设置目标点云
setIndicesSource(&);					设置源点云索引
setIndicesTarget(&);					设置目标点云索引
setSearchMethodTarget(&);               设置目标点云的搜索方法
setSearchMethodSource(&);               设置源点云的搜索方法
				初始化列表已经设置，也可以用上述函数单独设置搜索方法

两个纯虚函数是本类的核心函数，需注意的是要区分对应关系和相互对应关系，以最近邻点为原则建立对应关系为例。

对应关系：为给定的源点云点在目标点云中搜索最近邻点，将该点在源点云中的索引作为查询索引，将目标点云中搜索到的最近邻点的索引作为匹配索引，将两点间的距离作为对应关系的距离，然后将三者存储为一个对应关系。

相互对应关系：以源点云中的a作为查询点和目标点云中的b建立了对应关系，反过来，以目标点云中的b作为查询点和源点云中的a也建立了对应关系，此时就说a和b建立了相互对应关系。

/ 纯虚函数，子类具体实现，用来确定源点云和目标点云间的对应关系并输出 /
virtual void
determineCorrespondences(
    pcl::Correspondences& correspondences,
    double max_distance = std::numeric_limits<double>::max()) = 0;

/ 纯虚函数，子类具体实现，用来确定源点云和目标点云间的相互对应关系并输出 /
virtual void
determineReciprocalCorrespondences(
    pcl::Correspondences& correspondences,
    double max_distance = std::numeric_limits<double>::max()) = 0;

本部分解释了protected的变量定义和函数定义。

需要解释的是定义源点云的搜索方法变量是为了建立相互对应关系。

初始化函数是继承于PCLBase类并重写了，它实现了为搜索方法tree_和tree_reciprocal_设置数据源。

protected:
string corr_name_;						  定义对应关系估计的方法名
KdTreePtr tree_;						  定义目标点云的搜索方法
KdTreeReciprocalPtr tree_reciprocal_;	  定义源点云的搜索方法
PointCloudTargetConstPtr target_;		  定义目标点云指针变量
IndicesPtr target_indices_;				  定义目标点云索引指针变量
PointRepresentationConstPtr point_representation_;    定义点表示法
PointCloudTargetPtr input_transformed_;	  定义变换后的源点云
vector<pcl::PCLPointField> input_fields_; 定义源点云的字段信息

inline const std::string&
getClassName() const                      获取对应关系估计的方法名
{
  return (corr_name_);
}

bool initCompute();					初始化
注：此函数仅更新目标点云的kd-tree
bool initComputeReciprocal();		相互对应关系时的初始化
注：此函数仅更新源点云的kd-tree
注：找对应关系前需要该步骤进行初始化，会检查点云是否为空，为tree传递需搜索的点云数据

/ 无需特别关注以下变量 /
bool target_cloud_updated_;
bool source_cloud_updated_;
						避免源点云、目标点云更新时，重建kd-tree
bool force_no_recompute_;				标识符，目标点云的树操作将不会进行
bool force_no_recompute_reciprocal_;	标识符，源点云的树操作将不会进行

 CorrespondenceEstimation类继承于CorrespondenceEstimationBase类，该类实现了确定查询点和匹配点的对应关系，具体是使用KD-tree的搜索方法建立最近邻点对。

class CorrespondenceEstimation
: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
public:	
    using ...                           各种变量，方便本类使用
    CorrespondenceEstimation ():...		构造函数及初始化corr_name_变量
    ...
}

 CorrespondenceEstimation类将基类中的纯虚函数进行了重写，实现了对应关系和相互对应关系的确定，具体的代码逻辑实现在下部分进行详细介绍。

/ 确定对应关系 /
void determineCorrespondences(
    pcl::Correspondences& correspondences,
    double max_distance = std::numeric_limits<double>::max()) override;
/ 确定相互对应关系 /
void determineReciprocalCorrespondences(
    pcl::Correspondences& correspondences,
    double max_distance = std::numeric_limits<double>::max()) override;

correspondence_estimation.hpp文件

determineCorrespondences()函数和determineReciprocalCorrespondences()函数是对应关系模块的核心函数，它用来确定两点云的对应关系。

首先介绍determineCorrespondences()函数，用来确定对应关系。

template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences(
    pcl::Correspondences& correspondences, double max_distance)
{...}

if (!initCompute())        初始化
  return;

double max_dist_sqr = max_distance * max_distance;    距离阈值的平方

correspondences.resize(indices_->size());    调整大小

pcl::Indices index(1);                       定义索引变量
std::vector<float> distance(1);              定义存储距离的变量
pcl::Correspondence corr;                    定义对应关系变量
unsigned int nr_valid_correspondences = 0;   定义满足距离阈值的对应关系的数量变量

/ 两点云的数据类型相同时 /
if (isSamePointType<PointSource, PointTarget>()) {
  / 遍历源点云索引 /
  for (const auto& idx : (*indices_)) {
    tree_->nearestKSearch((*input_)[idx], 1, index, distance);    搜索该索引处的最近邻点
    if (distance[0] > max_dist_sqr)        不满足距离阈值时跳出本次循环
      continue;

    / 满足距离阈值时将信息赋予correspondences[] /
    corr.index_query = idx;                查询索引
    corr.index_match = index[0];           匹配索引
    corr.distance = distance[0];           距离
    correspondences[nr_valid_correspondences++] = corr;
  }
}
else {
  PointTarget pt;                          定义一个PointTarget类型的点

  / 遍历源点云索引 /
  for (const auto& idx : (*indices_)) {
    / 将该索引处的源点复制给pt /
    copyPoint((*input_)[idx], pt);

    tree_->nearestKSearch(pt, 1, index, distance);    搜索该点处的最近邻点
    if (distance[0] > max_dist_sqr)                   同上
      continue;

    corr.index_query = idx;
    corr.index_match = index[0];
    corr.distance = distance[0];
    correspondences[nr_valid_correspondences++] = corr;
  }
}
correspondences.resize(nr_valid_correspondences);     调整数量大小
deinitCompute();

本部分介绍determineReciprocalCorrespondences()函数，用来确定相互对应关系。

template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
    determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
                                       double max_distance)
{...}

if (!initCompute())        初始化
  return;
if (!initComputeReciprocal())
  return;

double max_dist_sqr = max_distance * max_distance;    距离阈值的平方

correspondences.resize(indices_->size());    调整大小
pcl::Indices index(1);                       定义索引变量
std::vector<float> distance(1);              定义存储距离的变量
pcl::Indices index_reciprocal(1);            定义存储相互对应关系的索引变量
std::vector<float> distance_reciprocal(1);   定义存储相互对应关系的距离变量
pcl::Correspondence corr;                    定义对应关系变量
unsigned int nr_valid_correspondences = 0;   定义满足距离阈值的对应关系的数量变量
int target_idx = 0;                          定义存储匹配索引的变量，为了后续确定相互对应关系用

/ 两点云的数据类型相同时 /
if (isSamePointType<PointSource, PointTarget>()) {
  / 遍历源点云索引 /
  for (const auto& idx : (*indices_)) {
    tree_->nearestKSearch((*input_)[idx], 1, index, distance);    搜索该索引在目标点云中的最近邻点
    if (distance[0] > max_dist_sqr)        不满足距离阈值时跳出本次循环
      continue;

    target_idx = index[0];                 将搜索到的匹配索引赋予target_idx变量

    tree_reciprocal_->nearestKSearch(      为匹配索引搜索源点云中的最近邻点
        (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
    / 不满足距离阈值或者反过来搜索到的源点不是该遍历的源点，此时就不是相互对应关系 /
    if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
      continue;

    / 满足距离阈值时将信息赋予correspondences[] /
    corr.index_query = idx;
    corr.index_match = index[0];
    corr.distance = distance[0];
    correspondences[nr_valid_correspondences++] = corr;
  }
}
else {
  PointTarget pt_src;                      定义一个PointTarget类型的点
  PointSource pt_tgt;                      定义一个PointSource类型的点

  / 遍历源点云索引 /
  for (const auto& idx : (*indices_)) {

    / 将该索引处的源点复制给pt /
    copyPoint((*input_)[idx], pt_src);

    tree_->nearestKSearch(pt_src, 1, index, distance);        同上
    if (distance[0] > max_dist_sqr)
      continue;
    
    target_idx = index[0];                 存储搜索到的目标索引

    / 将该索引处的目标点复制给pt /
    copyPoint((*target_)[target_idx], pt_tgt);

    tree_reciprocal_->nearestKSearch(
        pt_tgt, 1, index_reciprocal, distance_reciprocal);    同上
    if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
      continue;

    corr.index_query = idx;
    corr.index_match = index[0];
    corr.distance = distance[0];
    correspondences[nr_valid_correspondences++] = corr;
  }
}
correspondences.resize(nr_valid_correspondences);
deinitCompute();

 总结

 对应关系模块可以用来确定两点云中满足最近邻原则的对应点对，为后续的两点云配准提供基础。该模块中对应关系的数据形式是结构体，核心函数是determineCorrespondences()和determineReciprocalCorrespondences()，该函数可以单独使用于读者所编写的程序中，实现二次开发的目的。