cartographer代码学习笔记-ScanMatch

最新推荐文章于 2025-05-28 19:55:45 发布

Running小学生

最新推荐文章于 2025-05-28 19:55:45 发布

阅读量539

点赞数 9

CC 4.0 BY-SA版权

分类专栏： cartographer 文章标签：学习笔记

本文链接：https://blog.youkuaiyun.com/weixin_28683491/article/details/134882011

cartographer 专栏收录该内容

20 篇文章

订阅专栏

博客主要是ScanMatch代码解析的学习笔记。介绍了在AddAccumulatedRangeData函数中通过调用ScanMatch获取位置，详细解析了RealTimeCorrelativeScanMatcher2D的SearchParameters、GenerateRotatedScans等部分，还提及CeresScanMatcher2D::Match，给出了相关参考资料。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

在AddAccumulatedRangeData函数中通过调用ScanMatch获取位置

  // local map frame <- gravity-aligned frame
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
  if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }

1、 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)//过滤后的点云数据
{
  if (active_submaps_.submaps().empty()) { //暂无active_submaps_时
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }
  //存在active_submaps_，取子图
  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;
//如果实时匹配参数为true
  if (options_.use_online_correlative_scan_matching()) {
    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);
  }

  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;
}

1.1 RealTimeCorrelativeScanMatcher2D

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud,
    const ProbabilityGrid& probability_grid,
    transform::Rigid2d* pose_estimate) const {
  CHECK_NOTNULL(pose_estimate);
//将点云旋转到预测的方向上
  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, probability_grid.limits().resolution());
//旋转点云数据
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  //旋转后的点云加平移
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      probability_grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
   // 生成所有的候选解                        
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
     //计算候选解得分
  ScoreCandidates(probability_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; //返回得分
}

1.1.1 SearchParameters

构建搜索框参数

搜索角度步长论文中计算公式
在这里插入图片描述

SearchParameters::SearchParameters(const double linear_search_window,
                                   const double angular_search_window,
                                   const sensor::PointCloud& point_cloud,
                                   const double resolution)
    : resolution(resolution) {
  // We set this value to something on the order of resolution to make sure that
  // the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;
  for (const Eigen::Vector3f& point : point_cloud) {
    const float range = point.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }
  const double kSafetyMargin = 1. - 1e-3;
  //角度分辨率计算 根据论文公式计算
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));
  //计算搜索角度的个数                                     
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
      //搜索扩大2倍
  num_scans = 2 * num_angular_perturbations + 1;
//x,y搜索栅格数的计算
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
  linear_bounds.reserve(num_scans);
  /*
    struct LinearBounds {
    int min_x;
    int max_x;
    int min_y;
    int max_y;
  };
  */
  for (int i = 0; i != num_scans; ++i) {
  //根据num_linear_perturbations 设定区间范围
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations});
  }
}

1.1.2 GenerateRotatedScans

根据search_parameters生成旋转后的点云

std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud& point_cloud,
    const SearchParameters& search_parameters) {
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans);//设置旋转点云大小

//设定搜索起始角度
  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
           delta_theta += search_parameters.angular_perturbation_step_size) {//角度递增
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))));
  }
  return rotated_scans;
}

按照角度旋转后的旋转向量

transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))

将点云按照旋转向量进行旋转得到旋转后的点云数据

sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ())))

1.1.3 DiscretizeScans

将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引typedef std::vector<Eigen::Array2i> DiscreteScan2D;

std::vector<DiscreteScan2D> DiscretizeScans(
    const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
    const Eigen::Translation2f& initial_translation) {
  std::vector<DiscreteScan2D> discrete_scans;
  discrete_scans.reserve(scans.size());
  for (const sensor::PointCloud& scan : scans) {
    discrete_scans.emplace_back();
    discrete_scans.back().reserve(scan.size());
    for (const Eigen::Vector3f& point : scan) {
      const Eigen::Vector2f translated_point =
          Eigen::Affine2f(initial_translation) * point.head<2>();
          
      discrete_scans.back().push_back(
          map_limits.GetCellIndex(translated_point));
    }
  }
  return discrete_scans; //返回的就直接是 num_scans 帧各个点云在栅格地图中的索引。
}

每个旋转后的点云加上平移得到world的坐标

      const Eigen::Vector2f translated_point =
          Eigen::Affine2f(initial_translation) * point.head<2>();

获取激光点在栅格下的索引值

map_limits.GetCellIndex(translated_point)

  Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
    // Index values are row major and the top left has Eigen::Array2i::Zero()
    // and contains (centered_max_x, centered_max_y). We need to flip and
    // rotate.
    return Eigen::Array2i(
        common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
        common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
  }

1.1.4 GenerateExhaustiveSearchCandidates

std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
    const SearchParameters& search_parameters) const {
  int num_candidates = 0;
  // 计算候选点总量
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    const int num_linear_x_candidates =
        (search_parameters.linear_bounds[scan_index].max_x -
         search_parameters.linear_bounds[scan_index].min_x + 1);
    const int num_linear_y_candidates =
        (search_parameters.linear_bounds[scan_index].max_y -
         search_parameters.linear_bounds[scan_index].min_y + 1);
    num_candidates += num_linear_x_candidates * num_linear_y_candidates;
  }
  std::vector<Candidate2D> candidates;
  candidates.reserve(num_candidates);
  //遍历所有点云数据
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
       //一个xy位置上对应一个点云数据
    for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
         x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
         ++x_index_offset) {
      for (int y_index_offset =
               search_parameters.linear_bounds[scan_index].min_y;
           y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
           ++y_index_offset) {
        candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
                                search_parameters);
      }
    }
  }
  CHECK_EQ(candidates.size(), num_candidates);
  return candidates;
}

1.1.5ScoreCandidates

计算候选者得分，暴力匹配

void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
    const ProbabilityGrid& probability_grid,
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const {
   //遍历所有候选解
  for (Candidate2D& candidate : *candidates) {
    candidate.score = 0.f;
    for (const Eigen::Array2i& xy_index :
         discrete_scans[candidate.scan_index]) { //discrete_scans存储的是对应栅格索引
      const Eigen::Array2i proposed_xy_index( //激光索引值+候选者在xy上的平移
          xy_index.x() + candidate.x_index_offset,
          xy_index.y() + candidate.y_index_offset);
      const float probability =
          probability_grid.GetProbability(proposed_xy_index); //根据索引获取对应栅格概率
      candidate.score += probability; 
    }
     //计算候选解得分
    candidate.score /= 
        static_cast<float>(discrete_scans[candidate.scan_index].size()); 
    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()));
    CHECK_GT(candidate.score, 0.f);
  }