(02)Cartographer源码无死角解析-(36) PoseExtrapolator→AddImuData()、AddOdometryData()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

接下来就是对 PoseExtrapolator 的成员函数进行细致分析了，需要注意的是，对于成员函数的分析，也是有逻辑，有目的的，并不是杂乱无章的分析。主要围绕着如下接口函数分析：

  //const = 0，表示该函数不改变成员变量且为纯虚函数，子类必须从重写
  virtual common::Time GetLastPoseTime() const = 0;
  //返回最后一次推断器推断出位姿的时间点
  virtual common::Time GetLastExtrapolatedTime() const = 0;
    //添加 pose，与前面的 GetLastPoseTime() 函数就联系起来了。
  virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0;
  //添加 Imu 数据，
  virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
  //添加里程计数据
  virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0;
  //推断器进行位姿推断
  virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0;

  //该函数输入一个存储时间的vector容器对象
  //返回该些时间点对应的位姿，以及最后一个时间点的重力对齐矩阵
  virtual ExtrapolationResult ExtrapolatePosesWithGravity(
      const std::vector<common::Time>& times) = 0;

  // Returns the current gravity alignment estimate as a rotation from
  // the tracking frame into a gravity aligned frame.
  // 传入一个时间点，返回该时间点的重力对齐矩阵
  virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0;

上面的注释暂时不一定是全正确的，后续一边分析一遍修改。这些接口函数都在 src/cartographer/cartographer/mapping/pose_extrapolator.cc 中被重写。
 

二、时间获取

先来看两个比较简单的函数：

// 返回上次校准位姿的时间
common::Time PoseExtrapolator::GetLastPoseTime() const {
    
    
  // 如果尚未添加任何位姿, 则返回Time::min()
  if (timed_pose_queue_.empty()) {
    
    
    return common::Time::min();
  }
  return timed_pose_queue_.back().time;
}

// 获取上一次预测位姿的时间
common::Time PoseExtrapolator::GetLastExtrapolatedTime() const {
    
    
  if (!extrapolation_imu_tracker_) {
    
    
    return common::Time::min();
  }
  return extrapolation_imu_tracker_->time();
}

逻辑比较简单，如果 timed_pose_queue_ 与 extrapolation_imu_tracker_ 为空则返回 common::Time::min()，否则返回最后一个数据的时间点。
 

三、AddImuData()

对于添加数据有三个函数：

void PoseExtrapolator::AddPose()
void PoseExtrapolator::AddImuData()
void PoseExtrapolator::AddOdometryData()

从简单的说起，也就是 PoseExtrapolator::AddImuData() ，该函数代码实现如下：

// 向imu数据队列中添加imu数据,并进行数据队列的修剪
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
    
    
  CHECK(timed_pose_queue_.empty() ||
        imu_data.time >= timed_pose_queue_.back().time);
  imu_data_.push_back(imu_data);
  TrimImuData();
}

逻辑比较建简单，如果 timed_pose_queue_ 队列不为空的时候，要求目前传入的 imu_data 时间点大于队列中最后一个元素的时间点。然后把 imu_data 添加到 timed_pose_queue_ 成为最后一个数据。然后调用 TrimImuData() 函数。

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

同时满足如下三个条件，则会一直循环把 imu_data_ 的最前面(第一个) 数据抛掉。

$(1)$ imu_data_ 中的元素个数大于1 → imu_data_.size() > 1

$(2)$ timed_pose_queue_ 队列不为空 → !timed_pose_queue_.empty()

$(3)$ timed_pose_queue_ 最后一个元素的时间需要大于 imu_data_[1].time → imu_data_[1].time <= timed_pose_queue_.back().time

也就说 ①imu_data_元素只有一个的时候，会停止循环。②或者timed_pose_queue_队列为空，也会停止循环。③亦或者 imu_data_ 第一个元素的时间大于 timed_pose_queue_ 最后一个元素的时间，也会停止循环。

$小伙伴们，迷糊吗?$ 总的来说， AddImuData() 函数起始已经保证了 imu_data_ 数据是按时间顺序排列的，排在后面数据的时间戳大于前面数据的时间戳。目的就是让 imu_data_ 队列中的所有数据的时间戳，都大于 timed_pose_queue_ 最后一个数据的时间戳。

其作用是什么，为什么要这样做，后续再分析探讨。
 

四、AddOdometryData()→逻辑讲解

首先这里说一下，该函数目的是获得如下两个变量：

angular_velocity_from_odometry_  //根据里程计计算出来的角速度
linear_velocity_from_odometry_   //根据里程计计算出来的线速度

这里先对其逻辑进行解析，源码的注释在后面。

$(1)$ 首先保证新添加的数据都迟于之前的数据，然后添加到 odometry_data_ 队列中。然后对 odom 数据队列进行修剪，保证修剪之后的 odometry_data_ 中的数据时间戳，都小于 timed_pose_queue_ 队列中最后一个数据的时间戳。

$(2)$ 获得 odometry_data_ 队列第一个数据与最后一个数据，求得时间差 odometry_time_delta，以及两个时刻 odometry 的位姿变换，源码实现如下：

  const transform::Rigid3d odometry_pose_delta =
      odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;

首先，这里的 odometry_data_newest.pose 与 odometry_data_oldest.pose 说的机器人位姿，至于其是相对于那个坐标系，确实不是很清楚了，要看数据包是怎么规划的，这里假设其是相对于第一帧数据的坐标系。
现在把odometry_data_newest.pose记为 $odom{e}_{new}^{first}$(队列中，第一个数据的位姿), 另外 odome_data_oldest.pose 记录为${\mathbf{o}\mathbf{d}\mathbf{o}\mathbf{m}\mathbf{e}}_{odl}^{first}$, (队列中，最后时刻位姿)。现在呢，要求的是从最后一个数据到第一个数据的位姿变换 odometry_pose_delta 记为 ${\mathbf{o}\mathbf{d}\mathbf{o}\mathbf{m}\mathbf{e}}_{old}^{new}$，数学公式对应如下：
$${\mathbf{o}\mathbf{d}\mathbf{o}\mathbf{m}\mathbf{e}}_{old}^{new}=[{\mathbf{o}\mathbf{d}\mathbf{o}\mathbf{m}\mathbf{e}}_{new}^{old}{]}^{-1}\ast {\mathbf{o}\mathbf{d}\mathbf{o}\mathbf{m}\mathbf{e}}_{odl}^{first}\text{\hspace{1em}(01)}$$从这里可以看出，第一个数据相对于最后一个数据的位姿变换，与他们所在的坐标系是没有关系的。

$(3)$ 通过 步骤2 求得了 第一个数据相对于最后一个数据的位姿变换 ${\mathbf{o}\mathbf{d}\mathbf{o}\mathbf{m}\mathbf{e}}_{old}^{new}$ 之后，进一步把该位姿的姿态变换成旋转向量(轴角)，其含义表示，最后一个数据的位姿围绕着一个向量旋转一定角度，变换成了第一个数据。等价于机器人在 new 时刻，围绕着一个向量旋转与一定角度角度之后，能回到之前 old 时刻的位姿。有的朋友会问？角度呢？RotationQuaternionToAngleAxisVector$(下一篇博客讲解推导过程)$ 函数返回值是一个3维向量，其表示旋转轴，该轴的长度就是这个旋转的角度，也就是说返回值并不是一个单位向量。

$(4)$ 计算最后一个数据到第一个数据平移，然后除以时间，得到该时间段 x,y,z 轴的平均线速度。代码中的存储变量为 linear_velocity_in_tracking_frame_at_newest_odometry_time。从命名看表示：机器人在最后一个odom数据时刻的线速度。这里需要注意的一个点就是，odometry_time_delta 通常为负数，计算代码如下:

  // 最新与最老odom数据间的时间差
  const double odometry_time_delta =
      common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);

也就是说，odometry_pose_delta 表示new到old位姿变换，odometry_time_delta 表示 odl 与 new 时间差(负数)，那么求得的 odometry_time_delta 表示 old 到 new 的平均位姿变换。

$(5)$ 获得机器人在最后一个odom数据时刻的方向(位姿)，求得该方向调用了 ExtrapolateRotation() 函数，该函数主推断机器人最新时刻得姿态变化量，具体细节后面再讨论，核心代码如下：

  // 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到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());

$(6)$ 将tracking frame的线速度进行旋转, 得到 local 坐标系下 (机器人)tracking frame 的线速度，核心代码如下：

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

linear_velocity_from_odometry_ 表示 local 坐标系，由 odometry 数据得到的机器人线速度。记为 $linea{r}^{local}$， 另外 orientation_at_newest_odometry_time 最新里程计数据时刻 tracking frame 在 local 坐标系下的姿态，记为 ${\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}$。最后剩下的 linear_velocity_in_tracking_frame_at_newest_odometry_time 表示 机器人在最后一个odom数据时刻的线速度，这里的线速度是基于 机器人tracking frame 坐标系的，记为 $linea{r}^{tracking}$，那么计算公式如下：
$$linea{r}^{local}={\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}\ast linea{r}^{tracking}\text{\hspace{1em}(02)}$$

五、AddOdometryData()→代码注释

// 向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_ 队列中
  odometry_data_.push_back(odometry_data);

  // 修剪odom的数据队列
  TrimOdometryData();
  // 数据队列中至少有2个数据
  //如果修剪后的数据少于两个，表示所有的里程计数据时间戳都先于 timed_pose_queue_ 最后一个数据的时间戳
  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;
}

六、结语

需要注意的是：

angular_velocity_from_odometry_  //根据里程计计算出来的角速度
linear_velocity_from_odometry_   //根据里程计计算出来的线速度

这两个数据的来源都是与 odome 相关的，也就是说 ，如果系统没有配置 odome 模式，那么推测器就不会结合 odome 数据进行位姿推测。最核心的代码为倒数第二句：

  // 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到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());

这里利用一个当前时刻精准的位姿 timed_pose_queue_.back().pose.rotation()，然后乘以当前时刻预测的位姿变换率 ExtrapolateRotation($\cdots$)，推测出 odometry_data_newest.time 时刻的位姿 orientation_at_newest_odometry_time 。不是很理解也没有关系，下面会具体详细介绍。

另外，这里再提及一下，PoseExtrapolator::AddOdometryData() 至少两个数据以上才会进行位姿预测，PoseExtrapolator::AddImuData() 则只需要一个数据，就会进行位姿预测。相关代码如下：

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

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

也就是说，从修剪数据部分的代码可以看出。

$RotationQuaternionToAngleAxisVector()$ 函数并没有讲解其具体推导过程，后续会推导出为什么旋转向量(轴角)除以时间差可以得到角速度。