PCL - ICP代碼研讀（十八 ） - DataContainerInterface和DataContainer

前言

在PCL - ICP代碼研讀（十七 ） - CorrespondenceRejectorDistance架構中，看到CorrespondenceRejectorDistance有個DataContainerPtr類型的成員變數data_container_，它的作用是將target_,tree_,target_cloud_updated_等變數封裝起來。

DataContainerInterface提供了一個計算配對分數的介面；而DataContainer繼承自DataContainerInterface，是為souce點雲和target點雲的容器，並給出了配對分數計算函數的實現。

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

DataContainerInterface

/** @b DataContainerInterface provides a generic interface for computing correspondence
 * scores between correspondent points in the input and target clouds
 * \ingroup registration
 */
class DataContainerInterface {
public:

using

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

  using Ptr = shared_ptr<DataContainerInterface>;
  using ConstPtr = shared_ptr<const DataContainerInterface>;

destructor

  virtual ~DataContainerInterface() = default;

計算配對分數的介面

  virtual double
  getCorrespondenceScore(int index) = 0;
  virtual double
  getCorrespondenceScore(const pcl::Correspondence&) = 0;
  virtual double
  getCorrespondenceScoreFromNormals(const pcl::Correspondence&) = 0;
};

DataContainer

DataContainer是為輸入和輸出點雲的容器，並實現了DataContainerInterface中計算配對分數的介面。

/** @b DataContainer is a container for the input and target point clouds and implements
 * the interface to compute correspondence scores between correspondent points in the
 * input and target clouds \ingroup registration
 */
template <typename PointT, typename NormalT = pcl::PointNormal>
class DataContainer : public DataContainerInterface {

using

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

  using PointCloud = pcl::PointCloud<PointT>;
  using PointCloudPtr = typename PointCloud::Ptr;
  using PointCloudConstPtr = typename PointCloud::ConstPtr;

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

  using Normals = pcl::PointCloud<NormalT>;
  using NormalsPtr = typename Normals::Ptr;
  using NormalsConstPtr = typename Normals::ConstPtr;

constructor和destructor

public:
  /** \brief Empty constructor. */
  DataContainer(bool needs_normals = false)
  : input_()
  , input_transformed_()
  , target_()
  , input_normals_()
  , input_normals_transformed_()
  , target_normals_()
  , tree_(new pcl::search::KdTree<PointT>)
  , class_name_("DataContainer")
  , needs_normals_(needs_normals)
  , target_cloud_updated_(true)
  , force_no_recompute_(false)
  {}

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

點雲的setter和getter

DataContainer用於管理點雲，所以有點雲的setter和getter作為成員函數也就不奇怪了。

  /** \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
   */
  inline void
  setInputSource(const PointCloudConstPtr& cloud)
  {
    input_ = cloud;
    // 這麼沒有source_cloud_updated_ = true;?
  }

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

  /** \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
   */
  inline void
  setInputTarget(const PointCloudConstPtr& target)
  {
    target_ = target;
    target_cloud_updated_ = true;
  }

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

target點雲搜索方法setter

  /** \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;
    // 更新target tree後,target_cloud_updated_要設為true,表示在getCorrespondenceScore中需要做tree_->setInputCloud
    target_cloud_updated_ = true;
  }

法向量setter和getter

DataContainer不只管理點雲，同時也管理了點雲的法向量，所以才有法向量setter和getter作為其成員函數。

  /** \brief Set the normals computed on the input point cloud
   * \param[in] normals the normals computed for the input cloud
   */
  inline void
  setInputNormals(const NormalsConstPtr& normals)
  {
    input_normals_ = normals;
  }

  /** \brief Get the normals computed on the input point cloud */
  inline NormalsConstPtr
  getInputNormals()
  {
    return (input_normals_);
  }

  /** \brief Set the normals computed on the target point cloud
   * \param[in] normals the normals computed for the input cloud
   */
  inline void
  setTargetNormals(const NormalsConstPtr& normals)
  {
    target_normals_ = normals;
  }

  /** \brief Get the normals computed on the target point cloud */
  inline NormalsConstPtr
  getTargetNormals()
  {
    return (target_normals_);
  }

配對分數計算

為source點雲的第index個點尋找它在target點雲中的最近鄰，然後回傳它們的距離平方作為配對分數。

  /** \brief Get the correspondence score for a point in the input cloud
   * \param[in] index index of the point in the input cloud
   */
  inline double
  getCorrespondenceScore(int index) override
  {
    if (target_cloud_updated_ && !force_no_recompute_) {
      tree_->setInputCloud(target_);
      // 這裡是不是應該把target_cloud_updated_設回false?
    }
    pcl::Indices indices(1);
    std::vector<float> distances(1);
    // 預設的correspondence score是距離平方
    if (tree_->nearestKSearch((*input_)[index], 1, indices, distances))
      return (distances[0]);
    /**
     * 與correspondence_rejection_distance.cpp的
     * CorrespondenceRejectorDistance::getRemainingCorrespondences比對,
     * 這個score小於某個閾值的才會被保留
     **/
    return (std::numeric_limits<double>::max());
  }

計算輸入的配對corr中兩點的距離平方：

  /** \brief Get the correspondence score for a given pair of correspondent points
   * \param[in] corr Correspondent points
   */
  inline double
  getCorrespondenceScore(const pcl::Correspondence& corr) override
  {
    // Get the source and the target feature from the list
    const PointT& src = (*input_)[corr.index_query];
    const PointT& tgt = (*target_)[corr.index_match];

    // corr裡面兩個點的距離
    return ((src.getVector4fMap() - tgt.getVector4fMap()).squaredNorm());
  }

回傳配對點的法向量內積當作配對分數：

  // 註解in和put分開了?
  // typo:normmals->normals
  /** \brief Get the correspondence score for a given pair of correspondent points based
   * on the angle between the normals. The normmals for the in put and target clouds
   * must be set before using this function \param[in] corr Correspondent points
   */
  inline double
  getCorrespondenceScoreFromNormals(const pcl::Correspondence& corr) override
  {
    // assert ( (input_normals_->size () != 0) && (target_normals_->size () != 0) &&
    // "Normals are not set for the input and target point clouds");
    assert(input_normals_ && target_normals_ &&
           "Normals are not set for the input and target point clouds");
    const NormalT& src = (*input_normals_)[corr.index_query];
    const NormalT& tgt = (*target_normals_)[corr.index_match];
    // 兩個法向量的內積,即cos(angle)
    // 分數是越大越好?
    return (double((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) +
                   (src.normal[2] * tgt.normal[2])));
  }

private成員

DataContainer為輸入和輸出點雲的容器，所以有input_,target_,input_normals_,target_normals_等成員變數也就不奇怪了。

private:
  /** \brief The input point cloud dataset */
  PointCloudConstPtr input_;

  // CorrespondenceEstimationBase也有,作用為何?
  /** \brief The input transformed point cloud dataset */
  PointCloudPtr input_transformed_;

  /** \brief The target point cloud datase. */
  PointCloudConstPtr target_;

  /** \brief Normals to the input point cloud */
  NormalsConstPtr input_normals_;

  // 作用為何?
  /** \brief Normals to the input point cloud */
  NormalsPtr input_normals_transformed_;

  /** \brief Normals to the target point cloud */
  NormalsConstPtr target_normals_;

  /** \brief A pointer to the spatial search object. */
  KdTreePtr tree_;

  /** \brief The name of the rejection method. */
  std::string class_name_;

  /** \brief Should the current data container use normals? */
  bool needs_normals_;

  /** \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 */
  bool target_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 Get a string representation of the name of this class. */
  inline const std::string&
  getClassName() const
  {
    return (class_name_);
  }
};
} // namespace registration
} // namespace pcl