PCL - ICP代碼研讀（二一 ） - TransformationEstimationSVD架構

前言

TransformationEstimationSVD是TransformationEstimation的子類別，提供了estimateRigidTransformation函數的具體實現。

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

TransformationEstimationSVD

/** @b TransformationEstimationSVD implements SVD-based estimation of
 * the transformation aligning the given correspondences.
 *
 * \note The class is templated on the source and target point types as well as on the
 * output scalar of the transformation matrix (i.e., float or double). Default: float.
 * \author Dirk Holz, Radu B. Rusu
 * \ingroup registration
 */
template <typename PointSource, typename PointTarget, typename Scalar = float>
class TransformationEstimationSVD
: public TransformationEstimation<PointSource, PointTarget, Scalar> {
public:

using

定義名稱，方便後續使用：

  using Ptr = shared_ptr<TransformationEstimationSVD<PointSource, PointTarget, Scalar>>;
  using ConstPtr =
      shared_ptr<const TransformationEstimationSVD<PointSource, PointTarget, Scalar>>;

  using Matrix4 =
      typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4;

constructor和destructor

  /** \brief Constructor
   * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/
  // use_umeyama是第三方的軟體?
  TransformationEstimationSVD(bool use_umeyama = true) : use_umeyama_(use_umeyama) {}

  ~TransformationEstimationSVD(){};

估計剛體變換的函數

前四個public的estimateRigidTransformation都是最後一個protected的estimateRigidTransformation函數的wrapper。

  /** \brief Estimate a rigid rotation transformation between a source and a target
   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
   * \param[in] cloud_tgt the target point cloud dataset
   * \param[out] transformation_matrix the resultant transformation matrix
   */
  // estimateRigidTransformation函數的wrapper
  inline void
  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                              const pcl::PointCloud<PointTarget>& cloud_tgt,
                              Matrix4& transformation_matrix) const override;

  /** \brief Estimate a rigid rotation transformation between a source and a target
   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
   * \param[in] indices_src the vector of indices describing the points of interest in
   * \a cloud_src
   * \param[in] cloud_tgt the target point cloud dataset
   * \param[out] transformation_matrix the resultant transformation matrix
   */
  // estimateRigidTransformation函數的wrapper
  inline void
  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                              const pcl::Indices& indices_src,
                              const pcl::PointCloud<PointTarget>& cloud_tgt,
                              Matrix4& transformation_matrix) const override;

  /** \brief Estimate a rigid rotation transformation between a source and a target
   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
   * \param[in] indices_src the vector of indices describing the points of interest in
   * \a cloud_src
   * \param[in] cloud_tgt the target point cloud dataset
   * \param[in] indices_tgt the vector of indices describing the correspondences of the
   * interest points from \a indices_src
   * \param[out] transformation_matrix the resultant transformation matrix
   */
  // estimateRigidTransformation函數的wrapper
  inline 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 override;

  /** \brief Estimate a rigid rotation transformation between a source and a target
   * point cloud using SVD. \param[in] cloud_src the source point cloud dataset
   * \param[in] cloud_tgt the target point cloud dataset
   * \param[in] correspondences the vector of correspondences between source and target
   * point cloud \param[out] transformation_matrix the resultant transformation matrix
   */
  // estimateRigidTransformation函數的wrapper
  void
  estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                              const pcl::PointCloud<PointTarget>& cloud_tgt,
                              const pcl::Correspondences& correspondences,
                              Matrix4& transformation_matrix) const override;

protected版本的estimateRigidTransformation函數接受souce點雲和target點雲的ConstCloudIterator作為輸入，輸出它們之間的轉換矩陣transformation_matrix：

protected:
  /** \brief Estimate a rigid rotation transformation between a source and a target
   * \param[in] source_it an iterator over the source point cloud dataset
   * \param[in] target_it an iterator over the target point cloud dataset
   * \param[out] transformation_matrix the resultant transformation matrix
   */
  // 如果use_umeyama_為true,就是pcl::umeyama函數的wrapper,否則是getTransformationFromCorrelation函數的wrapper
  void
  estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
                              ConstCloudIterator<PointTarget>& target_it,
                              Matrix4& transformation_matrix) const;

getTransformationFromCorrelation

接受去除均值後的矩陣和它們原來的重心作為輸入，套用公式計算剛體變換矩陣。

  /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src
   * * tgt' \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen
   * format \param[in] centroid_src the input source centroid, in Eigen format
   * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
   * \param[in] centroid_tgt the input target cloud, in Eigen format
   * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
   */
  // 輸入的點雲要是Eigen::Matrix,然後要是demean的
  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;

protected成員變數

use_umeyama_表示是否要調用Eigen庫的Umeyama點雲配凖方法。

  bool use_umeyama_;
};