PCL - ICP代碼研讀（十二 ） - CorrespondenceEstimationBase架構

前言

CorrespondenceEstimationBase是一個抽象類別，提供了如CorrespondenceEstimation，CorrespondenceEstimationNormalShooting等用於估計兩點雲間配對的類別的代碼框架。

本篇主要對應到correspondence_estimation.h這個檔案。

CorrespondenceEstimationBase

#pragma once

#include <pcl/common/io.h> // for getFields
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>

#include <string>

namespace pcl {
namespace registration {
/** \brief Abstract @b CorrespondenceEstimationBase class.
 * All correspondence estimation methods should inherit from this.
 * \author Radu B. Rusu
 * \ingroup registration
 */
template <typename PointSource, typename PointTarget, typename Scalar = float>
class CorrespondenceEstimationBase : public PCLBase<PointSource> {
public:

using

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

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

因為PCLBase已經定義了deinitCompute,input_,indices_,setIndices，這裡用了using表示沿用PCLBase中的定義。

參考改變成員的訪問權限，此處using的另外一個作用為：將原來是PCLBase的protected成員的導入CorrespondenceEstimationBase的public權限區塊。

  // 有自己override一個initCompute
  // using PCLBase<PointSource>::initCompute;
  using PCLBase<PointSource>::deinitCompute;
  using PCLBase<PointSource>::input_;
  using PCLBase<PointSource>::indices_;
  using PCLBase<PointSource>::setIndices;

其餘皆是用using來定義名稱，方便後續使用。

  using KdTree = pcl::search::KdTree<PointTarget>;
  using KdTreePtr = typename KdTree::Ptr;

  using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
  using KdTreeReciprocalPtr = typename KdTree::Ptr;

  using PointCloudSource = pcl::PointCloud<PointSource>;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

  using PointCloudTarget = pcl::PointCloud<PointTarget>;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

  using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;

constructor和destructor

  /** \brief Empty constructor. */
  CorrespondenceEstimationBase()
  : corr_name_("CorrespondenceEstimationBase")
  , tree_(new pcl::search::KdTree<PointTarget>)
  , tree_reciprocal_(new pcl::search::KdTree<PointSource>)
  , target_()
  , point_representation_()
  , input_transformed_()
  , target_cloud_updated_(true)
  , source_cloud_updated_(true)
  , force_no_recompute_(false)
  , force_no_recompute_reciprocal_(false)
  {}

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

點雲的setter和getter

與Registration::setInputSource的代碼對照，可以發現這個函數的內容與Registration::setInputSource幾乎一致，只差在少了對輸入點雲cloud是否為空的檢查，並多了input_fields_相關的代碼。

  /** \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
   */
  inline void
  setInputSource(const PointCloudSourceConstPtr& cloud)
  {
    // 表示需要再算一次tree_reciprocal_
    source_cloud_updated_ = true;
    PCLBase<PointSource>::setInputCloud(cloud);
    input_fields_ = pcl::getFields<PointSource>();
  }

getInputSource直接返回input_點雲：

  /** \brief Get a pointer to the input point cloud dataset target. */
  inline PointCloudSourceConstPtr const
  getInputSource()
  {
    return (input_);
  }

setInputTarget的定義位於correspondence_estimation.hpp這份檔案：

  /** \brief Provide a pointer to the input target
   * (e.g., the point cloud that we want to align the input source to)
   * \param[in] cloud the input point cloud target
   */
  // 不需要類似target_cloud_updated_ = true;的語句? -> 在hpp裡有
  // 它的定義在hpp裡面
  inline void
  setInputTarget(const PointCloudTargetConstPtr& cloud);

getInputTarget直接返回target_點雲：

  /** \brief Get a pointer to the input point cloud dataset target. */
  inline PointCloudTargetConstPtr const
  getInputTarget()
  {
    return (target_);
  }

法向量相關函數

在預設情況下，點對估計器是不需要點雲有法向量的。這也是此處requiresSourceNormals和requiresTargetNormals函數回傳false的原因。

注意這些函數都被宣告為virtual的，如此一來，CorrespondenceEstimationBase的各個子類別便可以自行決定是否需要法向量。

  /** \brief See if this rejector requires source normals */
  virtual bool
  requiresSourceNormals() const
  {
    return (false);
  }

  /** \brief Abstract method for setting the source normals */
  virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
  {
    PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
             "input source normals\n",
             getClassName().c_str());
  }

  /** \brief See if this rejector requires target normals */
  virtual bool
  requiresTargetNormals() const
  {
    return (false);
  }

  /** \brief Abstract method for setting the target normals */
  virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
  {
    PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
             "input target normals\n",
             getClassName().c_str());
  }

索引的setter和getter

setIndicesSource函數調用了pcl::PCLBase::setIndices，詳見# PCL - PCLBase代碼研讀（二）- PCLBase實現 - pcl::PCLBase::setIndices。（這裡不需要source_cloud_updated_ = true;?）

  /** \brief Provide a pointer to the vector of indices that represent the
   * input source point cloud.
   * \param[in] indices a pointer to the vector of indices
   */
  // 表示輸入點雲的indices向量?
  inline void
  setIndicesSource(const IndicesPtr& indices)
  {
    // 設定好indices_這個成員變數
    setIndices(indices);
  }

其餘三個函數的意義顯而易見：

  /** \brief Get a pointer to the vector of indices used for the source dataset. */
  inline IndicesPtr const
  getIndicesSource()
  {
    return (indices_);
  }

  /** \brief Provide a pointer to the vector of indices that represent the input target
   * point cloud. \param[in] indices a pointer to the vector of indices
   */
  // 表示target點雲的indices向量?
  inline void
  setIndicesTarget(const IndicesPtr& indices)
  {
    // 這裡有target_cloud_updated_ = true, setInputTarget函數的定義裡也有
    target_cloud_updated_ = true;
    target_indices_ = indices;
  }

  /** \brief Get a pointer to the vector of indices used for the target dataset. */
  inline IndicesPtr const
  getIndicesTarget()
  {
    return (target_indices_);
  }

最近鄰查找相關函數

這四個函數的功用也顯而易見，要注意的是，在setSearchMethodTarget（setSearchMethodSource）中，需要這一行：target_cloud_updated_ = true;（source_cloud_updated_ = true;）。這表示在initCompute（initComputeReciprocal）函數中，還得更新tree_（tree_reciprocal_）。

  /** \brief Provide a pointer to the search object used to find correspondences in
   * the target cloud.
   * \param[in] tree a pointer to the spatial search object.
   * \param[in] force_no_recompute If set to true, this tree will NEVER be
   * recomputed, regardless of calls to setInputTarget. Only use if you are
   * confident that the tree will be set correctly.
   */
  inline void
  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
  {
    tree_ = tree;
    force_no_recompute_ = force_no_recompute;
    // Since we just set a new tree, we need to check for updates
    // 需要再調用一次initCompute,執行tree_->setInputCloud,target_cloud_updated_才會被設回false
    target_cloud_updated_ = true;
  }

  /** \brief Get a pointer to the search method used to find correspondences in the
   * target cloud. */
  inline KdTreePtr
  getSearchMethodTarget() const
  {
    return (tree_);
  }

  /** \brief Provide a pointer to the search object used to find correspondences in
   * the source cloud (usually used by reciprocal correspondence finding).
   * \param[in] tree a pointer to the spatial search object.
   * \param[in] force_no_recompute If set to true, this tree will NEVER be
   * recomputed, regardless of calls to setInputSource. Only use if you are
   * extremely confident that the tree will be set correctly.
   */
  inline void
  setSearchMethodSource(const KdTreeReciprocalPtr& tree,
                        bool force_no_recompute = false)
  {
    tree_reciprocal_ = tree;
    force_no_recompute_reciprocal_ = force_no_recompute;
    // Since we just set a new tree, we need to check for updates
    // 需要再調用一次initComputeReciprocal,執行tree_reciprocal_->setInputCloud,source_cloud_updated_才會被設回false
    source_cloud_updated_ = true;
  }

  /** \brief Get a pointer to the search method used to find correspondences in the
   * source cloud. */
  inline KdTreeReciprocalPtr
  getSearchMethodSource() const
  {
    return (tree_reciprocal_);
  }

估計點對的函數

這兩個函數是CorrespondenceEstimationBase的核心所在，用於估計兩點雲間的點對。

  /** \brief Determine the correspondences between input and target cloud.
   * \param[out] correspondences the found correspondences (index of query point, index
   * of target point, distance) \param[in] max_distance maximum allowed distance between
   * correspondences
   */
  virtual void
  determineCorrespondences(
      pcl::Correspondences& correspondences,
      double max_distance = std::numeric_limits<double>::max()) = 0;

  /** \brief Determine the reciprocal correspondences between input and target cloud.
   * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
   * correspondence, and Tgt_i has Src_i as one.
   *
   * \param[out] correspondences the found correspondences (index of query and target
   * point, distance) \param[in] max_distance maximum allowed distance between
   * correspondences
   */
  virtual void
  determineReciprocalCorrespondences(
      pcl::Correspondences& correspondences,
      double max_distance = std::numeric_limits<double>::max()) = 0;

point_representation_的setter

與point_representation_相關的代碼皆待研究。

  /** \brief Provide a boost shared pointer to the PointRepresentation to be used
   * when searching for nearest neighbors.
   *
   * \param[in] point_representation the PointRepresentation to be used by the
   * k-D tree for nearest neighbor search
   */
  // 這個的作用是?
  inline void
  setPointRepresentation(const PointRepresentationConstPtr& point_representation)
  {
    point_representation_ = point_representation;
  }

clone函數

返回一份自己的克隆：

  /** \brief Clone and cast to CorrespondenceEstimationBase */
  virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
  clone() const = 0;

protected成員變數

點雲相關：

• target_：target點雲
• target_indices_：target點雲索引

最近鄰搜索相關（此處與Registration - 最近鄰搜索相關protected成員變數一致）：

• tree_
• tree_reciprocal_
• target_cloud_updated_
• source_cloud_updated_
• force_no_recompute_
• force_no_recompute_reciprocal_

其它：

• corr_name_：點對估計算法的名稱
• point_representation_：待研究
• input_transformed_：目前尚未看到它在哪裡被使用
• input_fields_：儲存source點雲點的各欄位資訊（pcl::PCLPointField）的向量

protected:
  /** \brief The correspondence estimation method name. */
  std::string corr_name_;

  /** \brief A pointer to the spatial search object used for the target dataset. */
  KdTreePtr tree_;

  /** \brief A pointer to the spatial search object used for the source dataset. */
  KdTreeReciprocalPtr tree_reciprocal_;

  // source也有對應的input_
  /** \brief The input point cloud dataset target. */
  PointCloudTargetConstPtr target_;

  // source也有對應的indices_
  /** \brief The target point cloud dataset indices. */
  IndicesPtr target_indices_;

  // 作用為何?
  /** \brief The point representation used (internal). */
  PointRepresentationConstPtr point_representation_;

  // 作用為何?
  /** \brief The transformed input source point cloud dataset. */
  PointCloudTargetPtr input_transformed_;

  // 作用為何?
  /** \brief The types of input point fields available. */
  std::vector<pcl::PCLPointField> input_fields_;

  /** \brief Variable that stores whether we have a new target cloud, meaning we need to
   * pre-process it again. This way, we avoid rebuilding the kd-tree for the target
   * cloud every time the determineCorrespondences () method is called. */
  // 如果是true,代表需要重新計算tree_
  bool target_cloud_updated_;
  /** \brief Variable that stores whether we have a new source cloud, meaning we need to
   * pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
   * source cloud every time the determineCorrespondences () method is called. */
  // 如果是true,代表需要重新計算tree_reciprocal_
  bool source_cloud_updated_;
  /** \brief A flag which, if set, means the tree operating on the target cloud
   * will never be recomputed*/
  bool force_no_recompute_;

  /** \brief A flag which, if set, means the tree operating on the source cloud
   * will never be recomputed*/
  bool force_no_recompute_reciprocal_;
};

protected成員函數

getClassName回傳corr_name_，即配對估計器的名稱：

  /** \brief Abstract class get name method. */
  inline const std::string&
  getClassName() const
  {
    return (corr_name_);
  }

記得在using那部分的代碼中，using PCLBase<PointSource>::initCompute;這一行是被注釋掉的，這是因為CorrespondenceEstimationBase有自己定義的initCompute函數。

  /** \brief Internal computation initialization. */
  bool
  initCompute();

source點雲版本的initCompute函數：

  /** \brief Internal computation initialization for reciprocal correspondences. */
  bool
  initComputeReciprocal();