PCL - ICP代碼研讀（十五 ） - CorrespondenceEstimation實現

前言

接續PCL - ICP代碼研讀（十四 ） - CorrespondenceEstimation架構，本篇主要介紹correspondence_estimation.h中宣告但未給出實現的determineCorrespondences和determineReciprocalCorrespondences函數。

本篇對應到correspondence_estimation.hpp這份檔案。

CorrespondenceEstimation

determineCorrespondences函數

determineCorrespondences函數尋找兩點雲中距離小於max_distance者當作點對，並填入correspondences這個容器裡。

template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences(
    pcl::Correspondences& correspondences, double max_distance)
{

首先調用CorrespondenceEstimationBase::initCompute函數對搜索方法tree_初始化：

  if (!initCompute())
    return;

距離平方超過max_dist_sqr的點對會被排除：

  // max_distance預設值是std::numeric_limits<double>::max(),這樣不會overflow?
  double max_dist_sqr = max_distance * max_distance;

一開始先將correspondencesresize成點雲索引indices_的大小：

  // 一開始先resize成輸入點雲索引的大小
  correspondences.resize(indices_->size());

與最近鄰搜索相關的變數：

  pcl::Indices index(1);
  std::vector<float> distance(1);
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;

如果source點雲和target點雲的點型別相同，則執行if分支：

  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
  // macro!
  if (isSamePointType<PointSource, PointTarget>()) {
    // Iterate over the input set of source indices

遍歷輸入點雲的索引：

    // 遍歷輸入點雲的索引
    for (const auto& idx : (*indices_)) {

尋找目標點雲中離(*input_)[idx]最近的點：

      // 尋找目標點雲中離input_[idx]最近的點
      tree_->nearestKSearch((*input_)[idx], 1, index, distance);

排除距離平方超過max_dist_sqr（即距離大於max_distance）的點：

      // 排除距離大於max_distance的點
      if (distance[0] > max_dist_sqr)
        continue;

由source點雲中的idx對應到target點雲的index[0]點：

      // 由source點雲中的idx對應到target點雲的index[0]點
      corr.index_query = idx;
      corr.index_match = index[0];      
      // 距離平方?
      corr.distance = distance[0];

填入correspondences這個容器：

      // 儲存到correspondences數據結構中
      correspondences[nr_valid_correspondences++] = corr;
    }
  }

如果source點雲和target點雲的點型別不同，則執行else分支。與if分支相比，這裡需要先定義一個類型為PointTarget的點，然後將source點雲中的點(*input_)[idx]複製給pt。

  else {
    PointTarget pt;

    // Iterate over the input set of source indices
    for (const auto& idx : (*indices_)) {
      // Copy the source data to a target PointTarget format so we can search in the
      // tree
      // 先把原始點雲中的點複製到目標點雲類型的點pt中,然後再進行搜索
      copyPoint((*input_)[idx], pt);

      tree_->nearestKSearch(pt, 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      corr.index_query = idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }

將correspondences容器縮小為有效點對的個數nr_valid_correspondences：

  // 知道實際有多少配對後再resize成配對數:nr_valid_correspondences
  correspondences.resize(nr_valid_correspondences);

PCLBase::deinitCompute函數為空：

  deinitCompute();
}

determineReciprocalCorrespondences函數

與determineCorrespondences函數類似，determineReciprocalCorrespondences函數也是用於尋找點對。但是這裡的方式較為嚴格，要求配對的兩點互為最近鄰，也就是說：除了(*target_)[target_idx]必須是離(*input_)[idx]最近的鄰居外，還要求(*input_)[idx]必須是(*target_)[target_idx]在source點雲中的最近鄰。

template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
    determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
                                       double max_distance)
{
  if (!initCompute())
    return;

  // setup tree for reciprocal search
  // Set the internal point representation of choice
  if (!initComputeReciprocal())
    return;
  double max_dist_sqr = max_distance * max_distance;

  correspondences.resize(indices_->size());
  pcl::Indices index(1);
  std::vector<float> distance(1);
  pcl::Indices index_reciprocal(1);
  std::vector<float> distance_reciprocal(1);
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;
  int target_idx = 0;

  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
  // macro!
  if (isSamePointType<PointSource, PointTarget>()) {
    // Iterate over the input set of source indices
    for (const auto& idx : (*indices_)) {
      tree_->nearestKSearch((*input_)[idx], 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      // target點雲中離source點雲的第idx個點最近的點是的target_idx
      target_idx = index[0];

      tree_reciprocal_->nearestKSearch(
          (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
      if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
        continue;
      // source點雲中離target點雲的第target_idx個點最近的點是的index_reciprocal[0],也就是idx

      corr.index_query = idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else {
    PointTarget pt_src;
    PointSource pt_tgt;

    // Iterate over the input set of source indices
    for (const auto& idx : (*indices_)) {
      // Copy the source data to a target PointTarget format so we can search in the
      // tree
      copyPoint((*input_)[idx], pt_src);

      tree_->nearestKSearch(pt_src, 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      target_idx = index[0];

      // Copy the target data to a target PointSource format so we can search in the
      // tree_reciprocal
      copyPoint((*target_)[target_idx], pt_tgt);

      tree_reciprocal_->nearestKSearch(
          pt_tgt, 1, index_reciprocal, distance_reciprocal);
      if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
        continue;

      corr.index_query = idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  correspondences.resize(nr_valid_correspondences);
  deinitCompute();
}

} // namespace registration
} // namespace pcl

//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS
// pcl::registration::CorrespondenceEstimation<T,U>;

#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */