cartographer源码解析(3)

本节将开始介绍cartographer前端点云与地图匹配获取当前位姿的算法。上节末尾我们提到ScanMatch函数，源码如下：

std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
  // 地图未创建直接返回
  // LOG(INFO)<<"local trajectory scan match called.";
  if (active_submaps_.submaps().empty()) {
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }
  // 当前用来匹配的submap
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 暴力搜索粗匹配
  if (options_.use_online_correlative_scan_matching()) {
    LOG(INFO) << "correlative scan matching called";
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  // ceres非线性优化精匹配
  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }
  return pose_observation;
}

这里先提一下激活的子图(ActiveSubmap)。第一帧点云传入时即创建一个子图并将数据存入，当点云帧数达到一定数量(默认90帧)时，若此时仅一个子图(前子图)，则创建一个新子图(后子图)，之后传入的点云将同时添加到这两个子图中。一旦前子图中点云帧数达到极限(默认两倍即180帧)，前子图的Finish函数会被调用对其进行冻结并剔除出ActiveSubmap。此时，后子图将变为前子图，且通常此时其帧数也刚好达到参数设定的数量(90)，因此会新创建一个子图。

默认情况下use_online_correlative_scan_matching()获取到的参数配置为false，即不使用暴力匹配(我习惯这么称呼，直译似乎是实时相关性扫描匹配)。该匹配的目的在于给后面ceres精匹配提供一个位姿的初值。如果有尝试将ceres匹配单独剥离测试，会发现其对初值的要求非常高，一旦偏离匹配就会失败，这是ceres匹配原理决定的。因此，虽然目前未对暴力匹配进行单独测试，但大致可以估计其使用场景是运动速度/角速度非常高的物体上的SLAM，低速运动(V<2m/s, Va<1.05rad/s)下仅使用ceres匹配即可。

下面从Match函数开始介绍实时相关性扫描匹配。

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const {
  CHECK(pose_estimate != nullptr);

  // 根据初始位姿旋转点云，并生成位置与角度搜索参数
  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());

  // 根据参数生成一组旋转点云，平移到世界坐标系
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  // 暴力搜索并打分                         
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  *pose_estimate = transform::Rigid2d(
      {initial_pose_estimate.translation().x() + best_candidate.x,
       initial_pose_estimate.translation().y() + best_candidate.y},
      initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
  return best_candidate.score;
}

 默认平移窗口参数为+-0.1m，角度窗口+-20度。计算角度步长，近似为sin(分辨率 / 最大扫描距离)；计算平移步长为分辨率。以此为依据平移与旋转点云，并平移到世界坐标系生成备选项(candidates)。将各点坐标映射到概率地图(probability_grid)中，计算概率值总和并最终选择和最大的备选项，此处概率值计算方式为1-地图像素值(浮点0-1.0f)，即黑色(障碍或墙壁)概率值大，白色(未被占用空间/free space)概率值小。

void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
    const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const {
  for (Candidate2D& candidate : *candidates) {
    switch (grid.GetGridType()) {
      case GridType::PROBABILITY_GRID:
        candidate.score = ComputeCandidateScore(
            static_cast<const ProbabilityGrid&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);
        break;
      case GridType::TSDF:
        candidate.score = ComputeCandidateScore(
            static_cast<const TSDF2D&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);
        break;
    }
    candidate.score *=
        std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
                                   options_.translation_delta_cost_weight() +
                               std::abs(candidate.orientation) *
                                   options_.rotation_delta_cost_weight()));
  }
}

先计算概率值得分，找到尽可能使点云贴合墙壁/障碍的位姿变换；后加入平移与旋转代价，尽可能使结果在初值附近，避免匹配结果漂移过远。

float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
                            const DiscreteScan2D& discrete_scan,
                            int x_index_offset, int y_index_offset) {
  float candidate_score = 0.f;
  for (const Eigen::Array2i& xy_index : discrete_scan) {
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset);
    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    candidate_score += probability;
  }
  candidate_score /= static_cast<float>(discrete_scan.size());
  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

最终实时相关性匹配将在图像分辨率与激光扫描半径的精度约束下产生一个最可能的变换，作为ceres匹配的初值，提高ceres匹配的准确度。下一节将介绍ceres匹配的细节。