Cartographer代码阅读笔记：Local SLAM的实现

本人热衷于机器人导航算法研究，希望与志同道合伙伴一起探讨有关技术。

前沿

本文将研究一下Cartographer中前端实现的过程，其主要流程为：将激光雷达数据和位姿推理器（Pose Extrapolator）估计的位姿输入到扫描匹配器（Scan Matching），并使用Ceres库完成优化计算，得到机器人位姿的观测值。观测值一方面反馈给位姿推理器用于修正其估计值，另一方面提供给运动滤波器用于判断机器人是否运动了。如果运动了，就将此刻的点云数据插入到当前正在维护的子图中。同时输出插入的结果，包含了位姿、子图、激光扫描数据和时间信息。

Cartographer有关其他分析文章：回到目录

有关文件在：cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

接下来探讨一下每个函数的具体实现过程：

代码解析

AddRangeData函数分析

/** 
 * @brief 激光雷达扫描数据处理
 * @param sensor_id 激光雷达的索引
 * @param unsynchronized_data 未经时间同步的扫描数据
 * @return 扫描匹配的结果
 */
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
  // 把传入的索引和扫描数据记录在数据收集器中range_data_collator_，同时获得一个做了时间同步的扫描数据
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }

  // 将同步时间记录到临时对象time中
  const common::Time& time = synchronized_data.time;

  // 如果配置中设置不使用IMU估计位姿，则在此初始化位姿推理器；
  // 如果设置使用IMU，将在AddImuData()函数中初始化位姿推理器
  if (!options_.use_imu_data()) {
    InitializeExtrapolator(time);
  }

  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return nullptr;
  }

  CHECK(!synchronized_data.ranges.empty());
  // 确保扫描数据中最后一个激光束对应的时间戳大于等于0
  CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);

  // 获取第一个激光束的时间戳
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time[3]);

  // 与位姿推理器的时间比较判断位姿推理器是否完成位姿更新    
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

  // 如果没有累计数据，就用成员变量记录当前时间
  if (num_accumulated_ == 0) {
    accumulation_started_ = std::chrono::steady_clock::now();
  }

  // 创建临时容器range_data_poses，用于记录各个激光束对应的机器人位姿
  std::vector<transform::Rigid3f> range_data_poses;
  range_data_poses.reserve(synchronized_data.ranges.size());
  // 控制警告信息打印
  bool warned = false;

  // 遍历同步后扫描数据中的激光束数据
  for (const auto& range : synchronized_data.ranges) {
    // 计算激光束对应时间
    common::Time time_point = time + common::FromSeconds(range.point_time[3]);

    // 判断当前激光束对应时间戳比位姿推理器最新更新时间滞后，说明位姿推理器有最新的位姿信息。
    if (time_point < extrapolator_->GetLastExtrapolatedTime()) { 
      // 打印警告信息
      if (!warned) {
        LOG(ERROR)
            << "Timestamp of individual range data point jumps backwards from "
            << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
        warned = true;
      }
      // 调整时间为位姿推理器的时间
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }

    // 根据上面计算的时间进行位姿插值。
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

  // 如果程序执行到此还没有累计扫描数据，就重置对象accumulated_range_data_
  if (num_accumulated_ == 0) {
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

  // 进行hit点和miss点的记录
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;

    // 通过左乘机器人位姿，将机器人坐标系下的传感器位姿转换到局部子图的坐标系下
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);

    // 通过左乘机器人位姿，将机器人坐标系下的激光束转换到局部子图的坐标系下
    const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();

    // 激光束的测量距离（原始扫描数据就带有距离信息，这里来回计算一遍有点多余）
    const Eigen::Vector3f delta = hit_in_local - origin_in_local;
    const float range = delta.norm();

    // 根据距离判断是hit还是miss。
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        accumulated_range_data_.misses.push_back(
            origin_in_local +
            options_.missing_data_ray_length() / range * delta);
      }
    }
  }
  ++num_accumulated_;

  // 如果累计的数据足够，则进行匹配。
  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    // 计数器清零
    num_accumulated_ = 0;
    // 获取重力向量。
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));
    // 更新累计扫描数据的原点信息
    accumulated_range_data_.origin = range_data_poses.back().translation();

    // TransformToGravityAlignedFrameAndFilter()函数实现了以重力方向为参考修正累计扫描数据，同时完成了体素滤波
    return AddAccumulatedRangeData(
        time,
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment);
  }
  return nullptr;
}

AddAccumulatedRangeData函数分析

/** 
 * @brief 激光雷达扫描数据处理
 * @param time 当前同步时间
 * @param gravity_aligned_range_data 经过重力修正后的扫描数据
 * @param gravity_alignment 重力方向
 * @return 扫描匹配的结果
 */
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment) {
  if (gravity_aligned_range_data.returns.empty()) {
    LOG(WARNING) << "Dropped empty horizontal range data.";
    return nullptr;
  }

  // 从位姿推理器获得6自由度的位姿
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);

  // 把上面的6自由度位姿投影成3自由度的位姿。作为scan match 的初值。      
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

  // 进行ScanMatch匹配。
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, gravity_aligned_range_data);
  if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }

  // 位姿重新转换成6自由度。
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 更新位姿推理器
  extrapolator_->AddPose(time, pose_estimate);

  // 将扫描数据转换到新的位姿下，即Submap坐标系。
  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));

  // 将转换后的扫描数据插入子图中，返回插入结果。                         
  std::unique_ptr<InsertionResult> insertion_result =
      InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
                       pose_estimate, gravity_alignment.rotation());

  // 记录累积时间
  auto duration = std::chrono::steady_clock::now() - accumulation_started_;
  // 监控延时
  kLocalSlamLatencyMetric->Set(
      std::chrono::duration_cast<std::chrono::seconds>(duration).count());

  // 返回匹配结果
  return common::make_unique<MatchingResult>(
      MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                     std::move(insertion_result)});
}

ScanMatche函数分析

/** 
 * @brief 进行扫描匹配
 * @param time 记录了当前时间
 * @param pose_prediction 位姿推理器预测的位姿
 * @param gravity_aligned_range_data 重力修正过后的扫描数据
 * @return 扫描匹配修正后的位姿
 */
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::RangeData& gravity_aligned_range_data) {
  // 获得一张最新的子图    
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();

  // 临时变量记录初始位姿。
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 构建一个自适应体素滤波器，同时对扫描数据进行滤波处理
  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
      options_.adaptive_voxel_filter_options());
  const sensor::PointCloud filtered_gravity_aligned_point_cloud =
      adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }

  // 判断是否进行real-time CSM匹配
  if (options_.use_online_correlative_scan_matching()) {
    CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
             proto::GridOptions2D_GridType_PROBABILITY_GRID);
    double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *static_cast<const ProbabilityGrid*>(matching_submap->grid()),
        &initial_ceres_pose);
    kFastCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  
  auto pose_observation = common::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;

  // 通过对象ceres_scan_matcher_把扫描匹配问题描述成一个最小二乘问题，使用优化库Ceres库寻优，
  // 得到一个使得观测数据出现概率最大化的位姿估计，位姿估计值记录在pose_observatio中
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);

  // 如果成功求得扫描匹配修正后位姿，就通过kCeresScanMatcherCostMetric,kScanMatcherResidualDistanceMetric
  // 和kScanMatcherResidualAgnleMetric检查一下计算结果
  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost); 

    double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance); 

    double residual_angle = std::abs(pose_observation->rotation().angle() -
                                     pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle); 
  }

  // 返回修正后的位姿
  return pose_observation;
}

InsertIntoSubmap()函数分析

/** 
 * @brief 扫描数据插入当前正在维护的子图
 * @param time 当前同步时间
 * @param range_data_in_local 记录了优化后的位姿估计下的扫描数据
 * @param gravity_aligned_range_data 记录了优化前在重力修正的扫描数据
 * @param pose_estimate 记录了优化后的位姿估计
 * @param gravity_alignment 重力方向
 * @return 扫描匹配的结果
 */
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
    const common::Time time, const sensor::RangeData& range_data_in_local,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& pose_estimate,
    const Eigen::Quaterniond& gravity_alignment) {
  // 通过运动滤波器判断机器人是否发生明显运动，若未发生则直接返回，这样可以减少计算量
  if (motion_filter_.IsSimilar(time, pose_estimate)) {
    return nullptr;
  }

  // 将对象active_submaps_中维护的子图存到临时容器insertion_submaps中
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
  for (const std::shared_ptr<Submap2D>& submap : active_submaps_.submaps()) {
    insertion_submaps.push_back(submap);
  }

  // 把当前扫描数据插入到active_submaps_中
  active_submaps_.InsertRangeData(range_data_in_local);

  // 构建一个自适应体素滤波器对优化前重力修正的扫描数据进行滤波，目的是减少闭环检测的计算量
  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
      options_.loop_closure_adaptive_voxel_filter_options());
  const sensor::PointCloud filtered_gravity_aligned_point_cloud =
      adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);

// 构建InsertionResult对象并返回
  return common::make_unique<InsertionResult>(InsertionResult{
      std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},  // 'high_resolution_point_cloud' is only used in 3D.
          {},  // 'low_resolution_point_cloud' is only used in 3D.
          {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
          pose_estimate}),
      std::move(insertion_submaps)});
}

小结

函数AddRangeData()和它调用的函数AddAccummulatedRangeData()基本控制Local SLAM实现过程。AddRangeData()主要是筛选激光点云并将它们划分为hit点和miss点，AddAccumulatedRangeData()则调用函数ScanMatch()和InsertIntoSubmap()完成扫描匹配和子图更新的操作。

返回顶部 or 回到目录