Cartographer前端之位姿推测器pose_extrapolator.cc

pose_extrapolator 主要用于融合里程、IMU和历史位置的输出，目前我用的版本是 2021.05.07 master branch，在旧的tag 0.2.0上是有ukf的，后来的版本没有ufk了。

cartographer 算法中提供了一个位置和姿态估计器，其主要作用是在前端匹配时作为预测位置的输入，即可认为是匹配的初始位置和姿态。在前面已经详细讲解了前端scan math的三种方法，其中前端的优化匹配和相关匹配一定程度上依赖于初始位姿，因此获取预测位置十分重要。

获取预测的位置方式有许多种，比如其他slam算法中常见的odometry转换、IMU积分、上刻位置近似等。而cartographer提供了一个3d预测估计器实现（包含2d实现），可自动融合历史位置、odometer和IMU数据。其调用接口目录为：

cartographer/mapping/pose_extrapolator_interface.h

接口内部为虚函数，具体有两种实现方式，目前2d使用一种PoseExtrapolator，其位姿估计器具体实现代码目录为：

cartographer/mapping/pose_extrapolator.h

一、代码讲解

1.pose_extrapolator类定义

 //时间参数，两次预测估计最小时间差
  const common::Duration pose_queue_duration_;
  // 带时间戳的位置结构体
  struct TimedPose {
    common::Time time;
    transform::Rigid3d pose;
  };
  // 带时间戳的位置队列
  std::deque<TimedPose> timed_pose_queue_;
  // 从位置队列估计的线速度和角速度
  Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();

  // 重力加速度常数
  const double gravity_time_constant_;
  // Imu 原始data 队列，一般仅保留两个或最新预测时间之后的所有序列
  std::deque<sensor::ImuData> imu_data_;

  // 全局的航向推算器
  std::unique_ptr<ImuTracker> imu_tracker_;
  // 共享临时航向角推算器
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;

  // 推算新的位置缓存
  TimedPose cached_extrapolated_pose_;

  // 里程计队列信息，一般仅保留两个或最新预测时间之后的所有序列
  std::deque<sensor::OdometryData> odometry_data_;
  // 通过里程计估计线速度和角速度
  Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();

2.插入IMU数据 AddImuData()

// 添加imu data
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();
}

由于IMU可直接提供角速度信息，因此无需进行一定预处理，即已获取imu的角速度信息；

3.插入里程计数据 AddOdometryData()

这里根据最新和最旧odom获取线速度和角速度

// 添加 odometrydata
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;

  // 提取里程计和当前真实位置的旋转转移矩阵
  // 其中odometry_imu_tracker_ 与 与刚添加历史位置时的imu_tracker_信息一致
  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;
}

4.根据位姿获取速度 UpdateVelocitiesFromPose()

// 从历史位置估计当前的速度
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
  if (timed_pose_queue_.size() < 2) {
    // We need two poses to estimate velocities.
    return;
  }
  CHECK(!timed_pose_queue_.empty());
  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);
  // 时间间隔需满足超出一定阈值
  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;
  // 计算线速度
  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;
}

5.插入历史位置 AddPose()

// 添加历史位置
void PoseExtrapolator::AddPose(const common::Time time,
                               const transform::Rigid3d& pose) {
  // 2d slam imu_tracker_ 没有初始化
  // 第一次加入历史位置时，需初始化
  if (imu_tracker_ == nullptr) {
    // 当前时间为初始时间
    common::Time tracker_start = time;
    // 如果有imu数据的话， 取imu和当前时间最早时间
    if (!imu_data_.empty()) {
      tracker_start = std::min(tracker_start, imu_data_.front().time);
    }
    // 初始化 角度推算器
    imu_tracker_ =
        absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
  }
  // 记录对应时间和位置，放入一个队列存储
  timed_pose_queue_.push_back(TimedPose{time, pose});
  // 确保仅保留两个，同时最新位置的间隔不超出参数阈值
  while (timed_pose_queue_.size() > 2 &&
         timed_pose_queue_[1].time <= time - pose_queue_duration_) {
    timed_pose_queue_.pop_front();
  }
  // 从位置中更新速度信息
  UpdateVelocitiesFromPoses();
  //将全局imu推算器执行推算，主要推算估计的航向信息
  AdvanceImuTracker(time, imu_tracker_.get());
  TrimImuData();
  TrimOdometryData();
  // 智能指针创建，并共享内容，无需delete
  // 即每次添加新的pose时同时开辟两个tracker
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

插入历史位置进行更新估计为主要和必要方法，即使没有odometer和imu插入数据也可正常工作，因此AddPose（）可为主要函数，即目的按照时间顺序插入历史真实位置（实际上为scan-match后较为准确的位置）。然后进行估计线速度和角速度。通过角速度再次进行积分获取航向变换信息。

6.航向角积分 AdvanceImuTracker()

//imu 估计器更新，即迭代更新当前估计位置的航向信息， 估计器采用指针可调用不同具体估计器
// 
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  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.
    // 计算出当前估计的朝向信息和重力加速度信息
    imu_tracker->Advance(time);
    // 无imu，则无线性加速度信息
    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;
      });
  // 将imudata中每一时刻进行估计更新
  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);
}

由于已估计出目前的线速度和角速度，为获取估计姿态则需要对角速度进行积分，然后获取角度旋转矩阵。由于航向积分并非像平移一样线性加减即可，因此cartographer专门提供了一个用于角度积分的ImuTracker类。

7.获取预测位置 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();
    // 获取估计旋转矩阵
    // 然后对上次的航向进行旋转，获取估计的航向
    // extrapolation_imu_tracker_ 与最新加入历史位置时的imu_tracker_一致
    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;
}

以上最终输出预测值接口，在上次真实位置基础上进行积分，即获取平移矩阵和旋转矩阵，进行转换获取最终估计位置和姿态；

其中位置平移矩阵积分较为简单，可参考代码即可。

8.获取运动平移ExtrapolateTranslation()

//获取当前时间与插入最新位置的平移矩阵
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_;
}

9.获取运动旋转ExtrapolateRotation()

通过角度积分器获取角度

// 获取当前时间与插入最新位置的旋转变换矩阵
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(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();
}

二、思想总结

位姿估计器解决的是在实现前端scan-match之前的预测值，即激光新的一帧匹配前获取当前预测定位信息。同时假设传感器数据之间，slam本体以恒定线速度和恒定角速度移动。根据上次的scan-match的准确位置后进行积分，获取预测位置和姿态。

其中估计的恒定速度主要两种方式组成：采用两次scan-match位置进行微分获得；另一种通过里程计进行微分获得；由于里程计的更新速度更快，故里程计可计算最新结果时，优先使用；

估计的角速度则有三种方式组成：1.采用两次scan-match的航向进行微分获得，频率最低；2.采用里程计进行微分；3.采用IMU直接获取，优先级最高；

cartographer针对imu和odometer是否使用可进行配置，根据配置信息和时间序列进行选择具体方式，最后根据估计的线速度和角速度进行实时积分，获取预测估计位置。

最基本的配置odometry和imu均无，则估计器实际上仅是根据历史位置进行推算出线速度和角速度，进行估计位置。

说到这里也可以通过后端优化node数设置为0,单独用odom、imu来检测前端的效果。