cartographer代码学习-第一帧雷达数据到来时栅格地图的原点

在cartographer中，存在map与sub_map两种地图，对于每一张sub_map，它的地图原点为当前机器人所在的map坐标系下的位姿。那么对于初始化状态时，第一帧雷达数据到来时建立的第一张sub_map的原点是如何确定的？是否是默认的（0，0，0）呢？

根据前面学习的传感器的数据走向可知获取的laser数据会给到LocalTrajectoryBuilder2D::ScanMatch函数进行扫描匹配，展开看一下这个函数：

/**
 * @brief 进行扫描匹配
 * 
 * @param[in] time 点云的时间
 * @param[in] pose_prediction 先验位姿
 * @param[in] filtered_gravity_aligned_point_cloud 匹配用的点云
 * @return std::unique_ptr<transform::Rigid2d> 匹配后的二维位姿
 */
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()) {
    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;

  // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
  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进行扫描匹配
  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);
  }
  // 返回ceres计算后的位姿
  return pose_observation;
}

在这里的函数最前面我们可以看到它做了一个判断，当当前没有子图存在时，执行：

absl::make_unique<transform::Rigid2d>(pose_prediction)

make_unique是一个智能指针，这里相当于直接返回了一个先验位姿pose_prediction。这个pose_prediction是如何获取的呢？我们可以再看一下LocalTrajectoryBuilder2D::AddAccumulatedRangeData函数中这一部分：

// Computes a gravity aligned pose prediction.
  // 进行位姿的预测, 先验位姿
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  // 将三维位姿先旋转到姿态为0, 再取xy坐标将三维位姿转成二维位姿
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

  // Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloud
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }

  // local map frame <- gravity-aligned frame
  // 扫描匹配, 进行点云与submap的匹配
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

最前面ExtrapolatePose是一个位姿推测器，根据各个传感器的先验信息推测出当前机器人所在位姿（IMU、里程计、GPS等）。注意到这里的位姿推测器需要传参一个时间戳，我们展开看一下它的具体原理：

// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  CHECK_GE(time, newest_timed_pose.time);
  // 如果本次预测时间与上次计算时间相同 就不再重复计算
  if (cached_extrapolated_pose_.time != time) {
    // 预测tracking frame在local坐标系下time时刻的位置
    const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
    // 预测tracking frame在local坐标系下time时刻的姿态
    const Eigen::Quaterniond rotation =
        newest_timed_pose.pose.rotation() *
        ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
    cached_extrapolated_pose_ =
        TimedPose{time, transform::Rigid3d{translation, rotation}};
  }
  return cached_extrapolated_pose_.pose;
}

对于time时刻的位姿，是根据timed_pose_queue_队列中最新的时间下的位姿加上平移预测以及旋转的预测来计算该时间戳下的位姿。对于timed_pose_queue_队列，我们可以先看一下LocalTrajectoryBuilder2D::InitializeExtrapolator函数关于位姿推测器的初始化：

// 初始化位姿推测器
  extrapolator_ = absl::make_unique<PoseExtrapolator>(
      ::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
                                              .constant_velocity()
                                              .pose_queue_duration()), // 0.001s
      options_.pose_extrapolator_options()
          .constant_velocity()
          .imu_gravity_time_constant()); // 10
  // 添加初始位姿
  extrapolator_->AddPose(time, transform::Rigid3d::Identity());

在这里的最后面我们可以看到，当算法初始化完一个位姿推测器后，直接向其传入了一个初始化全零的坐标，作为位姿推测器的初值。但是这里的全零的坐标并不一定是第一帧激光雷达到达是位姿推测器输出的pose_prediction，这是因为它的timed_pose_queue_队列是会随着其他传感器更新的，例如IMU一般频率会比laser高很多，可能在激光数据到来之前timed_pose_queue_就已经被更新了。

后面：

const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

则是将推测器推测的3D的位姿转换成2D的。如果这里的IMU本身不准，也可能会存在XY方向的运动分量，导致位置发生一定变化。

因此，在cartographer中，第一帧的雷达数据到来时的栅格地图的原点其实不一定是（0，0，0），而是来自于初始化到激光数据到来的过程中位姿推测器的变化，如果位姿推测器的数据有所变化，则初始值会随之该变。