PCL库源代码阅读—配准模块(Registration)—变换估计(transformation_estimation)

想躺平的点云工程师

于 2024-05-26 19:45:00 发布

阅读量607

点赞数 29

文章标签：自动驾驶人工智能

本文链接：https://blog.csdn.net/mazheng11111111/article/details/138494253

版权

pcl库中变换估计模块(transformation_estimation)用来计算两点云配准的变换矩阵，不同的原理对应不同的文件，本文主要围绕经典ICP算法介绍相关的变换估计文件，具体为：transformation_estimation.h，transformation_estimation_svd.h，transformation_estimation_svd.hpp。其中，h文件是声明文件，hpp文件是代码的具体实现文件。

本文可结合经典ICP算法阅读：PCL库源代码阅读—配准模块(Registration)—ICP算法

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

transformation_estimation.h文件

本文件所定义的类是变换估计的基类，它使用的前提是两点云的对应关系已经确定。

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

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

TransformationEstimation类是所有变换估计方法的基类，它求解变换矩阵基于两点云间的对应关系已经确定。它本身是一个抽象类，具体功能需要子类实现。

template <typename PointSource, typename PointTarget, typename Scalar = float>
class TransformationEstimation {
    ...
}

下列函数均是为了计算变换矩阵，且均为纯虚函数，具体功能的实现由子类完成，4个函数的区别在于参数的不同。读者如需了解对应关系的相关内容，可参考：PCL库源代码阅读—配准模块(Registration)—对应关系模块(Correspondence)

/ 估计刚体变换矩阵，参数为源点云、目标点云数据集，变换矩阵 /
virtual void
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                            const pcl::PointCloud<PointTarget>& cloud_tgt,
                            Matrix4& transformation_matrix) const = 0;
 / 参数多了源点云中的查询索引 /
 virtual void
 estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                             const pcl::Indices& indices_src,
                             const pcl::PointCloud<PointTarget>& cloud_tgt,
                             Matrix4& transformation_matrix) const = 0;
/ 参数多了对应关系的查询索引和匹配索引 /
virtual void
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                            const pcl::Indices& indices_src,
                            const pcl::PointCloud<PointTarget>& cloud_tgt,
                            const pcl::Indices& indices_tgt,
                            Matrix4& transformation_matrix) const = 0;
/ 参数为源点云和目标点云的对应关系，最为常用 /
virtual void
estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                            const pcl::PointCloud<PointTarget>& cloud_tgt,
                            const pcl::Correspondences& correspondences,
                            Matrix4& transformation_matrix) const = 0;

transformation_estimation_svd.h文件

本文件基于SVD的方法计算变换矩阵，前提是已经确定了两点云的对应关系。

TransformationEstimationSVD类继承于TransformationEstimation类，实现了基于SVD方法的变换矩阵估计。estimateRigidTransformation()函数所实现的功能在上部分已经介绍，代码的详细解读见transformation_estimation_svd.hpp文件部分。

class TransformationEstimationSVD
: public TransformationEstimation<PointSource, PointTarget, Scalar> {
    ...
}

protected:
  / 参数为源点云、目标点云的迭代器 /
  void
  estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
                              ConstCloudIterator<PointTarget>& target_it,
                              Matrix4& transformation_matrix) const;

  / 计算基于SVD方法的变换矩阵
    cloud_src_demean：去均值处理的源点云        centroid_src：源点云的质心
    cloud_tgt_demean：去均值处理的目标点云      centroid_tgt：目标点云质心 /
  virtual void
  getTransformationFromCorrelation(
      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
      const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
      const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
      const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
      Matrix4& transformation_matrix) const;

  bool use_umeyama_;        是否使用第三方的umeyama方法计算变换矩阵

transformation_estimation_svd.hpp文件

本文件具体实现了transformation_estimation_svd.h文件中函数的功能，其中最为常用的是以对应关系为参数计算变换矩阵的函数。

由于变换矩阵估计的函数都以点云迭代器为参数的estimateRigidTransformation函数为基础，因此先介绍该函数，其余函数按顺序进行解读。

点云迭代器的使用类似于C++中容器的迭代器。

具体计算变换矩阵T的代码有两种，一种是调用第三方Eigen库中的umeyama函数实现，这是默认的方法，另一种是自己实现。

/ 参数为点云迭代器 /
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
                                ConstCloudIterator<PointTarget>& target_it,
                                Matrix4& transformation_matrix) const
{
  / 将格式转换为第三方的Eigen格式 /
  const int npts = static_cast<int>(source_it.size());    存储源点云的数量

  / use_umeyama_默认为true /
  if (use_umeyama_) {        
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src(3, npts);    定义3*npts大小的矩阵
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt(3, npts);

    / 遍历点云并赋值 /
    for (int i = 0; i < npts; ++i) {
      cloud_src(0, i) = source_it->x;
      cloud_src(1, i) = source_it->y;
      cloud_src(2, i) = source_it->z;
      ++source_it;

      cloud_tgt(0, i) = target_it->x;
      cloud_tgt(1, i) = target_it->y;
      cloud_tgt(2, i) = target_it->z;
      ++target_it;
    }

    / 调用pcl::umeyama函数计算基于SVD方法的变换矩阵，该函数是Eigen库中的 /
    transformation_matrix = pcl::umeyama(cloud_src, cloud_tgt, false);
  }
  / 本部分是自己实现基于SVD方法计算变换矩阵的代码 /
  else {
    source_it.reset();        重置迭代器
    target_it.reset();
    
    transformation_matrix.setIdentity();    设置为单位矩阵

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;    存储质心坐标，4*1维
    
    compute3DCentroid(source_it, centroid_src);    计算点云的三维质心
    compute3DCentroid(target_it, centroid_tgt);
    source_it.reset();        重置迭代器
    target_it.reset();

    / 动态矩阵用来存储去均值后的点云数据 /
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
        cloud_tgt_demean;
    demeanPointCloud(source_it, centroid_src, cloud_src_demean);    去均值处理，即减去质心坐标
    demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);

    / 基于SVD方法计算变换矩阵T /
    getTransformationFromCorrelation(cloud_src_demean,
                                     centroid_src,
                                     cloud_tgt_demean,
                                     centroid_tgt,
                                     transformation_matrix);
  }
}

getTransformationFromCorrelation()函数实现了基于SVD方法计算变换矩阵T的功能，前提是需要去均值处理后的点云数据和点云质心。

/ 具体计算变换矩阵T /
template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
    getTransformationFromCorrelation(
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_src_demean,
        const Eigen::Matrix<Scalar, 4, 1>& centroid_src,
        const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& cloud_tgt_demean,
        const Eigen::Matrix<Scalar, 4, 1>& centroid_tgt,
        Matrix4& transformation_matrix) const
{
  transformation_matrix.setIdentity();    设置为单位矩阵

  / 定义H = source * target'，取左上角3*3的矩阵块，因为去均值处理后的点云是4*n维 /
  Eigen::Matrix<Scalar, 3, 3> H =
      (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);

  / 对H进行SVD分解 /
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3>> svd(
      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU();
  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV();

  / 修正分解得到的矩阵，确保是正交矩阵 /
  if (u.determinant() * v.determinant() < 0) {
    for (int x = 0; x < 3; ++x)
      v(x, 2) *= -1;
  }

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();    计算旋转矩阵R

  transformation_matrix.topLeftCorner(3, 3) = R;    赋值操作，更新旋转矩阵部分
  / 根据 t = q - R*p 原理计算平移矩阵 /
  const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3));
  transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc;

  / 调试信息，不用管 /
  if (pcl::console::isVerbosityLevelEnabled(pcl::console::L_DEBUG)) {
    size_t N = cloud_src_demean.cols();
    ...
  }
}

上述两个函数是本文件的核心函数，其余不同参数的estimateRigidTransformation()函数都是在此基础上实现的。

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                Matrix4& transformation_matrix) const
{
  const auto nr_points = cloud_src.size();    存储源点云的数量
  if (cloud_tgt.size() != nr_points) {        源点云数量和目标点云数量不一致时终止程序
    ...
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src);    定义常量点云迭代器
  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
  / 计算变换矩阵T /
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::Indices& indices_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                Matrix4& transformation_matrix) const
{
  / 数量不一致时终止程序 /
  if (indices_src.size() != cloud_tgt.size()) {
    ...
    return;
  }
 
  / 定义常量点云迭代器，用于遍历特定索引范围内的点 /
  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
  / 计算变换矩阵T /
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::Indices& indices_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                const pcl::Indices& indices_tgt,
                                Matrix4& transformation_matrix) const
{
  / 数量不一致时终止程序 /
  if (indices_src.size() != indices_tgt.size()) {
    ...
    return;
  }

  / 定义常量点云迭代器，用于遍历特定索引范围内的点 /
  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
  / 计算变换矩阵T /
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimationSVD<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                const pcl::Correspondences& correspondences,
                                Matrix4& transformation_matrix) const
{
  / 定义常量点云迭代器，用来遍历与对应关系相关联的点 /
  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
  / 计算变换矩阵T /
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

总结

本文主要围绕经典ICP算法介绍了相关的变换估计文件，其中transformation_estimation.h是变换估计的基文件，定义了计算变换矩阵T的抽象函数，transformation_estimation_svd.h文件实现了基于SVD方法计算变换矩阵T的功能。

想躺平的点云工程师

关注

29
点赞
踩
11

收藏

觉得还不错? 一键收藏
打赏
0
评论
PCL库源代码阅读—配准模块(Registration)—变换估计(transformation_estimation)

pcl库中变换估计模块(transformation_estimation)用来计算两点云配准的变换矩阵，不同的原理对应不同的文件，本文主要围绕经典ICP算法介绍相关的变换估计文件，具体为：transformation_estimation.h，transformation_estimation_svd.h，transformation_estimation_svd.hpp。其中，h文件是声明文件，hpp文件是代码的具体实现文件。
复制链接

扫一扫