PCL - ICP代碼研讀（三 ） - Registration初始化

前言

接續PCL - ICP代碼研讀（二 ） - Registration架構，這邊繼續來看Registration成員函數的實現。本篇主要關注的是registration/include/pcl/registration/impl/registration.hpp這份文件中與初始化相關的函數。

setInputSource

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::setInputSource(
    const PointCloudSourceConstPtr& cloud)
{

檢查輸入的點雲cloud是否為空，如果是，則退出：

  if (cloud->points.empty()) {
    PCL_ERROR("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
              getClassName().c_str());
    return;
  }

source_cloud_updated_是一個flag，表示source點雲更新了，待會在initComputeReciprocal函數中需要更新source點雲所對應的kdtree。

  // 表示等會要更新source點雲的kdtree
  source_cloud_updated_ = true;

參考PCL - PCLBase代碼研讀（二）- PCLBase實現，PCLBase::setInputCloud的功能就只是把成員變數input_設為輸入的點雲cloud而已。

  PCLBase<PointSource>::setInputCloud(cloud);
}

setInputTarget

此處代碼與setInputSource大同小異，只是這裡設定的是target點雲。

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::setInputTarget(
    const PointCloudTargetConstPtr& cloud)
{
  if (cloud->points.empty()) {
    PCL_ERROR("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n",
              getClassName().c_str());
    return;
  }
  target_ = cloud;
  // 表示等會要更新target點雲的kdtree
  target_cloud_updated_ = true;
}

initCompute

在initCompute函數中，會設定tree_及correspondence_estimation_。

template <typename PointSource, typename PointTarget, typename Scalar>
bool
Registration<PointSource, PointTarget, Scalar>::initCompute()
{

照例先檢查輸入的點雲target_是否為空：

  if (!target_) {
    PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
              getClassName().c_str());
    return (false);
  }

預設情況下force_no_recompute_為false，這時如果target_cloud_updated_為true，就需要更新target對點雲對應的kdtree：tree_。更新完後將target_cloud_updated_設回 false。

如果force_no_recompute_為true，則不會更新tree_。(第一次設定tree_時也不會?)

  // Only update target kd-tree if a new target cloud was set
  // 在force_no_recompute_為false時,target_cloud_updated_為true代表需要更新target點雲的kdtree
  if (target_cloud_updated_ && !force_no_recompute_) {
    // 更新target點雲的kdtree
    tree_->setInputCloud(target_);
    // 更新完後設回false
    target_cloud_updated_ = false;
  }

correspondence_estimation_用於估計兩點雲間的點對，此處設定它在target點雲中查找點的方式為tree_，在source點雲中查找點的方式為tree_reciprocal_。
(如果tree_reciprocal_為空會發生何事?)

  // Update the correspondence estimation
  if (correspondence_estimation_) {
    correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_);
    correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,
                                                      force_no_recompute_reciprocal_);
  }

PCLBase::initCompute函數用於設定fake_indices_及indices_成員變數。

  // 什麼意思?
  // Note: we /cannot/ update the search method on all correspondence rejectors, because
  // we know nothing about them. If they should be cached, they must be cached
  // individually.

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

initComputeReciprocal

在initComputeReciprocal函數中，則會設定tree_reciprocal_。

// 好像沒看到initComputeReciprocal被呼叫過?
template <typename PointSource, typename PointTarget, typename Scalar>
bool
Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal()
{
  if (!input_) {
    PCL_ERROR("[pcl::registration::%s::compute] No input source dataset was given!\n",
              getClassName().c_str());
    return (false);
  }

  if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
    // 更新source點雲的kdtree
    tree_reciprocal_->setInputCloud(input_);
    source_cloud_updated_ = false;
  }

  // 不需要這段?(摘自initCompute)
  // 所以要先呼叫initComputeReciprocal再呼叫initCompute
  // if (correspondence_estimation_) {
  //   correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,
  //                                                     force_no_recompute_reciprocal_);
  // }

  return (true);
}