Cartographer源码解析——位姿推测器PoseExtrapolator

这一章主要了解外推器，上一章讲到激光点云畸变的矫正，其中最关键的是推测估计出一帧点云中每一个激光点所对应的机器人的位姿。这里就用到了外推器去推测每个发射点的位置。

具体代码调用如下：

    // Step: 2 预测出每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

  if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

  // Drop any returns below the minimum range and convert returns beyond the
  // maximum range into misses.
  // 对每个数据点进行处理
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    // 获取在tracking frame 下点的坐标
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    // 将点云的origins坐标转到 local slam 坐标系下
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    
    // Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);

首先给外推的位姿std::vector<transform::Rigid3f> range_data_poses;进行赋值，填充的元素为extrapolator_->ExtrapolatePose(time_point).cast<float>()，其中time_point为每一个激光点所对应的时间戳，由此可见每一个点都是由位姿推测器推测出来的，用于下面的激光点云矫正。

下面就开始深入了解外推器是如何进行外推位姿的，首先从ExtrapolatePose这个函数讲起。函数代码实现如下：

// 预测得到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;
}

 代码逐行分析：

CHECK_GE(time, newest_timed_pose.time);

首先检查要外推的time是否大于等于上一次外推的pose的时间

if (cached_extrapolated_pose_.time != time) {

对某个时间点进行不重复的外推。排除已经外推过的time。

既然要外推位姿pose，那么就是两方面，一个是平移translation，一个是旋转rotation，首先看平移translation。

   const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();

这个平移就是在先前外推位姿的平移newest_timed_pose.pose.translation()的基础上，再加上一个

平移的变化量ExtrapolateTranslation(time) 。对ExtrapolateTranslation这个函数进行分析：

// 返回从最后一个位姿的时间 到time时刻 的tracking frame在local坐标系下的平移量
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);
      
  // 使用tracking frame 在 local坐标系下的线速度 乘以时间 得到平移量的预测
  if (odometry_data_.size() < 2) {
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  // 如果不使用里程计就使用通过pose计算出的线速度
  return extrapolation_delta * linear_velocity_from_odometry_;
}

代码逐行分析：

const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const double extrapolation_delta =
      common::ToSeconds(time - newest_timed_pose.time);

首先把带有时间戳位姿队列的最后一个数据赋给newest_timed_pose，作为位姿队列中最新的时间位姿。然后计算我们要外推的time与newest_timed_pose.time的差值作为Δt。

  if (odometry_data_.size() < 2) {
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  // 如果不使用里程计就使用通过pose计算出的线速度
  return extrapolation_delta * linear_velocity_from_odometry_;

如果里程计数据队列的个数小于2，那么我们就用外推器里面存储的pose计算出的线速度linear_velocity_from_poses_，然后与Δt相乘得到位移改变量。如果里程计数据队列的数据充足，那么就用里程计算出的一个速度linear_velocity_from_odometry_与Δt相乘得到位移的改变量。

平移讲完接下来看旋转rotation：

    const Eigen::Quaterniond rotation =
        newest_timed_pose.pose.rotation() *
        ExtrapolateRotation(time, extrapolation_imu_tracker_.get());

同样是最新的位姿的旋转向量newest_timed_pose.pose.rotation()的基础上，乘以Δrotation,那么如何算Δrotation，可以看到传入了两个实参，不仅有time，而且还有extrapolation_imu_tracker_.get()，对extrapolation_imu_tracker_.get()这个函数进行分析：

// 计算从imu_tracker到time时刻的姿态变化量
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());

  // 更新imu_tracker的状态到time时刻
  AdvanceImuTracker(time,imu_tracker);

  // 通过imu_tracker_获取上一次位姿校准时的姿态
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数
  return last_orientation.inverse() * imu_tracker->orientation();
}

代码逐行分析：

  CHECK_GE(time, imu_tracker->time());

首先检查time是否大于等于imu_tracker->time()

 // 更新imu_tracker的状态到time时刻
  AdvanceImuTracker(time,imu_tracker);

更新imu_tracker的状态到time时刻

// 通过imu_tracker_获取上一次位姿校准时的姿态
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数
  return last_orientation.inverse() * imu_tracker->orientation();

首先是把imu_tracker_->orientation()作为上一次的last_orientation,也就是更新值，其中imu_tracker_是有一个下划线。从这个带下划线的imu_tracker_拿出一个一个朝向角，说明imu_tracker_是一直实时记录保存着最新估计的imu的朝向角的。然后传入的imu_tracker是往前外推的imu_tracker的朝向角。所以是往前外推一下的orientation乘以上一次位姿的orientation的逆，也就是差，得到了Δorientation。最后再乘以最新的旋转newest_timed_pose.pose.rotation()得到time时刻下的旋转rotation。那么我们再来看一下，imu_tracker->orientation()是怎么计算的。也即是AdvanceImuTracker(time, imu_tracker)这个函数。

/**
 * @brief 更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻
 * 
 * @param[in] time 要预测到的时刻
 * @param[in] imu_tracker 给定的先验状态
 */
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  // 检查指定时间是否大于等于 ImuTracker 的时间
  CHECK_GE(time, imu_tracker->time());

  // 不使用imu 或者 预测时间之前没有imu数据的情况
  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数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定
    
    // 预测当前时刻的姿态与重力方向
    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_);
    return;
  }

  // imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time);
  }

  // c++11: std::lower_bound() 是在区间内找到第一个大于等于 value 的值的位置并返回, 如果没找到就返回 end() 位置
  // 在第四个参数位置可以自定义比较规则,在区域内查找第一个 **不符合** comp 规则的元素

  // 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->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;
      });

  // 然后依次对imu数据进行预测, 以及添加观测, 直到imu_data_的时间大于等于time截止
  while (it != imu_data_.end() && it->time < time) {
    // 预测出当前时刻的姿态与重力方向
    imu_tracker->Advance(it->time);
    // 根据线速度的观测,更新重力的方向,并根据重力的方向对上一时刻预测的姿态进行校准
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    // 更新角速度观测
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  // 最后将imu_tracker的状态预测到time时刻
  imu_tracker->Advance(time);
}

代码逐行分析：
 

 CHECK_GE(time, imu_tracker->time());

检查指定时间是否大于等于 ImuTracker 的时间

// 不使用imu 或者 预测时间之前没有imu数据的情况
  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数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定
    
    // 预测当前时刻的姿态与重力方向
    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_);
    return;
  }

如果不使用imu 或者 预测时间之前没有imu数据的情况，执行花括号里面的语句。

首先了解imu_tracker->Advance(time)

/**
 * @brief 预测出time时刻的姿态与重力方向
 * 
 * @param[in] time 要预测的时刻
 */
void ImuTracker::Advance(const common::Time time) {
  CHECK_LE(time_, time);
  const double delta_t = common::ToSeconds(time - time_);
  // 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态
  orientation_ = (orientation_ * rotation).normalized();

  // 根据预测出的姿态变化量,预测旋转后的线性加速度的值
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  // 更新时间
  time_ = time;
}

代码逐行分析：

 CHECK_LE(time_, time);

检查上一次的time_是否小于等于要预测的time

const double delta_t = common::ToSeconds(time - time_);

计算要预测的时间与上一次记录时间的差值

 // 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));

上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数，其中imu_angular_velocity_是imu的角速度。如果不用imu了，那么他就是0，那么rotation就是0。

// 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态
  orientation_ = (orientation_ * rotation).normalized();

使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态，若rotation为0，那么orientation_就保持不变。

// 根据预测出的姿态变化量,预测旋转后的线性加速度的值
  gravity_vector_ = rotation.conjugate() * gravity_vector_;

更新一下重力向量的表示。gravity_vector_就是考虑重力对齐的关系。主要考虑的是imu航向角是否与水平面水平，如果不是，那么就要将重力划分到imu坐标系中。

time_ = time;

最后将time_更新为本次预测的时间

接着回溯到之前的函数：

// 使用 假的重力数据对加速度的测量进行更新
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());

其中imu_tracker是外推器的imu_tracker。这一步是添加imu的线速度加速度观测。因为我们假设我们没有imu数据，再进一步假设我们在2D平面上运行。那么我们只有一个在Z方向上的重力向量Eigen::Vector3d::UnitZ()。

/**
 * @brief 更新线性加速度的值,并根据重力的方向对上一时刻的姿态进行校准
 * 
 * @param[in] imu_linear_acceleration imu的线加速度的大小
 */
void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) {
  // Update the 'gravity_vector_' with an exponential moving average using the
  // 'imu_gravity_time_constant'.
  // 指数滑动平均法 exponential moving average
 
  // Step: 1 求delta_t, delta_t初始时刻为infinity, 之后为time_-last_linear_acceleration_time_
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;

  // Step: 2 求alpha, alpha=1-e^(-delta_t/10)
  // delta_t越大, alpha越大
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);

  // Step: 3 将之前的线加速度与当前传入的线加速度进行融合, 这里采用指数滑动平均法

  // 指数来确定权重, 因为有噪声的存在, 时间差越大, 当前的线性加速度的权重越大
  // 这里的gravity_vector_改成线性加速度更清晰一些
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
      
  // Change the 'orientation_' so that it agrees with the current
  // 'gravity_vector_'.
  // Step: 4 求得 线性加速度的值 与 由上一时刻姿态求出的线性加速度 间的旋转量
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());

  // Step: 5 使用这个旋转量来校准当前的姿态
  orientation_ = (orientation_ * rotation).normalized();

  // note: glog CHECK_GT: 第一个参数要大于第二个参数
  // 如果线性加速度与姿态均计算完全正确,那这二者的乘积应该是 0 0 1
  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}

代码逐行分析：

// Step: 1 求delta_t, delta_t初始时刻为infinity, 之后为time_-last_linear_acceleration_time_
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;

求delta_t, delta_t初始时刻为infinity, 之后将time_赋给last_linear_acceleration_time_做一下更新。

  // Step: 2 求alpha, alpha=1-e^(-delta_t/10)
  // delta_t越大, alpha越大
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);

  // Step: 3 将之前的线加速度与当前传入的线加速度进行融合, 这里采用指数滑动平均法

  // 指数来确定权重, 因为有噪声的存在, 时间差越大, 当前的线性加速度的权重越大
  // 这里的gravity_vector_改成线性加速度更清晰一些
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

指数平均滤波的权值是以指数式递减的
指数移动平均无论多旧的数据其总会被给予一定的权值
参考：https://blog.csdn.net/iceboy314159/article/details/89716290
相关链接：https://zhuanlan.zhihu.com/p/76188487

 将之前的线加速度与当前传入的线加速度进行融合。其中imu_linear_acceleration为imu的线加速度，IMU短时间内角速度还是比较准确的。 所以Δt越大，我们越不相信之前的角速度(1. - alpha) * gravity_vector_，就越相信当前的测量值alpha * imu_linear_acceleration。

// Step: 4 求得 线性加速度的值 与 由上一时刻姿态求出的线性加速度 间的旋转量
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());

FromTwoVectors就是从一个向量转到另一个向量是怎么旋转的。第一个向量是重力向量gravity_vector_。第二个向量是orientation_（四元数）共轭 也就是旋转的逆。就是把这个四元数转换成向量的形式。Z轴上的向量Eigen::Vector3d::UnitZ()都旋转到 orientation_.conjugate()这个方向上。变成这么一个朝向上的向量。

这里画图解释一下。首先假设我们有一个车，车体自己有一个坐标系以及航向。蓝色线为水平线

然后再画出我们已经算出来的重力向量。垂直于水平面。

我们假设算出的重力向量很准。然后假设我们还知道航向与水平面的夹角θ。首先我们要知道，航向的四元数的共轭向量，本质就是旋转的逆。正向旋转是逆时针，反向旋转是顺时针。

如果这个旋转的逆，乘以这个UnitZ ,也就是意味着UnitZ 顺时针旋转θ角度。 

 

绿色线为旋转之后的UnitZ。然后我们再求出UnitZ与重力向量的旋转向量，再将这个旋转向量乘以航向角，来纠正航向角，航向角向下旋转的角度就是新的UnitZ与重力向量之间的夹角。实现代码如下： 

  // Step: 5 使用这个旋转量来校准当前的姿态
  orientation_ = (orientation_ * rotation).normalized();

 效果如下：紫色为纠正后的航向角

使用重力加速度来纠正我们估计的朝向。
怎么纠正呢，理论上来说，我们车体坐标系上的Z轴上的单位向量，乘以航向角的逆，也就是逆航向角进行旋转，应该是跟重量向量方向是重合的，如果不重合，那么就对航向角进行矫正，矫正的大小就是旋转后的Z轴与重力向量之间的旋转向量rotation。
这就是使用重力加速度来纠正估计航向的原理

  // note: glog CHECK_GT: 第一个参数要大于第二个参数
  // 如果线性加速度与姿态均计算完全正确,那这二者的乘积应该是 0 0 1
  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);

orientation_ * gravity_vector_是对重力向量的矫正，让重力向量的精度更高，尽可能的垂直与水平面，之后，判断校正后的重力向量的z分量是否存在，存在是否大于0，理论上重力向量是朝上的，所以z的值是大于0的。

重力向量为什么朝上，重力和器件之间作用力和反作用力，重力向下，测量值朝上。

 回溯到之前的代码，接下来就是添加imu的角速度观测：

// 只能依靠其他方式得到的角速度进行测量值的更新
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;

进入到AddImuAngularVelocityObservation这个函数：

// 更新角速度
void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity) {
  imu_angular_velocity_ = imu_angular_velocity;
}

根据的三目运算符判断传给AddImuAngularVelocityObservation的实参是angular_velocity_from_poses_还是angular_velocity_from_odometry_，并直接将传进来的值对imu_angular_velocity_进行更新。最后AdvanceImuTracker这个函数的整体逻辑分析结束，注意这里的形参imu_tracker就是 extrapolator->imu_tracker_ 。

这里需要引起注意的一点就是在不使用imu或者预测时间之前没有imu数据的情况下，imu_angular_velocity_一定就是0吗？其实不一定。上面代码已经给出了imu_angular_velocity_由两个来源，如果里程计的数据很少，那么就从之前存储的位姿中计算出一个角速度，如果里程计速度够多，那么就从里程计中算出一个角速度。所以我们的IMU数据并不一定是0 ，有可能是历史的位姿上算出来的速度，有可能是里程计测算出来的速度。

回溯到ImuTracker::Advance这个函数：

/**
 * @brief 预测出time时刻的姿态与重力方向
 * 
 * @param[in] time 要预测的时刻
 */
void ImuTracker::Advance(const common::Time time) {
  CHECK_LE(time_, time);
  const double delta_t = common::ToSeconds(time - time_);
  // 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态
  orientation_ = (orientation_ * rotation).normalized();

  // 根据预测出的姿态变化量,预测旋转后的线性加速度的值
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  // 更新时间
  time_ = time;
}

所以ImuTracker::Advance 里面的imu_angular_velocity_ 并不一定是零，因此，其他的变量也并不是不会改变的，当没有IMU数据的时候。rotation 并不是0，orientation_也是一直更新的。
以上是假设没有IMU数据的情况下。其实就是用历史位姿或者里程计更新朝向角和重力向量。
AdvanceImuTracker 重点就是更新形参imu_tracker的角度。更新完角度之后，再看下一行：

// 通过imu_tracker_获取上一次位姿校准时的姿态
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数
  return last_orientation.inverse() * imu_tracker->orientation();

我们要算Δrotation 就是拿imu_tracker_的朝向角与imu_tracker作差就得到了Δrotation。得到的这个旋转ExtrapolateRotation(time, extrapolation_imu_tracker_.get())，再乘到最新的位姿newest_timed_pose.pose.rotation()上去。得到了最新的外推的位姿的旋转。

// 预测得到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;
}

其中，我们再求IMU角速度的时候，当没有IMU角速度，我们可以从历史位姿的角速度angular_velocity_from_poses_或者里程计的角速度angular_velocity_from_odometry_中推算出来，那么这两个速度是什么时候计算的呢？
首先看历史位姿角速度是在哪里更新的。

// 根据pose队列计算tracking frame 在 local坐标系下的线速度与角速度
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
  if (timed_pose_queue_.size() < 2) {
    // We need two poses to estimate velocities.
    return;
  }
  CHECK(!timed_pose_queue_.empty());
  // 取出队列最末尾的一个 Pose,也就是最新时间点的 Pose,并记录相应的时间
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  // 取出队列最开头的一个 Pose, 也就是最旧时间点的 Pose,并记录相应的时间
  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
  const auto oldest_time = oldest_timed_pose.time;
  // 计算两者的时间差
  const double queue_delta = common::ToSeconds(newest_time - oldest_time);
  // 如果时间差小于pose_queue_duration_(1ms), 不进行计算
  if (queue_delta < common::ToSeconds(pose_queue_duration_)) {
    LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
                 << queue_delta << " s";
    return;
  }
  const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
  const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
  // 平移量除以时间得到 tracking frame 在 local坐标系下的线速度
  linear_velocity_from_poses_ =
      (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
  // 角度变化量除以时间得到角速度得到 tracking frame 在 local坐标系下的角速度
  angular_velocity_from_poses_ =
      transform::RotationQuaternionToAngleAxisVector(
          oldest_pose.rotation().inverse() * newest_pose.rotation()) /
      queue_delta;
}

代码逐行分析：

  if (timed_pose_queue_.size() < 2) {
    // We need two poses to estimate velocities.
    return;
  }

最少需要2个pose才能够估计速度。

CHECK(!timed_pose_queue_.empty());

检查时间位姿队列是否为空，为空就会终止程序。

// 取出队列最末尾的一个 Pose,也就是最新时间点的 Pose,并记录相应的时间
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  // 取出队列最开头的一个 Pose, 也就是最旧时间点的 Pose,并记录相应的时间
  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
  const auto oldest_time = oldest_timed_pose.time;
  // 计算两者的时间差
  const double queue_delta = common::ToSeconds(newest_time - oldest_time);

取出timed_pose_queue_队列首尾元素和相应的时间，并计算两者的时间差。

  // 如果时间差小于pose_queue_duration_(1ms), 不进行计算
  if (queue_delta < common::ToSeconds(pose_queue_duration_)) {
    LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
                 << queue_delta << " s";
    return;
  }

队列中首尾元素的时间差要大于等于1ms才进行计算。

  const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
  const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
  // 平移量除以时间得到 tracking frame 在 local坐标系下的线速度
  linear_velocity_from_poses_ =
      (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
  // 角度变化量除以时间得到角速度得到 tracking frame 在 local坐标系下的角速度
  angular_velocity_from_poses_ =
      transform::RotationQuaternionToAngleAxisVector(
          oldest_pose.rotation().inverse() * newest_pose.rotation()) /
      queue_delta;

定义两个变量分别为首尾元素的位姿pose，并将位姿的平移量做差，再除以时间差就得到了一个线速度。再将首位元素位姿的四元数求逆，乘以尾部元素的四元数得到一个二者之间的差，并将四元数的差转换为欧拉角，除以时间得到角速度。

最后看一下UpdateVelocitiesFromPoses()这个函数是在哪里进行调用更新的。

// 将扫描匹配后的pose加入到pose队列中,计算线速度与角速度,并将imu_tracker_的状态更新到time时刻
void PoseExtrapolator::AddPose(const common::Time time,
                               const transform::Rigid3d& pose) {
  // 如果imu_tracker_没有初始化就先进行初始化
  if (imu_tracker_ == nullptr) {
    common::Time tracker_start = time;
    if (!imu_data_.empty()) {
      tracker_start = std::min(tracker_start, imu_data_.front().time);
    }
    // imu_tracker_的初始化
    imu_tracker_ =
        absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
  }

  // 在timed_pose_queue_中保存pose
  timed_pose_queue_.push_back(TimedPose{time, pose});

  // 保持pose队列中第二个pose的时间要大于 time - pose_queue_duration_
  while (timed_pose_queue_.size() > 2 && // timed_pose_queue_最少是2个数据
         timed_pose_queue_[1].time <= time - pose_queue_duration_) {
    timed_pose_queue_.pop_front();
  }

  // 根据加入的pose计算线速度与角速度
  UpdateVelocitiesFromPoses();

  // 将imu_tracker_更新到time时刻
  AdvanceImuTracker(time, imu_tracker_.get());

  // pose队列更新了,之前imu及里程计数据已经过时了
  // 因为pose是匹配的结果,之前的imu及里程计数据是用于预测的,现在结果都有了,之前的用于预测的数据肯定不需要了
  TrimImuData();
  TrimOdometryData();

  // 用于根据里程计数据计算线速度时姿态的预测
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  // 用于位姿预测时的姿态预测
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

是在AddPose()中调用的，那么AddPose又在哪里调用的呢？

src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& gravity_aligned_range_data,
    const transform::Rigid3d& gravity_alignment,
    const absl::optional<common::Duration>& sensor_duration) {
  // 如果处理完点云之后数据为空, 就报错. 使用单线雷达时不要设置min_z
  if (gravity_aligned_range_data.returns.empty()) {
    LOG(WARNING) << "Dropped empty horizontal range data.";
    return nullptr;
  }

  // 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);

  if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }

  // 将二维坐标旋转回之前的姿态
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 校准位姿估计器
  extrapolator_->AddPose(time, pose_estimate);

前端我们经过ScanMatch之后得到一个pose_estimate_2d，我们再将这个Pose放到外推器里面。刚才不是讲了一个外推器里面的有一个位姿队列，然后就把这个位姿存储到位姿队列中去。

进入AddPose这个函数后调用UpdateVelocitiesFromPoses()，根据加入的pose计算线速度与角速度。

前端：我们看完了从pose里面算出来的速度，再看从odom算出来的角速度。
首先再回忆一下前端的整个过程，首先来了一帧激光，就用这一帧激光建立一个子图，然后就是运动，运动到哪个位置就用外推器计算出来大概的位置，大概是用里程计去计算大概的位置。计算出来之后，我们拿着当前的激光去跟之前建立的子图做匹配，进一步匹配精细化我跟刚刚匹配的位姿。匹配得到了位姿之后，就将位姿放到外推器里面，同时在这个位姿上将新的激光插入到子图上，这就是我们的前端了

 接下来再看根据odom得到角速度angular_velocity_from_odometry_是如何进行计算的。

// 向odom数据队列中添加odom数据,并进行数据队列的修剪,并计算角速度与线速度
void PoseExtrapolator::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  CHECK(timed_pose_queue_.empty() ||
        odometry_data.time >= timed_pose_queue_.back().time);
  odometry_data_.push_back(odometry_data);

  // 修剪odom的数据队列
  TrimOdometryData();
  // 数据队列中至少有2个数据
  if (odometry_data_.size() < 2) {
    return;
  }

  // TODO(whess): Improve by using more than just the last two odometry poses.
  // Compute extrapolation in the tracking frame.
  // 取最新与最老的两个里程计数据
  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  // 最新与最老odom数据间的时间差
  const double odometry_time_delta =
      common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
  // 计算两个位姿间的坐标变换
  const transform::Rigid3d odometry_pose_delta =
      odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;

  // 两个位姿间的旋转量除以时间得到 tracking frame 的角速度
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;
  if (timed_pose_queue_.empty()) {
    return;
  }
  // 平移量除以时间得到 tracking frame 的线速度, 只在x方向有数值
  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;

  // 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到time时刻的姿态变化量
  // 得到预测的 最新里程计数据时刻 tracking frame 在 local 坐标系下的姿态
  const Eigen::Quaterniond orientation_at_newest_odometry_time =
      timed_pose_queue_.back().pose.rotation() *
      ExtrapolateRotation(odometry_data_newest.time,
                          odometry_imu_tracker_.get());
  // 将tracking frame的线速度进行旋转, 得到 local 坐标系下 tracking frame 的线速度
  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;
}

代码逐行分析：

CHECK(timed_pose_queue_.empty() ||
        odometry_data.time >= timed_pose_queue_.back().time);
  odometry_data_.push_back(odometry_data);

检查timed_pose_queue_是否为空 或者 要预估的里程计数据时间是否大于timed_pose_queue_队列末尾pose的时间。若至少有一个为真，就将里程计数据添加到odometry_data_中。

  // 修剪odom的数据队列
  TrimOdometryData();

看看该函数的内部实现：

// 修剪odom的数据队列,丢掉过时的odom数据
void PoseExtrapolator::TrimOdometryData() {
  // 保持odom队列中第二个数据的时间要大于最后一个位姿的时间, odometry_data_最少是2个
  while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
         odometry_data_[1].time <= timed_pose_queue_.back().time) {
    odometry_data_.pop_front();
  }
}

while循环需满足三个条件：

①odometry_data_.size() > 2里程计队列元素的个数大于2

②!timed_pose_queue_.empty() 带有时间戳位姿的队列不为空

③odometry_data_[1].time <= timed_pose_queue_.back().time里程计队列第二个数据的时间戳小于等于 timed_pose_queue_ 队列中最后一个数据的时间戳。

只有满足这三个条件，就会将odometry_data_队列中首个元素进行抛出，丢掉过时的odom数据。

  // 数据队列中至少有2个数据
  if (odometry_data_.size() < 2) {
    return;
  }

保证odometry_data_队列中元素数据的个数至少为2个。

// 取最新与最老的两个里程计数据
  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  // 最新与最老odom数据间的时间差
  const double odometry_time_delta =
      common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
  // 计算两个位姿间的坐标变换
  const transform::Rigid3d odometry_pose_delta =
      odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;

  // 两个位姿间的旋转量除以时间得到 tracking frame 的角速度
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;
  if (timed_pose_queue_.empty()) {
    return;
  }

 拿到最老和最新的odom数据，然后再计算得到Δtime，根据最新和最老的位姿算出Δpose 。然后再求出角速度。最后判断timed_pose_queue_是否为空，为空就退出。

  // 平移量除以时间得到 tracking frame 的线速度, 只在x方向有数值
  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;

根据求得的里程计两个位姿间变换的平移量除以Δtime得到线速度，这个线速度linear_velocity_in_tracking_frame_at_newest_odometry_time 的意思是在最新的里程计时间下的在tracking_frame坐标系下的线速度。

  // 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到time时刻的姿态变化量
  // 得到预测的 最新里程计数据时刻 tracking frame 在 local 坐标系下的姿态
  const Eigen::Quaterniond orientation_at_newest_odometry_time =
      timed_pose_queue_.back().pose.rotation() *
      ExtrapolateRotation(odometry_data_newest.time,
                          odometry_imu_tracker_.get());

上面代码算的是刚刚使用到的朝向，朝向怎么算的，跟之前的外推器计算的朝向差不多，就是外推器存储的最后的一个pose，timed_pose_queue_.back().pose.rotation(),在这个基础上，乘以一个Δrotation，ExtrapolateRotation(odometry_data_newest.time, odometry_imu_tracker_.get())。 但是这个Δrotation 是怎么算的呢？他使用了一个odometry_imu_tracker_。 当然这函数是一样的，只不过参数imu_tracker不一样。

// 将tracking frame的线速度进行旋转, 得到 local 坐标系下 tracking frame 的线速度
  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;

 将tracking frame的线速度进行旋转, 得到 local 坐标系下 tracking frame 的线速度。

为了实时的把里程计的速度转化到local坐标系下，我们就单独用了odometry_imu_tracker_。

我们再回顾一下imu_tracker_。

src/cartographer/cartographer/mapping/pose_extrapolator.h

  std::unique_ptr<ImuTracker> imu_tracker_;               // 保存与预测当前姿态
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;      // 用于计算里程计的姿态的ImuTracker
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_; // 用于预测姿态的ImuTracker

 extrapolation_imu_tracker_是用来做外推的主力。odometry_imu_tracker_只为了用于把里程计的速度转化为local坐标系里去。imu_tracker_还没讲到。但他时刻记录着过程中，起到的作用，记录最近一次pose的朝向角，我们总是在这个基础上，计算Δrotation。

我们首先想想AddPose什么时候用的，就是ScanMatch结束之后，我们得到了一个精细的Pose，然后将这个Pose传入到AddPose函数中，放入到外推器里面的位姿队列中。

void PoseExtrapolator::AddPose(const common::Time time,
                               const transform::Rigid3d& pose) {
	......
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

odometry_imu_tracker_的作用是只要来一帧里程计数据，就要更新里程计的速度，但是速度要转化到local坐标系里面去，怎么转化呢？就是算Δrotation ，再添加到最新的pose上面去。为什么两个位姿可以相减得到ExtrapolateRotation(time, extrapolation_imu_tracker_.get())，因为每一次数据来之后，我们得到一个很精细的pose。完了我们把imu_tracker_记录的角度给统一到odometry_imu_tracker_，extrapolation_imu_tracker_中去。那么下次我们再计算出角度之后，就去减掉imu_tracker_。就相当于减掉自己的上一时刻的位姿。imu_tracker_记录的是每一次我们ScanMatch结束之后，我们记录的精细的角度。他始终记录的是结果。odometry_imu_tracker_，extrapolation_imu_tracker_是两个工具人，始终在imu_tracker_的基础上去计算Δrotation，然后再把Δrotation 放置在存储pose队列里面最后的一个pose上面去，得到所需要时刻的pose。