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

前言

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算法的第一步是尋找點對，依據判斷標準可分為以下兩種：

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

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

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