cartographer中的pose_extrapolator梳理

本文主要目的有二：

1. 弄清extrapolator的涉及到的运动学变量；
2. 弄清odom、imu、scan match对应的数据在pose extrapolator中的对应作用。

PoseExtrapolator::ExtrapolatePose

推算当前给定时间 time 对应的位姿（平移+旋转）。包括调用PoseExtrapolator::ExtrapolateTranslation来计算平移增量以及调用PoseExtrapolator::ExtrapolateRotation来计算旋转增量

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) {
    const Eigen::Vector3d translation =
      //↓当前时刻到newest_timed_pose.time之间的位移。  ↓newest_timed_pose.time时刻的位置
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
        
    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;
}

PoseExtrapolator::ExtrapolateTranslation
主要就是odometry_data_队列的值足够的时候，使用odom推算的linear_velocity_线性速度；否则根据历史节点的位姿来推算linear_velocity_线性速度。然后根据当前时间和位姿队列中最新位姿的时间差，计算这段时间内的平移增量。

Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const double extrapolation_delta =
      common::ToSeconds(time - newest_timed_pose.time);
  if (odometry_data_.size() < 2) {
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  return extrapolation_delta * linear_velocity_from_odometry_;
}

PoseExtrapolator::ExtrapolateRotation

Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  AdvanceImuTracker(time, imu_tracker);
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  return last_orientation.inverse() * imu_tracker->orientation();
}

姿态外推，需要调用函数AdvanceImuTracker（更新imu跟踪器）。

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  if (imu_data_.empty() || time < imu_data_.front().time) {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    //在time时间之前都没有imu数据，这时候需要使用从历史位姿中估计的角速度和假的重力加速度来帮助构建一个稳定的imu跟踪器。
    imu_tracker->Advance(time);
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    LOG(INFO) << "There is no IMU data until 'time': "
                 << time << " s" << "odometry_data_.size() is: "<<odometry_data_.size();
    return;
  }
  //当前的imu跟踪器的数据 早于 imu_data_队列中的起始时间
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    //将imu跟踪器更新到 imu_data_队列中的起始时间
    imu_tracker->Advance(imu_data_.front().time);
  }
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });
  while (it != imu_data_.end() && it->time < time) { //迭代递推到当前时间time
    imu_tracker->Advance(it->time);
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  imu_tracker->Advance(time);
}