cartographer 源码解析(六)

你的名字 2022-09-11 15:11 284阅读 0赞

### 相关链接： ###

#### [cartographer 源码解析(一)][cartographer] ####

#### [cartographer 源码解析(二)][cartographer 1] ####

#### [cartographer 源码解析(三)][cartographer 2] ####

#### [cartographer 源码解析(四)][cartographer 3] ####

#### [cartographer 源码解析(五)][cartographer 4] ####

#### [cartographer 源码解析(六)][cartographer 5] ####

这一章节呢，主要讲外推器。我们上一章节说到激光的畸变与矫正，最关键的是什么呢？其实最关键的就是关于每个发射点的发射位置的估计推测，这里就用到了外推器去推测每个发射点的位置。  
**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`。主要是做了指数平均滤波 ( e x p o n e n t i a l m o v i n g a v e r a g e , E M A ) (exponential moving average,EMA) (exponentialmovingaverage,EMA)，也就是互补滤波

>  y i = ( 1 − α ) y i − 1 + α x i y\_i = (1-\\alpha)y\_\{i-1\}+\\alpha x\_i yi=(1−α)yi−1\+αxi  
> 指数平均滤波的权值是以指数式递减的  
> 指数移动平均无论多旧的数据其总会被给予一定的权值  
> 参考：[https://blog.csdn.net/iceboy314159/article/details/89716290][https_blog.csdn.net_iceboy314159_article_details_89716290]  
> 相关链接：[https://zhuanlan.zhihu.com/p/76188487][https_zhuanlan.zhihu.com_p_76188487]

首先是一个条件表达式，其中`time_`我们可以从之前讲的代码中得知已经更新到最新了，`last_linear_acceleration_time_`是上一次的线性加速度时间，两者想减得到 Δ t \\Delta t Δ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_`是一个常数，可以知道，当 Δ t \\Delta t Δ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`短时间内角速度还是比较准确的。 所以 Δ t \\Delta t Δ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()`这个方向上。变成这么一个朝向上的向量。

这里我画图解释一下，首先假设我们有一个车，车体自己有一个坐标系以及航向。蓝色线为水平线  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center]

然后再画出我们已经算出来的重力向量。  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 1]

我们假设算出的重力向量很准。然后呢，假设我们还知道航向与水平面的夹角 θ \\theta θ。首先我们要知道，航向的四元数的共轭向量，本质就是旋转的逆。正向旋转是逆时针，反向旋转是顺时针，  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 2]  
如果这个旋转的逆，乘以这个UnitZ ,也就是意味着UnitZ 顺时针旋转 θ \\theta θ角度。  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 3]  
绿色线为旋转之后的`UnitZ`。然后我们再求出`UnitZ`与重力向量直线的旋转向量，再将这个旋转向量乘以航向角，来纠正航向角，航向角向下旋转的角度就是新的UnitZ与重力向量之间的夹角。实现代码如下：

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

效果如下：紫色为纠正后的航向角  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 4]

使用重力加速度来纠正我们估计的朝向。  
怎么纠正呢，理论上来说，我们车体坐标系上的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**

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

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

首先求出 Δ t \\Delta t Δ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**

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

`AdvanceImuTracker`执行完之后，再看下一行

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

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

其中，我们再求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`之后得到一个Pose`pose_estimate_2d`我们再将这个Pose放到外推器里面。刚才不是讲了一个外推器里面的有一个位姿队列，然后就把这个位姿存储到位姿队列中去。

\------1:06:24-----

[cartographer]: https://dqwang.blog.csdn.net/article/details/118539690
[cartographer 1]: https://dqwang.blog.csdn.net/article/details/118881867
[cartographer 2]: https://dqwang.blog.csdn.net/article/details/119023628
[cartographer 3]: https://dqwang.blog.csdn.net/article/details/119154163
[cartographer 4]: https://dqwang.blog.csdn.net/article/details/119845028
[cartographer 5]: https://dqwang.blog.csdn.net/article/details/120352115
[https_blog.csdn.net_iceboy314159_article_details_89716290]: https://blog.csdn.net/iceboy314159/article/details/89716290
[https_zhuanlan.zhihu.com_p_76188487]: https://zhuanlan.zhihu.com/p/76188487
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center]: /images/20220828/d4509e91a3b74417b5264186629341cc.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 1]: /images/20220828/8aa12eacc6904d9696dad263cef429cd.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 2]: /images/20220828/67bc12538f404614b29f4cd1f1c9f94f.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 3]: /images/20220828/03b5d8aac9704e529b06524bc997e0bb.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16_pic_center 4]: /images/20220828/e25a08fd22bf45dbbd55d9aa63e9b93f.png