PCL - ICP代碼研讀（十三 ） - CorrespondenceEstimationBase實現

前言

從PCL - ICP代碼研讀（十二 ） - CorrespondenceEstimationBase架構中可以看到，CorrespondenceEstimationBase類別中宣告了成員函數setInputTarget，initCompute和initComputeReciprocal，但是並未給出實作，本篇就是介紹correspondence_estimation.hpp中這些函數的實現。

CorrespondenceEstimationBase

必要的include：

#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_

#include <pcl/common/copy_point.h>
#include <pcl/common/io.h>

namespace pcl {

namespace registration {

setInputTarget函數

此處的代碼似曾相識，與Registration::setInputTarget的代碼對照，可以發現這個函數的內容與Registration::setInputTarget幾乎一致，只差在多了point_representation_相關的代碼。

template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget(
    const PointCloudTargetConstPtr& cloud)
{
  if (cloud->points.empty()) {
    PCL_ERROR("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud "
              "dataset given!\n",
              getClassName().c_str());
    return;
  }
  target_ = cloud;

  // Set the internal point representation of choice
  if (point_representation_)
    tree_->setPointRepresentation(point_representation_);

  // 代表在initCompute中要tree_->setInputCloud
  target_cloud_updated_ = true;
}

注：參考PCL - ICP代碼研讀（十二 ） - CorrespondenceEstimationBase架構，CorrespondenceEstimationBase::setInputSource是inline函數，定義於correspondence_estimation.h。

initCompute函數

與Registration::initCompute的代碼對照，可以發現這個函數的內容就是Registration::initCompute中的前半部分。

template <typename PointSource, typename PointTarget, typename Scalar>
bool
CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute()
{
  if (!target_) {
    PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
              getClassName().c_str());
    return (false);
  }

  // Only update target kd-tree if a new target cloud was set
  // 在force_no_recompute_為false的情況下,如果target_cloud_updated_為true,就需要更新tree_
  if (target_cloud_updated_ && !force_no_recompute_) {
    // If the target indices have been given via setIndicesTarget
    if (target_indices_)
      tree_->setInputCloud(target_, target_indices_);
    else
      tree_->setInputCloud(target_);

    //表示setInputTarget後,已經做了相應的處理
    target_cloud_updated_ = false;
  }

  return (PCLBase<PointSource>::initCompute());
}

initComputeReciprocal函數

與Registration::initComputeReciprocal處的代碼相比較，可以發現此處並沒有檢查input_是否為空，並且多了point_representation_相關的代碼。

template <typename PointSource, typename PointTarget, typename Scalar>
bool
CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal()
{
  // Only update source kd-tree if a new target cloud was set
  /**
   * 在force_no_recompute_reciprocal_為false的情況下,
   * 如果source_cloud_updated_為true,就需要更新tree_reciprocal_
   **/
  if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
    if (point_representation_)
      tree_reciprocal_->setPointRepresentation(point_representation_);
    // If the target indices have been given via setIndicesTarget
    if (indices_)
      // 怎麼不直接用tree_reciprocal_->setInputCloud(input_, indices_);?
      tree_reciprocal_->setInputCloud(getInputSource(), getIndicesSource());
    else
      // 怎麼不直接用tree_reciprocal_->setInputCloud(input_);?
      tree_reciprocal_->setInputCloud(getInputSource());

    source_cloud_updated_ = false;
  }

  return (true);
}