(02)Cartographer源码无死角解析-(40) PoseExtrapolator→整体框架复盘，作用与目的讲解

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过前面的博客，对于 PoseExtrapolator 这个类可以说是十分了解了，但是其使用方式与具体作用了解还不够透彻，另外在往推测器中添加各种数据(如点云匹配获得的位姿，Imu传感器数据，里程计odome)时，因该如何取舍，或者说谁为主。

首先说其作用，或许有的朋友比较奇怪，既然通过点云匹配、或者Imu传感器与里程计odome能够获取位姿，那么直接使用他们的位姿不是就可以了吗？为什么还要搞个推断器来推测位姿？完全没必要啊！虽然点云匹配、或者Imu传感器与里程计odome能够获取位姿。但是，这些数据时存在时间间隔的，密度并不高。

这里举个例子，通过 Imu传感器获去到时刻 $\cdots$ 的位姿，但是如果我们想要知道 $2，4，6 \cdots$ 时刻的位姿，怎么办呢？大家容易想到的是通过插值的方式，其实 PoseExtrapolator 的原理差不多就是这个样子的。

另外，PoseExtrapolator 还能够估算出重力校正矩阵，该矩阵就是机器人(tracking_frame)相对于重力坐标系(Z轴与重力平行)的姿态。也就是说，通过该矩阵可以把机器人坐标系下的数据，变换到重力坐标系下，如之前点云数据Z轴过滤就使用到了该矩阵。

二、作用→LocalTrajectoryBuilder2D举例

这里就拿前面点云畸变校正过程来举例，说明 PoseExtrapolator 的作用，当然其他的地方也有使用到 PoseExtrapolator，就不再一一列举了。在 LocalTrajectoryBuilder2D::AddRangeData() 函数中，可以看到如下代码：

  // 预测得到每一个时间点的位姿
  for (const auto& range : synchronized_data.ranges) {
    
    
  	// Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
    range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<float>());
 }

不用说，因为云畸变的校正，就是需要利用机器人在每个点云时间点在 local 坐标系下的位姿，对该点云进行校正。点云数据可能分布在各个时间点，且此时点云数据才添加进来，该帧点云数据还未进行扫描匹配，当然不知道该帧点云数据的位姿。

那么，只能通过推断器获取每个点云在其对应时刻机器人的位姿，对点云进行校正。校正过程前面已经讲解过了。通过上面的函数，知道其调用 oseExtrapolator::ExtrapolatePose(const common::Time time) 函数进行位姿推断。

该函数前面分析过，其位移推断主要依赖来自于odome的最新线速度，如果没有使用odome。则使用最新(上一帧数据)点云匹配估算出来的 poses。旋转姿态(方向)主要依赖于成员变量 std::unique_ptr<ImuTracker> extrapolation_imu_tracker_。值得注意的是，该成员变量拷贝自 imu_tracker_ 变量。

在 PoseExtrapolator::AddPose() 函数的末尾，可以看到看到其拷贝过程。也就是说，每调用一次PoseExtrapolator::AddPose() 函数，添加 pose 数据。

首先→调用UpdateVelocitiesFromPoses()函数，对来自pose 的线速度linear_velocity_from_poses_ 与角速度angular_velocity_from_poses_ 进行更新。这里不需要指定时间 time，因为pose就是来自于time时刻。

随后→调用AdvanceImuTracker(time, imu_tracker_.get())函数，通过 imu_tracker_ 利用 Imu 数据，对 time 时刻的姿态进行推断。该处的推断只要依赖于Imu的线速度与角速度。

完成之后，再调用 TrimImuData(); 与 TrimOdometryData(); 函数进行数据修剪，其目的是把 time 时间之前的数据数据删除掉。然后再把 imu_tracker_ 拷贝给 odometry_imu_tracker_ 与 odometry_imu_tracker_。这里是个重要的点，就是每次添加 pose 之后，后续无论使用 odometry_imu_tracker_ 还是 extrapolation_imu_tracker_ 进行位姿推断，其都被校正过了。

三、推断预测模式

首先，对于里程计Odome与点云Pose进行位姿的预测，其都是基于匀速模型的，如下代码可以体现：

1、里程计Odome

//PoseExtrapolator::AddOdometryData() 函数存在如下代码
const double odometry_time_delta =common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);

angular_velocity_from_odometry_=transform::RotationQuaternionToAngleAxisVector(odometry_pose_delta.rotation()) /

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

该处预测线速度的方式比较简单，方向变换量与位移变换量，都除以时间变化量，显然是获得平均角速度与平均线速度。

2、点云Pose

// PoseExtrapolator::UpdateVelocitiesFromPoses() 函数中存在如下代码

// 平移量除以时间得到 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;

同上，方向变换量与位移变换量，都除以时间变化量，显然是获得平均角速度与平均线速度。

3、Imu数据

使用Imu数据则有所不同，要注意的是，关于 Imu 的更新，即对 imu_tracker_ 的更新，其是在 PoseExtrapolator::AddPose() 函数中通过调用 AdvanceImuTracker(time, imu_tracker_.get()) 函数进行更新的。

更新的过程并非是匀速的，而是会循环一次次添加 imu_data_ 数据，进行校正更新。其更加类似于瞬时速度的更新。代码体现如下：

  //下述代码位于 PoseExtrapolator::AdvanceImuTracker() 函数中

  // 然后依次对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推断器 imu_tracker_ 其只对旋转(姿态)进行推断，Imu的线速度与角速度数据也都是都用来校正优化姿态了。

四、各种位姿取舍

现在进入到下一个核心主题。就是对于多种数据获得的数据，然后求解获取到的位姿，应该如何取舍呢？回顾一下之前的代码：

	//PoseExtrapolator::ExtrapolatePosesWithGravity()函数存在如下代码
  // 进行当前线速度的预测
  const Eigen::Vector3d current_velocity = odometry_data_.size() < 2? linear_velocity_from_poses_: linear_velocity_from_odometry_;

对于线速度，源码中认为里程计odomet的线速度更加精准，如果没有里程计则使用来自于 pose 推断的线速度代替。

对于姿态或者说方向的推断，主要为 PoseExtrapolator::ExtrapolatePose() 函数，该函数使用点云匹配得到的pose 进行位姿推断，方向使用 extrapolation_imu_tracker_ (本质上为Imu) 进行推断，代码体现如下：

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

上面是存在，或者使用Imu的情况，如果没有使用Imu传感器，即没有Imu数据，那么就会执行 PoseExtrapolator::AdvanceImuTracker() 函数中的如下代码：

  // 不使用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，对于角速度(位姿)会优先使用来自于里程计odome的。

$\color{red}总结：$
①→预测平移时: 有里程计就用里程计的线速度, 没有里程计就用 pose 计算的线速度进行预测
②→预测姿态时: 有 IMU 就用 IMU 的角速度, 没有 IMU 时, 如果有里程计就用里程计计算出的角速度, 没有里程计就用 pose 计算的角速度进行预测
③→当前的线速度如果有里程计就用里程计计算出的, 没有里程计就用 pose 计算的角速度进行预测

五、可能的改进建议

这里主要记录一下 PoseExtrapolator 可能存在的一些问题，或许也是后续优化的一个方向或者小点。

$\color{blue}(1)$ pose 的距离越小, 匀速模型越能代替机器人的线速度与角速度, 计算 pose 的线速度与角速度时, 可以考虑使用最近的 2 个数据进行计算。

$\color{blue}(2)$ 里程计距离越短数据越准, 计算里程计的线速度与角速度时, 可以考虑使用最近的 2 个数据进行计算。

$\color{blue}(3)$ 使用里程计, 不使用 imu 时, 计算里程计的线速度方向时, 可以考虑使用里程计的角度进行计算

$\color{blue}(4)$ 使用里程计, 不使用 imu 时, 进行姿态的预测时, 可以考虑使用里程计的角度进行预测

$\color{blue}(5)$ 不使用里程计, 不使用 imu 时, 可以考虑用最近的 2 个 pose 计算线速度与角速度

$\color{blue}(6)$ 使用 pose 对 imu_tracker_的航向角进行校准。

六、重力校正深究

首先回顾一下重力校正矩阵的获得，在LocalTrajectoryBuilder2D::AddRangeData 函数中存在如下代码:

    // 获取重力对齐变换矩阵，该矩阵只包含旋转，平移为0，可理解机器人坐标系的Z轴需要与重力矢量平行
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));

然后在如下文件与函数中使用到了该矩阵：

	//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 
	//中的LocalTrajectoryBuilder2D::AddRangeData函数
   TransformToGravityAlignedFrameAndFilter(gravity_alignment.cast<float>() * range_data_poses.back().inverse(),accumulated_range_data_),

	//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 
	//中的LocalTrajectoryBuilder2D::AddAccumulatedRangeData函数，2D与3D有所不同
  	const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

首先其来源，gravity_alignment 表示的是重力坐标系(Z轴平行与重力方向) 到机器人(tracking_frame)坐标系旋转变换，注意这里是坐标系的变换，记为 $\mathbf {gravity\_alignment}^{gravity}_{robot}$ , 所以机器人(tracking_frame) 坐标系下的数据，直接左乘该变换矩阵，就可变换到重力坐标系下。

那么随之问题就来了，这个重力坐标系是如何确定的？或者说是在什么时候确定的呢？假设这里使用了Imu 初始化，那么其会执行 src/cartographer/cartographer/mapping/pose_extrapolator.cc 文件中的如下代码，创建 PoseExtrapolator 实例对象：

// 使用imu数据进行PoseExtrapolator的初始化
std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
    const common::Duration pose_queue_duration,
    const double imu_gravity_time_constant, const sensor::ImuData& imu_data) {
    
    
  auto extrapolator = absl::make_unique<PoseExtrapolator>(
      pose_queue_duration, imu_gravity_time_constant);
  extrapolator->AddImuData(imu_data);
  ......
  ......
  return extrapolator;
}

其构建PoseExtrapolator实例对象之后，立刻马上使用Imu数据覆盖了构造函数默认初始值。那么就相当于使用Imu数据把初始值覆盖了，如果机器人处于开机的时候处于水平状态，那么就相当于记录了他水平状态时，机器人(Imu)的位姿，等价于重力坐标系。有了初始值，后续就可以求出机器先对于初始位姿的位姿了。

那么，如果没有Imu呢？如在 LocalTrajectoryBuilder2D::AddRangeData() 函数中，可以看到如下代码：

  if (!options_.use_imu_data()) {
    
    
    InitializeExtrapolator(time);
  }

其会对 PoseExtrapolator 进行初始化，构建实例对象之后，立马用 extrapolator_->AddPose() 函数添加一个单位旋转进行初始化。由于没有传感器Imu数据，所以后续 PoseExtrapolator(推断器的校正)，体现在如下代码中：

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
    
    
  // 不使用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 推断器，不过没有关系，后续会根据 angular_velocity_from_poses_ 或者 angular_velocity_from_odometry_ 校正过来。不过后续求得的位姿，都是相对于单位旋转矩阵的，也可以认为其就是 local 坐标系的原点。

七、总结

总体来，PoseExtrapolator 是比较信任IMU的，次之里程计，最后才到 pose。其实个人认为应该更加信任 pose，只是这里对当前帧点云进行位姿推断的时候，pose 还是上一帧数据的，所以这里更加信任 IMU，因为他频率高。等后续，当前帧来自于点云的pose到来，则会用这个相对准确的数据去校正推断器。