PCL - ICP代碼研讀（二二 ） - TransformationEstimationSVD實現

PCL - ICP代碼研讀（二二 ） - TransformationEstimationSVD實現

前言

TransformationEstimationSVD類別中有五個estimateRigidTransformation函數，其中四個是public的，另一個是protected的。前四個public的estimateRigidTransformation都是protected的estimateRigidTransformation的wrapper。

本篇對應到transformation_estimation_svd.hpp這個檔案。

estimateRigidTransformation的wrapper

以下這幾個函數都是estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4& transformation_matrix)這個protected函數的wrapper。它們會將各自的輸入都轉為ConstCloudIterator後，再調用protected版本的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) {
    PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number "
              "or points in source (%zu) differs than target (%zu)!\n",
              static_cast<std::size_t>(nr_points),
              static_cast<std::size_t>(cloud_tgt.size()));
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
  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
{
  // source點雲索引的數量必須與target點雲點數一致
  if (indices_src.size() != cloud_tgt.size()) {
    PCL_ERROR("[pcl::TransformationSVD::estimateRigidTransformation] Number or points "
              "in source (%zu) differs than target (%zu)!\n",
              indices_src.size(),
              static_cast<std::size_t>(cloud_tgt.size()));
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
  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()) {
    PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number "
              "or points in source (%zu) differs than target (%zu)!\n",
              indices_src.size(),
              indices_tgt.size());
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
  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建構子會依據最後一個參數(bool source)來決定要使用Correspondence的index_query或index_match
  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

estimateRigidTransformation

這個函數用於求解Procrustes Transformation Problem。首先計算source點雲和target點雲的重心，接着對兩點雲做demean，然後將demean後的點雲和它們原來的重心一同傳入getTransformationFromCorrelation，計算得到transformation_matrix。

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
{
  // Convert to Eigen format
  const int npts = static_cast<int>(source_it.size());

根據use_umeyama_這個成員變數是否為true，此處會分為兩個分支。

在use_umeyama_為true的情況下，會遍歷輸入的點雲迭代器source_it和target_it建立Eigen::Matrix矩陣，然後調用pcl::umeyama計算它們之間的轉換矩陣：

  // umeyama:一種點雲配凖的方法,在Eigen中有實現
  if (use_umeyama_) {
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src(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還是Eigen版本?
    // Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
    // 第三個參數是with_scaling,表示可以同時估計尺度縮放程度
    transformation_matrix = pcl::umeyama(cloud_src, cloud_tgt, false);
  }

在use_umeyama_為false的情況下，則是套用ICP（Iterative Closest Point）算法推導所介紹的公式來求解。

首先將迭代器的指標移到起始位置，並將transformation_matrix設為單位矩陣：

  else {
    // 把迭代器的指標移到begin處
    source_it.reset();
    target_it.reset();
    // <cloud_src,cloud_src> is the source dataset
    // transformation_matrix: Eigen::Matrix<Scalar, 4, 4>
    // Eigen::Matrix::setIdentity: 設為單位矩陣
    transformation_matrix.setIdentity();

計算source點雲和target點雲的重心，得到centroid_src和centroid_tgt：

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    // Estimate the centroids of source, target
    // common/include/pcl/common/impl/centroid.hpp
    // 如果傳入的第一個參數是ConstCloudIterator<PointT>,則會從迭代器所指向的地方開始計算
    compute3DCentroid(source_it, centroid_src);
    compute3DCentroid(target_it, centroid_tgt);
    source_it.reset();
    target_it.reset();

將點雲去除均值，得到centroid_src_demean和cloud_tgt_demean：

    // Subtract the centroids from source, target
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean,
        cloud_tgt_demean;
    // 如果傳入的第一個參數是ConstCloudIterator<PointT>,則會從迭代器所指向的地方開始計算
    /**
     * 參看centroid.hpp中的demeanPointCloud,
     * 可發現cloud_src_demean的型別為Eigen::Matrix<Scalar, 4, Eigen::Dynamic>
     **/
    demeanPointCloud(source_it, centroid_src, cloud_src_demean);
    demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean);

getTransformationFromCorrelation函數接受demean後的點雲cloud_src_demean及cloud_tgt_demean和它們原來的重心作為參數，計算source點雲和target點雲之間的轉換矩陣並填入transformation_matrix中。

    getTransformationFromCorrelation(cloud_src_demean,
                                     centroid_src,
                                     cloud_tgt_demean,
                                     centroid_tgt,
                                     transformation_matrix);
  }
}

getTransformationFromCorrelation

getTransformationFromCorrelation函數是ICP算法的核心所在，這部分的代碼建議與ICP（Iterative Closest Point）算法推導對照參看。

這個函數接受demean（移除平均值）後的點雲cloud_src_demean及cloud_tgt_demean和它們原來的重心作為參數，它會套用公式計算旋轉矩陣和平移向量，並填入輸出的轉換矩陣transformation_matrix中。

// Procrustes Transformation Problem求解
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設為單位矩陣：

  /**
   * Procrustes Transformation Problem求解
   * A,B: 3*N matrix
   * B'A'^T = U * Sigma * V^T
   * R* = U * V^T
   * t* = (1/N)(B-RA)1 = B_centroid - R * A_centroid
   * 
   * 這裡(A,B互換,U,V也跟着互換?):
   * A2,B2: 4*N matrix
   * A2'B2'^T = U2 * Simga2 * V2^T
   * R* = V * U^T
   * t* = (1/N)(B-RA)1 = B_centroid - R * A_centroid
   **/
  transformation_matrix.setIdentity();

在ICP（Iterative Closest Point）算法推導中，是對$B^{\prime }{A}^{\prime T}$（$A^{\prime }$為demean後的source點雲，$B^{\prime }$為demean後的target點雲）做矩陣分解，得到$U\Sigma V^T$。在此處的代碼中，則是對$A^{\prime }{B}^{\prime T}$做矩陣分解，因為$A^{\prime }{B}^{\prime T}=(B^{\prime }{A}^{\prime T}{)}^T=(U\Sigma V^T{)}^T=V\Sigma U^T$。所以此處的$U,V$與推導中的$U,V$是互換的。

  /**
   * 因為輸入的點雲cloud_src_demean與cloud_tgt_demean都已經去除平均值,
   * 所以這裡只關注旋轉部分
   **/
  // Assemble the correlation matrix H = source' * target'
  // prime: demean
  Eigen::Matrix<Scalar, 3, 3> H =
      (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3);

  // Compute the Singular Value Decomposition
  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();

參考Euler’s theorem (rotation)，不帶反射的旋轉矩陣的判別式必須為1：

The matrix with positive determinant is a proper rotation and with a negative determinant an improper rotation (is equal to a reflection times a proper rotation).

所以有如下代碼：

  // Compute R = V * U'
  if (u.determinant() * v.determinant() < 0) {
    // 把v的最後一個column乘上-1?
    for (int x = 0; x < 3; ++x)
      v(x, 2) *= -1;
  }

在ICP（Iterative Closest Point）算法推導中，是取$R=U{V}^T$，因為這裡的$U,V$互換，所以代碼中是寫成$V{U}^T$：

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose();

由ICP（Iterative Closest Point）算法推導可知，取平移向量$t=\frac{1}{N}(B-RA)1$，也就是target點雲的重心減去旋轉矩陣$R$乘上source點雲的重心，可使損失函數最小化：

  // Return the correct transformation
  transformation_matrix.topLeftCorner(3, 3) = R;
  // 平移部分:先對src點雲的重心做旋轉,然後計算它與tgt點雲重心間的位移
  const Eigen::Matrix<Scalar, 3, 1> Rc(R * centroid_src.head(3));
  transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc;
}