PCL - ICP代碼研讀（四 ） - getFitnessScore函數

前言

接續PCL - ICP代碼研讀（三 ） - Registration初始化，本篇主要介紹Registration類別的getFitnessScore函數。這個函數用於計算source與target間的mse（mean squared error）。PCL中提供了兩個版本的getFitnessScore，分別適用於兩個向量或點雲。

getFitnessScore - for two vectors

傳入的兩個向量代表距離平方，這個函數用於計算它們之間的mse。

// 看不懂?
// 計算兩個向量的mse?
template <typename PointSource, typename PointTarget, typename Scalar>
inline double
Registration<PointSource, PointTarget, Scalar>::getFitnessScore(
    const std::vector<float>& distances_a, const std::vector<float>& distances_b)
{

Eigen::VectorXf::Map基於現存的向量或陣列之上，建立一個Eigen::VectorXf的映射。其建構子第一個參數表示現存向量的位址，第二個參數表示欲建立row數為多少的向量。

所以這段代碼的涵意為：分別取兩向量的前nr_elem個元素為map_a及map_b。

  unsigned int nr_elem =
      static_cast<unsigned int>(std::min(distances_a.size(), distances_b.size()));
  Eigen::VectorXf map_a = Eigen::VectorXf::Map(&distances_a[0], nr_elem);
  Eigen::VectorXf map_b = Eigen::VectorXf::Map(&distances_b[0], nr_elem);

回傳兩向量距離平方的平均值：

  return (static_cast<double>((map_a - map_b).sum()) / static_cast<double>(nr_elem));
}

注：因為傳入的兩個向量代表距離平方，所以代碼裡沒有做平方是正常的。但是兩個距離平方相減還會等於距離平方?應該要先開根號，相減後再平方才對？

getFitnessScore - for two point clouds

此函數用於計算經final_transform_轉換後的input_與target_間的mean square error。這個才是在ICP例程中所使用的函數。

// 距離超過max_range的不會被計入
template <typename PointSource, typename PointTarget, typename Scalar>
inline double
Registration<PointSource, PointTarget, Scalar>::getFitnessScore(double max_range)
{
  // 計算轉換後的input點雲與target點雲間的mean squared error
  double fitness_score = 0.0;

校正算法（即PCL - ICP代碼研讀（五 ） - align函數介紹的align函數）會估計出一個4*4的轉換矩陣final_transformation_，用它來對input_點雲做轉換，得到input_transformed_：

  // Transform the input dataset using the final transformation
  PointCloudSource input_transformed;
  transformPointCloud(*input_, input_transformed, final_transformation_);

只需要尋找一個最近鄰：

  pcl::Indices nn_indices(1);
  std::vector<float> nn_dists(1);

  // For each point in the source dataset
  int nr = 0;

遍歷轉換後的輸入點雲的每一個點，在目標點雲中尋找最近點：

  for (const auto& point : input_transformed) {
    // Find its nearest neighbor in the target
    tree_->nearestKSearch(point, 1, nn_indices, nn_dists);

排除距離過大的點對，符合條件者會計入fitness_score中：
（注意nearestKSearch回傳的nn_dists中的元素代表的是距離平方）

    // Deal with occlusions (incomplete targets)
    // 排除距離過大的點對
    if (nn_dists[0] <= max_range) {
      // Add to the fitness score
      fitness_score += nn_dists[0];
      nr++;
    }
  }

在這之前fitness_score是sum of squared error，這裡做平均之後，fitness_score變為mse（mean squared error）：

  if (nr > 0)
    return (fitness_score / nr);

如果有效的點對數量為0，則回傳double型別的最大值：

  return (std::numeric_limits<double>::max());
}

注：因為fitness score是越小越好，所以回傳最大值代表兩個點雲完全沒有對齊