cartographer 源码解析(六)

相关链接：

cartographer 源码解析(一)

cartographer 源码解析(二)

cartographer 源码解析(三)

cartographer 源码解析(四)

cartographer 源码解析(五)

cartographer 源码解析(六)

这一章节呢，主要讲外推器。我们上一章节说到激光的畸变与矫正，最关键的是什么呢？其实最关键的就是关于每个发射点的发射位置的估计推测，这里就用到了外推器去推测每个发射点的位置。
local_trajectory_builder_2d.cc

  if (synchronized_data.ranges.empty(std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
	......
	 for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
 	......
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
   	......
    }
  }    
	......
}

其中可以看到每个点都有一个外推的pose------range_data_poses[i]。
再往前追溯，就可以知道，其实每个点都是外推器给推出来的，我们可以找到代码看一下。
local_trajectory_builder_2d.cc

  for (const auto& range : synchronized_data.ranges) {
 	......
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

其中time_point是每一个激光点发射对应的时间戳。那么今天就仔细讲一下这个外推器是怎么进行位姿推算的。首先我们看ExtrapolatePose是怎么实现的。
pose_extrapolator.cc

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

下面进行逐行讲解

CHECK_GE(time, newest_timed_pose.time);

首先检查time是否大于等于外推最新的pose的时间，newest_timed_pose.time。其中，cached_extrapolated_pose_是外推器外推的pose.

if (cached_extrapolated_pose_.time != time) {

这句的意思是在某个时间点上进行不重复的外推。也就是曾经没有外推过的时间，这都是一些时间上的检查。
我们既然要外推pose，那么就是两方面，一个是平移translation，一个是旋转rotation
先看平移。

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

那么这个平移是什么呢？其实就是在先前pose的基础上newest_timed_pose.pose.translation()，再加上一个$\Delta$ ExtrapolateTranslation(time)。
pose_extrapolator.cc

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

我们外推器里面存储了一系列的pose,linear_velocity_from_poses_。比如说滑动窗口，一个10个pose，新来了pose，就把最老的pose给干掉。然后我们用最新的减去最旧的，最后再除以时间差，得到这个速度，也就是linear_velocity_from_poses_，得到了之后呢。然后我们要用我们要外推需要的时间time，减去最新的pose的时间。其中要外推的时间肯定比最新的pose时间大，相减之后得到的时间是$\Delta$t，extrapolation_delta，然后乘以从历史存储得到的速度，也就是linear_velocity_from_poses_，然后得到了在$\Delta$t时间内的平移。这是在里程计数据比较少的情况下， if (odometry_data_.size() < 2)。
但是当里程计数据足够呢？
那么我们就用时间$\Delta$t乘以从里程计获得的速度。代码如下:

return extrapolation_delta * linear_velocity_from_odometry_;

其中linear_velocity_from_odometry_是用里程计算出的一个速度。
如果里程计数据足够，就用里程计做外推，如果没有，就之前存储的位姿来推算速度。
好了，平移说完了，那么就再说说旋转Rotation。

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

同样是最新的位姿的旋转向量newest_timed_pose.pose.rotation()的基础上，乘以$\Delta$rotation,那么如何算$\Delta$rotation，可以看到传入了两个向量，不仅有time，而且还有extrapolation_imu_tracker_.get()，那么extrapolation_imu_tracker_.get()这个是什么东西呢。我们展开来看一下。
pose_extrapolator.h

  std::unique_ptr<ImuTracker> imu_tracker_;
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;

我们可以看到有三个imu_tracker。一个是imu_tracker_，一个是odometry_imu_tracker_，还有一个是extrapolation_imu_tracker_。这具体是干什么的呢？后面我们再讲。我们先回归pose_extrapolator.cc到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();
}

传入的imu_tracker是外推器imu_tracker------extrapolation_imu_tracker_

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

我们不会往历史时刻做外推，所以检查time大于等于 imu_tracker->time()。 AdvanceImuTracker(time, imu_tracker);这个先不展开讲，再看接下来做了什么。

const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();

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

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.
    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;
  }
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of '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) {
    imu_tracker->Advance(it->time);
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  imu_tracker->Advance(time);
}

接下来进行逐行解析吧，首先第一行。还是老套路，检查时间。

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

首先假设我们不直接使用imu，来做轨迹推算，直接用一个比较好的里程计来做推算。imu就是和编码器融合推演出一个比较好的里程计上面。不把imu直接用到cartographer上面。首先我们看这一种情况下是怎么操作的。

  if (imu_data_.empty() || time < imu_data_.front().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_);
    return;

首先看看imu_tracker->Advance(time);是干什么的？
imu_tracker.cc

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_ * rotation).normalized();
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

从函数名Advance中就知道，就是前进到这个时刻time。其中有一个是time_，我们可能就比较困惑了，这个是从哪里来的呢，其实这是imu的编码规范，其实就是类的成员，至于这里是哪个类呢，显然是ImuTracker这个类的成员。这是time_具体是什么呢？其实就是我们一次一次Advance到的时间。可以看到代码最后一行 time_ = time;，这就是我们最后的是时间，赋值给它。

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

而delta_t是我们将要赶到的时间点距离上一次赶到的时间点之间的差。

const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));

其中imu_angular_velocity_是imu的角速度。如果不用imu了，那么他就是0，那么rotation就是0。

orientation_ = (orientation_ * rotation).normalized();

orientation_就保持不变，乘以0还是原来的orientation_

gravity_vector_ = rotation.conjugate() * gravity_vector_;

gravity_vector_就是考虑重力对齐的关系。主要考虑的是imu航向角是否与水平面水平，如果不是，那么就要将重力划分到imu坐标系中。
回过头来，我们看一下Advance到底做了什么？
如果我们有imu数据的时候，也就是有了imu的角速度imu_angular_velocity_。通过这个角速度乘以时间$\Delta$t ，得到一个rotation。有了imu自己的rotation，然后更新自己的重力向量。然后还有什么呢？这一些的ImuTracker Rotation其实就是在算$\Delta$ rotation。然后更新自己的orientation_。
现在imu_tracker->Advance()是干什么，就是更新朝向角，再更新一下重力向量的表示。
然后再接着回溯代码

imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());

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

void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) {
  // Update the 'gravity_vector_' with an exponential moving average using the
  // 'imu_gravity_time_constant'.
  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_;
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
  // Change the 'orientation_' so that it agrees with the current
  // 'gravity_vector_'.
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
  orientation_ = (orientation_ * rotation).normalized();
  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}

虽然我们之前假设没有imu数据，但是当我们有imu数据的时候，我们添加imu线性加速度的作用是什么吧。我们先不管传入的是什么东西，我们先看这个函数吧。

void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) {
    ......
    }

首先填入进入的是一个线性加速度imu_linear_acceleration。主要是做了指数平均滤波$(exponentialmovingaverage,EMA)$，也就是互补滤波

$y_i=(1-\alpha )y_{i-1}+\alpha x_i$
指数平均滤波的权值是以指数式递减的
指数移动平均无论多旧的数据其总会被给予一定的权值
参考：https://blog.csdn.net/iceboy314159/article/details/89716290
相关链接：https://zhuanlan.zhihu.com/p/76188487

首先是一个条件表达式，其中time_我们可以从之前讲的代码中得知已经更新到最新了，last_linear_acceleration_time_是上一次的线性加速度时间，两者想减得到$\Delta t$， last_linear_acceleration_time_ > common::Time::min()这个是验证时间的有效性。

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

​下一步就是刚才引用的指数平均滤波的代码表示，其中imu_gravity_time_constant_是一个常数，可以知道，当$\Delta t$越小的时候，$\alpha$就越大。

const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

其中imu_linear_acceleration为imu的测量值，如果$\Delta$, IMU短时间内角速度还是比较准确的。 所以$\Delta t$越大，我们越不相信之前的角速度 (1. - alpha) * gravity_vector_，就越相信当前的测量值alpha * imu_linear_acceleration。虽然不太准，但这就是重力向量在机器人体内，也就是机体内怎么表示的。

接下来呢，我们首先算一个旋转向量rotation。

const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());

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

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

然后再画出我们已经算出来的重力向量。

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

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

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

 orientation_ = (orientation_ * rotation).normalized();

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

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

  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);

关于这两行代码呢。我有我自己的个人理解。可能不对，大家姑且一听，首先呢，orientation_ * gravity_vector_ 是对重力向量的矫正，让重力向量的精度更高，尽可能的垂直与水平面，之后，判断校正后的重力向量的z分量是否存在，存在是否大于0，理论上重力向量是朝上的，所以z的值是大于0的。为什么重力向量朝上，请看下面的引用。

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

这部分的代码我们讲完了，我们回溯一下，当前函数的名字是 ImuTracker::AddImuLinearAccelerationObservation 这个是IMU加入线性加速度观测，加入观测的目的是什么呢？纠正当前的状态。我们之前利用角速度，求出朝向角。然后求出重力朝向，重力朝向去矫正航向角。

然后再回溯到上一层。
pose_extrapolator.cc

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->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角速度观测。也就是AddImuAngularVelocityObservation。
假设我们第一次进入AdvanceImuTracker这个函数，且有IMU数据，并且我们满足If条件中的time < imu_data_.front().time这个条件。在这个条件下，我们继续看AddImuAngularVelocityObservation这个函数是什么意思。这个函数是添加了IMU角速度观测值。
pose_extrapolator.cc

void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity) {
  	imu_angular_velocity_ = imu_angular_velocity;
}

直接就是IMU的角速度imu_angular_velocity就是最新的角速度，直接更新一下就好了。
那我们再分析AdvanceImuTracker这个函数的整体逻辑吧。再次回顾，形参里面的imu_tracker是什么呀？是 extrapolator->imu_tracker
pose_extrapolator.cc

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

再往回推ExtrapolateRotation这个函数。找到函数PoseExtrapolator::ExtrapolatePose如下

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

回溯完毕，我们继续掉回头去讲AdvanceImuTracker。如果不用IMU数据imu_data_.empty()
那么继续执行imu_tracker->Advance(time);。具体干了什么呢，就是更新了一下imu_tracker 记录的朝向。

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_ * rotation).normalized();
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

首先求出$\Delta t$，在这个基础上乘以角速度，算出旋转，更新朝向，同时更新重力向量。用于后面加速度数据来了，用到重力向量gravity_vector_上，再进一步更新朝向角。因为我们假设没有IMU数据，所以角速度是0，也没有旋转，所以重力向量是没有更新的。
接下来执行的代码是 imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
基于没有IMU数据，所以先给一个假数据Eigen::Vector3d::UnitZ()，也就是没有做矫正。接下来添加IMU的角速度观测。

imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);

没有角速度

odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_

如果里程计的数据很少，那么就从之前存储的位姿中计算出一个角速度，如果里程计速度够多，那么就从里程计中算出一个角速度。所以我们的IMU数据并不一定是0 ，有可能是历史的位姿上算出来的速度，有可能是里程计测算出来的速度。
imu_tracker.cc

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_ * rotation).normalized();
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

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

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执行完之后，再看下一行

  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  return last_orientation.inverse() * imu_tracker->orientation();

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

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

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

void PoseExtrapolator::UpdateVelocitiesFromPoses() {
	......
	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;
  }

首先就是数据太少，就返回。再看下面就是一些变量的定义

  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  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);

其中他也是用最新的时间减去最老的时间来算queue_delta。

 const TimedPose& newest_timed_pose = timed_pose_queue_.back();

拿出最新的位姿

  const auto newest_time = newest_timed_pose.time;

同时拿出最新的时间

  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();

拿出最老的位姿

  const auto oldest_time = oldest_timed_pose.time;=

同时拿出最老的位姿。

linear_velocity_from_poses_ =
      (newest_pose.translation() - oldest_pose.translation()) / queue_delta;

最新位姿和最老位姿作差，除以这个时间跨度。算出一个线速度

angular_velocity_from_poses_ =
      transform::RotationQuaternionToAngleAxisVector(
          oldest_pose.rotation().inverse() * newest_pose.rotation()) /
      queue_delta;

再算一个角速度。

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

void PoseExtrapolator::AddPose(const common::Time time,
                               const transform::Rigid3d& pose) {
                               	......
                               	UpdateVelocitiesFromPoses();
                               	......
                               	}

是在AddPose()中调用的，那么AddPose又在哪里调用的呢
src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
	......
	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之后得到一个Posepose_estimate_2d我们再将这个Pose放到外推器里面。刚才不是讲了一个外推器里面的有一个位姿队列，然后就把这个位姿存储到位姿队列中去。

  UpdateVelocitiesFromPoses();

然后计算更新后Pose的速度。

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

可以看到ScanMatch修正之后得到的Pose

std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

然后再将位姿放到外推器里面去，也就是调用AddPose函数。如下代码所示

 const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  extrapolator_->AddPose(time, pose_estimate);

然后再在AddPose（）中去调用 UpdateVelocitiesFromPoses();函数，用Pose队列，去更新下速度。
接下来看用Odom算出来的角速度angular_velocity_from_odometry_是在哪里定义的。是在AddOdometryData中定义的。

void PoseExtrapolator::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  	......
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;
 	......
}

每次新来了Odom数据odometry_data之后，就用最新的队列去计算一下角速度。这里不仅计算角速度，而且还有 ExtrapolateRotation(odometry_data_newest.time, odometry_imu_tracker_.get()); 值得一提的是，我们再通过里程计计算角速度的这个地方遇到了odometry_imu_tracker_，这里我们暂时提一下，后面会进行详细讲解。
接下来看一下AddOdometryData（）是在哪里调用的，
local_trajectory_builder_2d.cc

void LocalTrajectoryBuilder2D::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator we cannot add odometry data.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return;
  }
  extrapolator_->AddOdometryData(odometry_data);
}

主要就是我们前端在接受了里程计数据odometry_data之后，然后把它送到外推器里面去extrapolator_->AddOdometryData(odometry_data);。然后就是刚才提到的计算角速度。具体代码如下：
pose_extrapolator.cc

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);
  TrimOdometryData();
  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();
  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;
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;
  if (timed_pose_queue_.empty()) {
    return;
  }
  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;
  const Eigen::Quaterniond orientation_at_newest_odometry_time =
      timed_pose_queue_.back().pose.rotation() *
      ExtrapolateRotation(odometry_data_newest.time,
                          odometry_imu_tracker_.get());
  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;
}

下面进行逐行解析，首先是接收到一帧里程计数据odometry_data之后，首先是进行一个检查
src/cartographer/cartographer/mapping/pose_extrapolator.cc

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

检查如果这个pose队列是空的，或者当前的里程计的时间大于pose队列最老的时间，满足这些条件我们才能进行下去。因为我们是往前推数据，而不是推已经存在的数据。然后有一个TrimOdometryData函数，会把time之前的odometry_data数据都给删掉。

void PoseExtrapolator::TrimOdometryData() {
  while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
         odometry_data_[1].time <= timed_pose_queue_.back().time) {
    odometry_data_.pop_front();
  }
}

在保证里程计数据大于2的情况下，且位姿队列不为空的情况下，也就是说，直到位姿队列中只剩下一个位姿的时候，循环才会停止。并且，如果队列就剩下两个数据，timed_pose_queue_.back().time的时间就等于odometry_data_[1].time的时间，满足条件，就一直往外抛出数据。因为之前的数据都没有用了，我们要使用的是pose之前的数据，之后的没有用了，只保留2个就可以了。
然后继续回溯到之前的代码的位置。
src/cartographer/cartographer/mapping/pose_extrapolator.cc

 if (odometry_data_.size() < 2) {
    return;
  }

如果数据不够，就继续返回。

  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  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;
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;

接下来是老套路了，都是拿到最老和最新的odom数据，然后再计算得到${\Delta }_{time}$，根据最新和最老的位姿算出${\Delta }_{pose}$。然后再求出角速度。

 const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;

然后还有一串长名字linear_velocity_in_tracking_frame_at_newest_odometry_time。最新的里程计时间点上的tracking_frame上的线速度。为什么起这么个名字，我们还是要讲一下。

  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;

上面代码主要是让速度从机体坐标转化成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(),在这个基础上，乘以一个${\Delta }_{rotation}$ ExtrapolateRotation(odometry_data_newest.time, odometry_imu_tracker_.get())。 但是这个${\Delta }_{rotation}$是怎么算的呢？他使用了一个odometry_imu_tracker_。 当然这函数是一样的，只不过参数imu_tracker不一样。

我们为了通过里程计，计算出来线速度，我们需要把线速度转化到local坐标系里面去。计算这个速度的时候，本身就要外推一下，然后使用一个外推器，推出一个朝向角，计算出一个速度。然后再用这个速度去外推一个pose。然后计算里程计的速度去做一个外推，用到了imu_tracker,为什么又搞了一个新的呢？

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

可以看到我们的odometry_tracker 也是先通过AdvanceImuTracker 这个来得到一个朝向角。然后再从后缀_的imu_tracker_中得到上一次的朝向角。算出来一个$\Delta$。
我们什么时候要用外推器外推的时间？
比如一帧激光进来了，你要外推当前机器人的pose。要外推一次。
然后每一帧的激光点，都要给一个点外推一个发射原点。
我们的外推器，每来一个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.
    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;
  }
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of '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) {
    imu_tracker->Advance(it->time);
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  imu_tracker->Advance(time);
}

为了实时的把里程计的速度转化到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_;
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;

extrapolation_imu_tracker_是用来做外推的主力。odometry_imu_tracker_只为了用于把里程计的速度转化为local坐标系里去。imu_tracker_还没讲到。但他时刻记录着过程中，起到的作用，记录最近一次pose的朝向角，我们总是在这个基础上，计算${\Delta }_{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坐标系里面去，怎么转化呢？就是算${\Delta }_{rotation}$，再添加到最新的pose上面去。
为什么两个位姿可以相减得到KaTeX parse error: Undefined control sequence: \Deltata at position 1: \̲D̲e̲l̲t̲a̲t̲a̲_{rotation}，因为每一次数据来之后，我们得到一个很精细的pose。完了我们把imu_tracker_记录的角度给统一到odometry_imu_tracker_，extrapolation_imu_tracker_中去。那么下次我们再计算出角度之后，就去减掉imu_tracker_。就相当于减掉自己的上一时刻的位姿。imu_tracker_记录的是每一次我们ScanMatch结束之后，我们记录的精细的角度。他始终记录的是结果。
odometry_imu_tracker_，extrapolation_imu_tracker_是两个工具人，始终在imu_tracker_的基础上去计算${\Delta }_{rotation}$，然后再把${\Delta }_{rotation}$放置在存储pose队列里面最后的一个pose上面去，得到所需要时刻的pose。