PCL - ICP代碼研讀（十七 ） - CorrespondenceRejectorDistance架構

前言

CorrespondenceRejectorDistance類別是CorrespondenceRejector的子類別，使用點對的距離作為拒絕點對的判斷條件：距離平方超過max_distance_的點對就會被拒絕。

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

CorrespondenceRejectorDistance

#pragma once

#include <pcl/registration/correspondence_rejection.h>
#include <pcl/conversions.h> // for fromPCLPointCloud2
#include <pcl/memory.h>      // for static_pointer_cast

namespace pcl {
namespace registration {
/**
 * @b CorrespondenceRejectorDistance implements a simple correspondence
 * rejection method based on thresholding the distances between the
 * correspondences.
 *
 * \note If \ref setInputCloud and \ref setInputTarget are given, then the
 * distances between correspondences will be estimated using the given XYZ
 * data, and not read from the set of input correspondences.
 *
 * \author Dirk Holz, Radu B. Rusu
 * \ingroup registration
 */
class PCL_EXPORTS CorrespondenceRejectorDistance : public CorrespondenceRejector {

using

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

  using CorrespondenceRejector::getClassName;
  using CorrespondenceRejector::input_correspondences_;
  using CorrespondenceRejector::rejection_name_;

public:
  using Ptr = shared_ptr<CorrespondenceRejectorDistance>;
  using ConstPtr = shared_ptr<const CorrespondenceRejectorDistance>;

constructor和destructor

  /** \brief Empty constructor. */
  CorrespondenceRejectorDistance() : max_distance_(std::numeric_limits<float>::max())
  {
    rejection_name_ = "CorrespondenceRejectorDistance";
  }

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

getRemainingCorrespondences

getRemainingCorrespondences函數在CorrespondenceRejector類別中是虛擬函數。此處CorrespondenceRejectorDistance類別必須給出它的完整定義。

  /** \brief Get a list of valid correspondences after rejection from the original set
   * of correspondences. \param[in] original_correspondences the set of initial
   * correspondences given \param[out] remaining_correspondences the resultant filtered
   * set of remaining correspondences
   */
  void
  getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
                              pcl::Correspondences& remaining_correspondences) override;

最大距離的getter和setter

成員變數max_distance_的getter和setter：

  /** \brief Set the maximum distance used for thresholding in correspondence rejection.
   * \param[in] distance Distance to be used as maximum distance between
   * correspondences. Correspondences with larger distances are rejected. \note
   * Internally, the distance will be stored squared.
   */
  virtual inline void
  setMaximumDistance(float distance)
  {
    // 傳入的是距離,但代碼中是用距離平方來做比較
    // 如果傳入的是float的最大值會overflow?
    max_distance_ = distance * distance;
  };

  /** \brief Get the maximum distance used for thresholding in correspondence rejection.
   */
  inline float
  getMaximumDistance() const
  {
    // 代碼裡面的xxx_distance_都是距離平方,所以要先開根號再回傳
    return std::sqrt(max_distance_);
  };

點雲，法向量設定相關函數

  /** \brief Provide a source point cloud dataset (must contain XYZ
   * data!), used to compute the correspondence distance.
   * \param[in] cloud a cloud containing XYZ data
   */
  template <typename PointT>
  inline void
  setInputSource(const typename pcl::PointCloud<PointT>::ConstPtr& cloud)
  {
    if (!data_container_)
      data_container_.reset(new DataContainer<PointT>);
    // static_pointer_cast是c++的函數?
    // 調用DataContainer::setInputSource
    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(cloud);
  }

  /** \brief Provide a target point cloud dataset (must contain XYZ
   * data!), used to compute the correspondence distance.
   * \param[in] target a cloud containing XYZ data
   */
  template <typename PointT>
  inline void
  setInputTarget(const typename pcl::PointCloud<PointT>::ConstPtr& target)
  {
    if (!data_container_)
      data_container_.reset(new DataContainer<PointT>);
    // 調用DataContainer::setInputTarget
    static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
  }

  /** \brief See if this rejector requires source points */
  bool
  requiresSourcePoints() const override
  {
    return (true);
  }

  /** \brief Blob method for setting the source cloud */
  void
  setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
  {
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    fromPCLPointCloud2(*cloud2, *cloud);
    setInputSource<PointXYZ>(cloud);
  }

  /** \brief See if this rejector requires a target cloud */
  bool
  requiresTargetPoints() const override
  {
    return (true);
  }

  /** \brief Method for setting the target cloud */
  void
  setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
  {
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    fromPCLPointCloud2(*cloud2, *cloud);
    setInputTarget<PointXYZ>(cloud);
  }

target點雲搜索方法setter

因為CorrespondenceRejectorDistance的target點雲和kdtree都是由data_container_管理的，所以此處是調用DataContainer::setSearchMethodTarget函數來設定target點雲搜索方法。

  /** \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.
   */
  template <typename PointT>
  inline void
  setSearchMethodTarget(const typename pcl::search::KdTree<PointT>::Ptr& tree,
                        bool force_no_recompute = false)
  {
    // 調用DataContainer::setSearchMethodTarget
    static_pointer_cast<DataContainer<PointT>>(data_container_)
        ->setSearchMethodTarget(tree, force_no_recompute);
  }

applyRejection

applyRejection函數是核心函數getRemainingCorrespondences的wrapper。調用getRemainingCorrespondences時預設傳入input_correspondences_。

protected:
  /** \brief Apply the rejection algorithm.
   * \param[out] correspondences the set of resultant correspondences.
   */
  // 原來applyRejection是getRemainingCorrespondences的wrapper?
  inline void
  applyRejection(pcl::Correspondences& correspondences) override
  {
    getRemainingCorrespondences(*input_correspondences_, correspondences);
  }

protected成員變數

• max_distance_：距離平方超過max_distance_的點對會被拒絕
• 注意到CorrespondenceRejectorDistance不像CorrespondenceEstimationBase，Registration一樣，沒有target_,tree_,target_cloud_updated_等成員變數，但是卻多了一個data_container_成員變數。在PCL - ICP代碼研讀（十八 ） - DataContainerInterface和DataContainer中將能看到，DataContainer實際上就是對target_,tree_,target_cloud_updated_等變數的封裝。

  // 註解錯了?
  //larger than -> larger than or equal to
  //will not be ignored -> will be ignored
  /** \brief The maximum distance threshold between two correspondent points in source
   * <-> target. If the distance is larger than this threshold, the points will not be
   * ignored in the alignment process.
   */
  float max_distance_;

  // DataContainerInterface定義於correspondence_rejection.h
  using DataContainerPtr = DataContainerInterface::Ptr;

  /** \brief A pointer to the DataContainer object containing the input and target point
   * clouds */
  DataContainerPtr data_container_;
};

} // namespace registration
} // namespace pcl

correspondence_rejection_distance.hpp

這個檔案似乎已經deprecated。

#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_
PCL_DEPRECATED_HEADER(1, 15, "")
#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_DISTANCE_HPP_ */