PCL - ICP代碼研讀（六） - IterativeClosestPoint架構

- 前言
- using
- public成員函數
- - constructor和destructor
  - copy constructor
  - destructor
  - getConvergeCriteria
  - setInputSource
  - setInputTarget
  - use_reciprocal_correspondence_的setter和getter
- protected成員函數
- protected成員變數

前言

icp.h

裡宣告了

IterativeClosestPoint

和

IterativeClosestPointWithNormals

兩個類別，他們兩個都是

Registration

的子類別。

本篇隻對

IterativeClosestPoint

類別著墨。

using

template <typename PointSource, typename PointTarget, typename Scalar = float>
class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> {
public:

用

using

來定義名稱，友善後續使用。

using PointCloudSource =
      typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

  using PointCloudTarget =
      typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

  using PointIndicesPtr = PointIndices::Ptr;
  using PointIndicesConstPtr = PointIndices::ConstPtr;

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

沿用

Registration

中的定義，同時將原來是

Registration

的

protected

成員的導入

IterativeClosestPoint

的

public

權限區塊。

using Registration<PointSource, PointTarget, Scalar>::reg_name_;
  using Registration<PointSource, PointTarget, Scalar>::getClassName;
  using Registration<PointSource, PointTarget, Scalar>::input_;
  using Registration<PointSource, PointTarget, Scalar>::indices_;
  using Registration<PointSource, PointTarget, Scalar>::target_;
  using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
  using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
  using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
  using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
  using Registration<PointSource, PointTarget, Scalar>::transformation_;
  using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
  using Registration<PointSource, PointTarget, Scalar>::
      transformation_rotation_epsilon_;
  using Registration<PointSource, PointTarget, Scalar>::converged_;
  using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
  using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
  using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_;
  using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
  using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
  using Registration<PointSource, PointTarget, Scalar>::correspondences_;
  using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
  using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
  using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;

定義一個public成員變數

convergence_criteria_

，表示當前算法收斂的情況：

typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
      convergence_criteria_;

沿用

Registration

中定義好的類別名稱

Matrix4

：

// 怎麼不寫using Registration<PointSource, PointTarget, Scalar>::Matrix4;?
  using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;

public成員函數

constructor和destructor

/** \brief Empty constructor. */
  IterativeClosestPoint()
  : x_idx_offset_(0)
  , y_idx_offset_(0)
  , z_idx_offset_(0)
  , nx_idx_offset_(0)
  , ny_idx_offset_(0)
  , nz_idx_offset_(0)
  , use_reciprocal_correspondence_(false)
  , source_has_normals_(false)
  , target_has_normals_(false)
  {
    reg_name_ = "IterativeClosestPoint";
    transformation_estimation_.reset(
        new pcl::registration::
            TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
    correspondence_estimation_.reset(
        new pcl::registration::
            CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
    convergence_criteria_.reset(
        new pcl::registration::DefaultConvergenceCriteria<Scalar>(
            nr_iterations_, transformation_, *correspondences_));
  };

其中

transformation_estimation_

，

correspondence_estimation_

及

convergence_criteria_

皆是shared_ptr，調用

std::shared_ptr::reset

來替換他們所管理的物件。

注：

shared_ptr

相關的知識，可以通過C++ std::shared_ptr 用法與範例學習一下

copy constructor

目前

IterativeClosestPoint

的copy constructor是處於禁用的狀態：

/**
   * \brief Due to `convergence_criteria_` holding references to the class members,
   * it is tricky to correctly implement its copy and move operations correctly. This
   * can result in subtle bugs and to prevent them, these operations for ICP have
   * been disabled.
   *
   * \todo: remove deleted ctors and assignments operations after resolving the issue
   */
  // 複製,移動IterativeClosestPoint會造成問題,是以目前先禁止使用
  IterativeClosestPoint(const IterativeClosestPoint&) = delete;
  IterativeClosestPoint(IterativeClosestPoint&&) = delete;
  IterativeClosestPoint&
  operator=(const IterativeClosestPoint&) = delete;
  IterativeClosestPoint&
  operator=(IterativeClosestPoint&&) = delete;

destructor

/** \brief Empty destructor */
  ~IterativeClosestPoint() {}

getConvergeCriteria

public成員變數

convergence_criteria_

的getter：

//注釋?
  /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the
   * IterativeClosestPoint class. This allows to check the convergence state after the
   * align() method as well as to configure DefaultConvergenceCriteria's parameters not
   * available through the ICP API before the align() method is called. Please note that
   * the align method sets max_iterations_, euclidean_fitness_epsilon_ and
   * transformation_epsilon_ and therefore overrides the default / set values of the
   * DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's
   * DefaultConvergenceCriteria.
   */
  inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
  getConvergeCriteria()
  {
    return convergence_criteria_;
  }

setInputSource

設定好source點雲本身，XYZ坐標偏移量

x_idx_offset_

y_idx_offset_

z_idx_offset_

，法向量XYZ坐標偏移量

nx_idx_offset_

ny_idx_offset_

nz_idx_offset_

及

source_has_normals_

。這些變數在

transformCloud

函數中會用到。

/**
   * 設定好source點雲及x_idx_offset_,y_idx_offset_,z_idx_offset_,
   * nx_idx_offset_,ny_idx_offset_,nz_idx_offset_及source_has_normals_
   **/
  // 這個函數在registration.h中是virtual的
  /** \brief Provide a pointer to the input source
   * (e.g., the point cloud that we want to align to the target)
   *
   * \param[in] cloud the input point cloud source
   */
  void
  setInputSource(const PointCloudSourceConstPtr& cloud) override
  {
    Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
    const auto fields = pcl::getFields<PointSource>();
    source_has_normals_ = false;
    for (const auto& field : fields) {
      if (field.name == "x")
        x_idx_offset_ = field.offset;
      else if (field.name == "y")
        y_idx_offset_ = field.offset;
      else if (field.name == "z")
        z_idx_offset_ = field.offset;
      else if (field.name == "normal_x") {
        source_has_normals_ = true;
        nx_idx_offset_ = field.offset;
      }
      else if (field.name == "normal_y") {
        source_has_normals_ = true;
        ny_idx_offset_ = field.offset;
      }
      else if (field.name == "normal_z") {
        source_has_normals_ = true;
        nz_idx_offset_ = field.offset;
      }
    }
  }

setInputTarget

設定好target點雲本身及

target_has_normals_

。

target_has_normals_

在

transformCloud

函數中會用到。

//設定好target點雲及target_has_normals_
  //沒有x_idx_offset_,nx_idx_offset_之類的?
  // 這個函數在registration.h中是virtual的
  /** \brief Provide a pointer to the input target
   * (e.g., the point cloud that we want to align to the target)
   *
   * \param[in] cloud the input point cloud target
   */
  void
  setInputTarget(const PointCloudTargetConstPtr& cloud) override
  {
    Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
    const auto fields = pcl::getFields<PointSource>();
    target_has_normals_ = false;
    for (const auto& field : fields) {
      if (field.name == "normal_x" || field.name == "normal_y" ||
          field.name == "normal_z") {
        target_has_normals_ = true;
        break;
      }
    }
  }

use_reciprocal_correspondence_的setter和getter

/** \brief Set whether to use reciprocal correspondence or not
   *
   * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence
   * or not
   */
  inline void
  setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
  {
    use_reciprocal_correspondence_ = use_reciprocal_correspondence;
  }

  /** \brief Obtain whether reciprocal correspondence are used or not */
  inline bool
  getUseReciprocalCorrespondences() const
  {
    return (use_reciprocal_correspondence_);
  }

protected成員函數

transformCloud

函數用於對輸入點雲的XYZ坐標及法向量做剛體變換：

//對輸入點雲的XYZ坐標及法向量做剛體變換
  /** \brief Apply a rigid transform to a given dataset. Here we check whether
   * the dataset has surface normals in addition to XYZ, and rotate normals as well.
   * \param[in] input the input point cloud
   * \param[out] output the resultant output point cloud
   * \param[in] transform a 4x4 rigid transformation
   * \note Can be used with cloud_in equal to cloud_out
   */
  virtual void
  transformCloud(const PointCloudSource& input,
                 PointCloudSource& output,
                 const Matrix4& transform);

computeTransformation

函數是ICP算法的核心所在：

//計算輸入到輸出點雲的轉換,然後將output設為轉換後的點雲
  /** \brief Rigid transformation computation method  with initial guess.
   * \param output the transformed input point cloud dataset using the rigid
   * transformation found \param guess the initial guess of the transformation to
   * compute
   */
  void
  computeTransformation(PointCloudSource& output, const Matrix4& guess) override;

determineRequiredBlobData

是一個輔助函數，用於決定是否需要將輸入的

pcl::PointCloud

轉換為

pcl::PCLPointCloud2

：

//?
  /** \brief Looks at the Estimators and Rejectors and determines whether their
   * blob-setter methods need to be called */
  virtual void
  determineRequiredBlobData();

protected成員變數

在做校正算法時，輸入及輸出的點雲類型可能不同，底層的數據結構也不盡相同，是以在實際處理點雲內部的數據時（如

transformCloud

函數），需要知道XYZ坐標分別位於一個點的什麼位置。另外如果該點包含法向量，還需要知道該點法向量的XYZ坐標相對於點起始位置的偏移量。

以下六個成員變數便是用來表示XYZ和法向量XYZ的offset，在

setInputSource

函數中會對他們進行初始化：

//屬於source點雲
  /** \brief XYZ fields offset. */
  std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;

  //屬於source點雲
  /** \brief Normal fields offset. */
  std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;

source點雲和target點雲可能帶有法向量也可能沒有，這兩個成員變數分別記錄兩點雲是否帶有法向量。主要在

transformCloud

函數中發揮作用。其中

source_has_normals_

在

setInputSource

函數中被初始化，

target_has_normals_

在

setInputTarget

函數中被初始化：

/** \brief Internal check whether source dataset has normals or not. */
  bool source_has_normals_;
  /** \brief Internal check whether target dataset has normals or not. */
  bool target_has_normals_;

校正算法輸入的點雲是

pcl::PointCloud

類型的，但是其內部所用到的

pcl::registration::CorrespondenceEstimationBase::setSourceNormals

，

pcl::registration::CorrespondenceRejector::setSourcePoints

和

pcl::registration::CorrespondenceRejector::setSourceNormals

函數（對應的target版本為

pcl::registration::CorrespondenceEstimationBase::setTargetNormals

，

pcl::registration::CorrespondenceRejector::setTargetPoints

和

pcl::registration::CorrespondenceRejector::setTargetNormals

函數）接受的卻是

pcl::PCLPointCloud2

類型的點雲。

在

determineRequiredBlobData

成員函數中，會判定是否需要用到上述的幾個函數，如果需要，

need_source_blob_

或/和

need_target_blob_

就會被設為true。然後在

computeTransformation

函數中依據這兩個flag決定是否需要執行

pcl::toPCLPointCloud2

。

//?
  /** \brief Checks for whether estimators and rejectors need various data */
  bool need_source_blob_, need_target_blob_;

在ICP算法的第一步是尋找點對，依據判斷標準可分為以下兩種：

目標點雲中的點B是原始點雲中點A在目標點雲中的最近鄰
目標點雲中的點B是原始點雲中點A在目標點雲中的最近鄰，同時A也是B在原始點雲中的最近鄰

可以看到第二個標準是較為嚴格的。這個flag便是用來選擇該採用上述標準中的哪一種：

/** \brief The correspondence type used for correspondence estimation. */
  bool use_reciprocal_correspondence_;

PCL - ICP代碼研讀（六） - IterativeClosestPoint架構