Cartographer 位姿推测器pose_extrapolator
目录
1. local_trajectory_builder_2d.cc
2. imu_tracker.h
3. imu_tracker.cc
3.1 构造函数
3.2 ImuTracker::Advance()
3.3 ImuTracker::AddImuAngularVelocityObservation()
3.4 ImuTracker::AddImuLinearAccelerationObservation()
4. pose_extrapolator.h
5. pose_extrapolator.cc
5.1 构造函数
5.2 PoseExtrapolator::InitializeWithImu()
5.3 PoseExtrapolator::AddImuData()
5.4 PoseExtrapolator::TrimImuData()
5.5 AddImuData()在何处调用
5.6 PoseExtrapolator::AddOdometryData()
5.6.1 PoseExtrapolator::ExtrapolateRotation()
5.6.2 PoseExtrapolator::AdvanceImuTracker()
5.6.3 PoseExtrapolator::AddOdometryData()
5.7 PoseExtrapolator::TrimOdometryData()
5.8 AddOdometryData()在何处调用
5.9 PoseExtrapolator::AddPose()
5.10 PoseExtrapolator::ExtrapolatePose()
5.11 PoseExtrapolator::EstimateGravityOrientation()
5.12 PoseExtrapolator::ExtrapolatePosesWithGravity()
1. local_trajectory_builder_2d.cc
在LocalTrajectoryBuilder2D::AddRangeData()对点云去畸变时：
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id,const sensor::TimedPointCloudData& unsynchronized_data) {
......range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<float>());
......
}在LocalTrajectoryBuilder2D::AddAccumulatedRangeData()执行扫描匹配时：
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(const common::Time time,const sensor::RangeData& gravity_aligned_range_data,const transform::Rigid3d& gravity_alignment,const absl::optional<common::Duration>& sensor_duration) {
......// 进行位姿的预测, 获取先验位姿const transform::Rigid3d non_gravity_aligned_pose_prediction =extrapolator_->ExtrapolatePose(time);
......
}都要通过位姿推测器获取根据imu和odometry的数据计算得到的机器人的先验位姿，之前笔记中没有说明pose_extrapolator的计算过程，现在通过代码分析一下。
2. imu_tracker.h
pose_extrapolator中用到了imu_tracker， 计算IMU 的角速度和线加速度数据，实时维护机器人的姿态、重力方向等状态。
namespace cartographer {
namespace mapping {// Keeps track of the orientation using angular velocities and linear
// accelerations from an IMU. Because averaged linear acceleration (assuming
// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
// though yaw does.
// 使用来自IMU的角速度和线性加速度跟踪方向
// 由于平均线性加速度(假设缓慢移动)是重力的直接量度, 因此, 横摆/俯仰不会漂移, 尽管偏航会漂移/*** @brief * ImuTracker 的主要作用是根据 IMU的角速度来预测姿态,* 并根据IMU的线加速度来确定重力的方向, 并使用重力的方向来对姿态进行校准*/
class ImuTracker {public:ImuTracker(double imu_gravity_time_constant, common::Time time);// Advances to the given 'time' and updates the orientation to reflect this.void Advance(common::Time time);// Updates from an IMU reading (in the IMU frame).void AddImuLinearAccelerationObservation(const Eigen::Vector3d& imu_linear_acceleration);void AddImuAngularVelocityObservation(const Eigen::Vector3d& imu_angular_velocity);// Query the current time.// 获取上一次预测位姿的时间戳common::Time time() const { return time_; }// Query the current orientation estimate.// 查询当前估计出的姿态Eigen::Quaterniond orientation() const { return orientation_; }private:const double imu_gravity_time_constant_;common::Time time_;common::Time last_linear_acceleration_time_;Eigen::Quaterniond orientation_; // 上一时刻的姿态（朝向/旋转）Eigen::Vector3d gravity_vector_; // 重力方向Eigen::Vector3d imu_angular_velocity_; // 角速度
};} // namespace mapping
} // namespace cartographer3. imu_tracker.cc
3.1 构造函数
ImuTracker::ImuTracker(const double imu_gravity_time_constant,const common::Time time): imu_gravity_time_constant_(imu_gravity_time_constant),time_(time),last_linear_acceleration_time_(common::Time::min()),orientation_(Eigen::Quaterniond::Identity()), // 初始方向角gravity_vector_(Eigen::Vector3d::UnitZ()), // 重力方向初始化为[0,0,1]imu_angular_velocity_(Eigen::Vector3d::Zero()) {} imu_gravity_time_constant是用于重力向量平滑的时间常数。在ImuTracker::AddImuLinearAccelerationObservation() 中通过指数滑动平均融合重力观测时使用。
time_是 ImuTracker 当前数据的时间戳。
last_linear_acceleration_time_记录上一次接收 IMU 线加速度数据的时间。
orientation_初始化值为单位四元数，为后续基于角速度的姿态预测提供起点。
gravity_vector_初始化值为(0, 0, 1) 向量，是初始重力向量的假设值，默认重力沿世界坐标系 Z 轴向上。后续会通过 IMU 线加速度观测不断校准，逐步逼近真实重力方向，是姿态校准的参考。
imu_angular_velocity_记录当前 IMU 的角速度观测值，初始化为零。
3.2 ImuTracker::Advance()
基于当前角速度，将 ImuTracker的状态（姿态、重力向量）从当前时间预测到目标时间time。
void ImuTracker::Advance(const common::Time time) {CHECK_LE(time_, time);// 1. 输入目标时间 time，计算当前跟踪器时间（time_）到目标时间的时间差const double delta_t = common::ToSeconds(time - time_);// 2. 姿态预测// 用当前存储的上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的// 旋转量（角轴向量），再转换为四元数表示的旋转：const Eigen::Quaterniond rotation =transform::AngleAxisVectorToRotationQuaternion(Eigen::Vector3d(imu_angular_velocity_ * delta_t));// 使用上一时刻的姿态 orientation_ 乘以姿态变化量，得到目标时间的预测姿态// person:（ 四元数乘法等价于 “旋转的复合”，即连续两次旋转的叠加。）orientation_ = (orientation_ * rotation).normalized();// 3. 重力向量预测// 重力向量会随姿态旋转而变化，用旋转量的共轭（逆旋转）更新重力向量//（表示姿态旋转后重力向量的新方向）：gravity_vector_ = rotation.conjugate() * gravity_vector_;// 4. 更新时间time_ = time;
}3.3 ImuTracker::AddImuAngularVelocityObservation()
// 更新角速度
void ImuTracker::AddImuAngularVelocityObservation(const Eigen::Vector3d& imu_angular_velocity) {imu_angular_velocity_ = imu_angular_velocity;
}3.4 ImuTracker::AddImuLinearAccelerationObservation()
函数的核心功能是利用 IMU 线加速度观测值优化重力向量估计，并校准机器人姿态，确保姿态与重力方向一致。
/*** @brief 更新线性加速度的值,并根据重力的方向对上一时刻的姿态进行校准* * @param[in] imu_linear_acceleration imu的线加速度的大小*/
void ImuTracker::AddImuLinearAccelerationObservation(const Eigen::Vector3d& imu_linear_acceleration) {// 1. 计算时间差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_; // 更新时间// 2.计算平滑系数。delta_t越大, alpha越大const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);// 3. 更新重力向量（gravity_vector_）gravity_vector_ =(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;// Change the 'orientation_' so that it agrees with the current// 'gravity_vector_'.// 4. 计算姿态校准旋转量const Eigen::Quaterniond rotation = FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());// 5. 使用这个旋转量来校准当前的姿态（orientation_）orientation_ = (orientation_ * rotation).normalized();// 6. 验证校准有效性// note: glog CHECK_GT: 第一个参数要大于第二个参数// 通过 CHECK_GT 检查校准后姿态与重力向量的乘积的 Z 分量是否接近 1（>0.99），确保重力方向大致沿 Z 轴，验证姿态校准的合理性。CHECK_GT((orientation_ * gravity_vector_).z(), 0.);CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
} 其中，采用指数滑动平均融合历史重力向量与当前线加速度观测：gravity_vector_ = (1 - alpha) * 历史重力向量 + alpha * 当前线加速度。因为 IMU 线加速度包含重力分量，此操作可得到更稳定的重力方向估计。
在 “4.计算姿态校准旋转量” 这一步中，核心是找到一个旋转（四元数），让 “更新后的重力向量” 与 “当前姿态所期望的重力方向” 对齐，为后续校准机器人姿态做准备。
orientation_是 “机器人坐标系到世界坐标系的旋转”，记作。其共轭（
orientation_.conjugate()）相当于逆旋转，是 “世界坐标系到机器人坐标系的旋转”，记作。
orientation_.conjugate() * Eigen::Vector3d::UnitZ()这句运算，相当于将初始重力向量转换到机器人坐标系下，得到 “当前姿态认为的重力方向”。
FromTwoVectors(a, b)函数的作用是计算一个旋转四元数，使得向量a经过该旋转后与向量b重合。其计算出的rotation用于修正orientation_暗示的重力方向与优化后的实际重力向量完全一致，从而实现姿态的校准：
orientation_ = (orientation_ * rotation).normalized();4. pose_extrapolator.h
class PoseExtrapolator : public PoseExtrapolatorInterface {
public:explicit PoseExtrapolator(common::Duration pose_queue_duration,double imu_gravity_time_constant);PoseExtrapolator(const PoseExtrapolator&) = delete;PoseExtrapolator& operator=(const PoseExtrapolator&) = delete;// 使用imu数据进行PoseExtrapolator的初始化static std::unique_ptr<PoseExtrapolator> InitializeWithImu(common::Duration pose_queue_duration, double imu_gravity_time_constant,const sensor::ImuData& imu_data);// Returns the time of the last added pose or Time::min() if no pose was added// yet.common::Time GetLastPoseTime() const override; // 返回上次校准位姿（将扫描匹配pose送入推测器）的时间common::Time GetLastExtrapolatedTime() const override; // 返回上一次预测位姿的时间// 添加观测数据void AddPose(common::Time time, const transform::Rigid3d& pose) override;void AddImuData(const sensor::ImuData& imu_data) override;void AddOdometryData(const sensor::OdometryData& odometry_data) override;// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿transform::Rigid3d ExtrapolatePose(common::Time time) override;// 推测机器人一系列的位姿ExtrapolationResult ExtrapolatePosesWithGravity(const std::vector<common::Time>& times) override;// Returns the current gravity alignment estimate as a rotation from// the tracking frame into a gravity aligned frame.// 机器人相对重力方向的姿态Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;private:void UpdateVelocitiesFromPoses(); // 校准推测器，根据pose队列计算tracking frame 在 local坐标系下的线速度与角速度void TrimImuData(); // 丢弃过期数据void TrimOdometryData();// 更新imu_tracker的状态, 并根据imu_data队列的数据将imu_tracker的状态预测到time时刻void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const;// 计算从imu_tracker到time时刻的姿态变化量Eigen::Quaterniond ExtrapolateRotation(common::Time time,ImuTracker* imu_tracker) const;Eigen::Vector3d ExtrapolateTranslation(common::Time time);// 保存一定时间内的poseconst common::Duration pose_queue_duration_;struct TimedPose {common::Time time;transform::Rigid3d pose;};std::deque<TimedPose> timed_pose_queue_; // 保存扫描匹配得到的的位姿// 线速度有2种计算途径 // person: 通过扫描匹配得到的位姿计算速度Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();const double gravity_time_constant_;std::deque<sensor::ImuData> imu_data_; // 保存odom数据std::unique_ptr<ImuTracker> imu_tracker_; // 保存与预测当前姿态std::unique_ptr<ImuTracker> odometry_imu_tracker_; // 用于计算里程计的姿态的ImuTrackerstd::unique_ptr<ImuTracker> extrapolation_imu_tracker_; // 用于预测姿态的ImuTrackerTimedPose cached_extrapolated_pose_;std::deque<sensor::OdometryData> odometry_data_; // 保存odom数据Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
};
类PoseExtrapolator继承了接口PoseExtrapolatorInterface，对其中的接口函数进行了实现。接下来一个一个看函数功能。
5. pose_extrapolator.cc
5.1 构造函数
/*** @brief 构造函数* * @param[in] pose_queue_duration 时间差 0.001s* @param[in] imu_gravity_time_constant 10*/
PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,double imu_gravity_time_constant): pose_queue_duration_(pose_queue_duration),gravity_time_constant_(imu_gravity_time_constant),cached_extrapolated_pose_{common::Time::min(),transform::Rigid3d::Identity()} {}pose_queue_duration代表位姿队列的持续时间：
// 保持pose队列中第二个pose的时间要大于 time - pose_queue_duration_while (timed_pose_queue_.size() > 2 && // timed_pose_queue_最少是2个数据timed_pose_queue_[1].time <= time - pose_queue_duration_) {timed_pose_queue_.pop_front();}time是最新位姿的时间。imu_gravity_time_constant代表 IMU 重力时间常数，后面再看其具体怎么使用。cached_extrapolated_pose_保存着上一次预测得到的位姿。这里的pose都是带时间戳的结构体：
struct TimedPose {common::Time time;transform::Rigid3d pose;};5.2 PoseExtrapolator::InitializeWithImu()
这个函数的作用是使用imu数据进行PoseExtrapolator的初始化，但cartographer中并没有用到，就先跳过。
5.3 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数据时间戳 大于 队列最后的扫描匹配后得到的准确位姿的时间。
5.4 PoseExtrapolator::TrimImuData()
// 修剪imu的数据队列,丢掉过时的imu数据
void PoseExtrapolator::TrimImuData() {// 保持imu队列中第二个数据的时间要大于最后一个位姿的时间, imu_data_最少是1个while (imu_data_.size() > 1 && !timed_pose_queue_.empty() &&imu_data_[1].time <= timed_pose_queue_.back().time) {imu_data_.pop_front();}
}
imu提供的是瞬时的线性加速度和角速度，单个 IMU 数据即可提供基础运动信息。当队列中只剩 1 个数据时，循环会终止，不会继续删除。避免将队列清空，确保始终保留 最新的那个 IMU 数据。
5.5 AddImuData()在何处调用
在local_trajectory_builder_2d.cc的LocalTrajectoryBuilder2D::AddImuData()函数中被调用：
// 将IMU数据加入到Extrapolator中
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";InitializeExtrapolator(imu_data.time);extrapolator_->AddImuData(imu_data);
}5.6 PoseExtrapolator::AddOdometryData()
// 向odom数据队列中添加odom数据,并进行数据队列的修剪,并计算角速度与线速度
void PoseExtrapolator::AddOdometryData(const sensor::OdometryData& odometry_data) {// 1. 时间顺序检查CHECK(timed_pose_queue_.empty() ||odometry_data.time >= timed_pose_queue_.back().time);odometry_data_.push_back(odometry_data);// 2. 加入队列并修剪过时数据TrimOdometryData();// 3. 检查数据量，确保可计算速度。数据队列中至少有2个数据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.// 4. 提取最新与最老的两个里程计数据，计算时间差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);// 5. 计算两个里程计位姿的相对变换const transform::Rigid3d odometry_pose_delta =odometry_data_newest.pose.inverse() * odometry_data_oldest.pose; // 两个里程计位姿间的变化量，tracking_frame坐标系// 6. 计算角速度（从里程计）// 两个位姿间的旋转量除以时间得到 tracking frame 的角速度angular_velocity_from_odometry_ =transform::RotationQuaternionToAngleAxisVector(odometry_pose_delta.rotation()) /odometry_time_delta;if (timed_pose_queue_.empty()) {return;}// 7. 计算线速度（从里程计）// （1）计算里程计坐标系下的线速度（平移变化/时间差）const Eigen::Vector3dlinear_velocity_in_tracking_frame_at_newest_odometry_time =odometry_pose_delta.translation() / odometry_time_delta;// （2） 计算最新里程计时刻的姿态（结合外推的旋转）// 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到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());// （3） 将线速度转换到 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;
}需要注意的是，5计算的是里程计位姿（旋转+平移）的变化量。
// 5. 计算两个里程计位姿的相对变换
const transform::Rigid3d odometry_pose_delta =odometry_data_newest.pose.inverse() * odometry_data_oldest.pose; pose是当前位置在local坐标系下的位姿，也可以看作是以当前坐标为原点的坐标系与local坐标系之间的变换关系。假设A.pose为，则A.pose.inverse()相当于
，B.pose为
。A.pose.inverse() * B.pose相当于
，也就是B在A坐标系下的位姿，相当于B与A的相对位姿，或者说A时刻相对于B时刻的位姿变化量。
7.（1）根据（平移变化量/时间差）计算得到线速度。
7.（2）将系统最新的 “基准位姿”的姿态（旋转），更新（预测）到当前odom数据插入位姿推测器时刻。
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() 是系统最新的 “基准位姿”（由扫描匹配得到）的旋转部分。其中调用了PoseExtrapolator::ExtrapolateRotation()函数。
5.6.1 PoseExtrapolator::ExtrapolateRotation()
函数用于计算imu_tracker到time时刻的姿态变化量：
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(const common::Time time, ImuTracker* const imu_tracker) const {// 检查time要大于等于imu_tracker上次预测位姿的时间CHECK_GE(time, imu_tracker->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();
} 其中又调用PoseExtrapolator::AdvanceImuTracker()函数更新imu_tracker的状态到time时刻。
5.6.2 PoseExtrapolator::AdvanceImuTracker()
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,ImuTracker* const imu_tracker) const {// 1. 检查指定时间是否大于等于 ImuTracker 上一次预测姿态的时间CHECK_GE(time, imu_tracker->time());// 2. 若 imu_data_ 为空，或目标时间早于最早的 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;}// 3. 同步到最早 IMU 数据时间// 若imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间if (imu_tracker->time() < imu_data_.front().time) {// Advance to the beginning of 'imu_data_'.imu_tracker->Advance(imu_data_.front().time);}// 4. 融合 IMU 数据将预测推进到目标时间：// 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->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;});// 然后依次对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_tracker的状态预测到目标time时刻imu_tracker->Advance(time);
}因为imu_data队列中有多个数据，其中可能有多个数据的时间戳处于[imu_tracker->time(), time]之间，所以要一个一个根据imu数据，将姿态的预测向目标time推进。
再回到ExtrapolateRotation()。在AdvanceImuTracker()之后，又执行了：
// 通过imu_tracker_获取上一次位姿校准时的姿态
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
// 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数
return last_orientation.inverse() * imu_tracker->orientation();其中imu_tracker是在ExtrapolateRotation(odometry_data_newest.time,
odometry_imu_tracker_.get())函数调用时传入的odometry_imu_tracker_.get()。而odometry_imu_tracker_是在扫描匹配后的位姿插入到位姿推测器中时对imu_tracker_的拷贝：
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_);
......
}所以imu_tracker->orientation()是上一次pose插入到位姿推测器时（t0时刻）的姿态，last_orientation = imu_tracker_->orientation()，是当前odom数据插入位姿推测器时（t1时刻）预测的姿态。
last_orientation.inverse() * imu_tracker->orientation() 代码的计算结果为t0到t1的姿态变化量。
5.6.3 PoseExtrapolator::AddOdometryData()
接下来再回到AddOdometryData()函数：
const Eigen::Quaterniond orientation_at_newest_odometry_time =timed_pose_queue_.back().pose.rotation() *ExtrapolateRotation(odometry_data_newest.time,odometry_imu_tracker_.get());这一步的计算结果，就是将系统最新的 “基准位姿”的姿态（旋转），更新（预测）到当前odom数据插入位姿推测器时刻。
再下来，7.（3）将里程计计算出的 “tracking_frame坐标系下的线速度” 转换到 “世界坐标系（local frame）” 下
linear_velocity_from_odometry_ =orientation_at_newest_odometry_time *linear_velocity_in_tracking_frame_at_newest_odometry_time; linear_velocity_in_tracking_frame_at_newest_odometry_time：这是从里程计数据直接计算出的线速度，物理意义是 “在tracking frame坐标系下，机器人的运动速度”，没有与系统的世界坐标系对齐。
orientation_at_newest_odometry_time：是 “最新里程计数据时刻（odometry_data_newest.time），tracking_frame坐标系在世界坐标系中的姿态”。
两者相乘后，得到 “最新里程计时刻，世界坐标系下的线速度向量”
通过姿态旋转将速度统一到世界坐标系，确保速度与基准位姿在同一坐标系下。
5.7 PoseExtrapolator::TrimOdometryData()
// 修剪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();}
} odometry_data_必须通过两个里程计位姿计算速度，与IMU不同。
5.8 AddOdometryData()在何处调用
在local_trajectory_builder_2d.cc的LocalTrajectoryBuilder2D::AddOdometryData()函数中被调用：
// 将里程计数据加入到Extrapolator中
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);
}5.9 PoseExtrapolator::AddPose()
AddPose()的使用在执行完扫描匹配之后，功能是使用scanMatch()匹配得到的精准位姿来校准pose_extrapolator。（实际未校准）
void PoseExtrapolator::AddPose(const common::Time time,const transform::Rigid3d& pose) {// 1. 如果imu_tracker_没有初始化就先进行初始化if (imu_tracker_ == nullptr) {common::Time tracker_start = time;if (!imu_data_.empty()) {tracker_start = std::min(tracker_start, imu_data_.front().time);}// imu_tracker_的初始化imu_tracker_ =absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);}// 2. 在timed_pose_queue_中保存posetimed_pose_queue_.push_back(TimedPose{time, pose});// 3. 保持pose队列中第二个pose的时间要大于 time - pose_queue_duration_while (timed_pose_queue_.size() > 2 && // timed_pose_queue_最少是2个数据timed_pose_queue_[1].time <= time - pose_queue_duration_) {timed_pose_queue_.pop_front();}// 4. 根据加入的pose计算线速度与角速度UpdateVelocitiesFromPoses();// 5. 将imu_tracker_更新到time时刻AdvanceImuTracker(time, imu_tracker_.get());// 6. 丢弃1过期数据// pose队列更新了,之前imu及里程计数据已经过时了// 因为pose是匹配的结果,之前的imu及里程计数据是用于预测的,现在结果都有了,之前的用于预测的数据肯定不需要了TrimImuData();TrimOdometryData();// 7. 保存imu_tracker_的副本// 用于根据里程计数据计算线速度时姿态的预测odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);// 用于位姿预测时的姿态预测extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}其中，将扫描匹配后的pose加入到pose队列中,计算线速度与角速度,并将imu_tracker_的状态更新到time时刻。
其中调用了PoseExtrapolator::UpdateVelocitiesFromPoses()函数进行基于pose的线速度与角速度计算：
// 根据pose队列计算tracking frame 在 local坐标系下的线速度与角速度
void PoseExtrapolator::UpdateVelocitiesFromPoses() {if (timed_pose_queue_.size() < 2) {// We need two poses to estimate velocities.return;}CHECK(!timed_pose_queue_.empty());// 取出队列最末尾的一个 Pose,也就是最新时间点的 Pose,并记录相应的时间const TimedPose& newest_timed_pose = timed_pose_queue_.back();const auto newest_time = newest_timed_pose.time;// 取出队列最开头的一个 Pose, 也就是最旧时间点的 Pose,并记录相应的时间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);// 如果时间差小于pose_queue_duration_(1ms), 不进行计算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;// 平移量除以时间得到 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;
}角度变化量由oldest_pose.rotation().inverse() * newest_pose.rotation()计算得到。
5.10 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) {// 预测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}};}return cached_extrapolated_pose_.pose;
}这个函数预测得到time时刻 tracking frame 在 local 坐标系下的位姿。其中，调用ExtrapolateTranslation(time)预测位移，调用ExtrapolateRotation(time, extrapolation_imu_tracker_.get())预测旋转（姿态）。
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);// 使用tracking frame 在 local坐标系下的线速度 乘以时间 得到平移量的预测if (odometry_data_.size() < 2) {return extrapolation_delta * linear_velocity_from_poses_;}// 如果不使用里程计就使用通过pose计算出的线速度return extrapolation_delta * linear_velocity_from_odometry_;
}PoseExtrapolator::ExtrapolateTranslation()的内容较简单，其使用 线速度*时间变化量 计算的到位移变化量。
5.11 PoseExtrapolator::EstimateGravityOrientation()
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(const common::Time time) {ImuTracker imu_tracker = *imu_tracker_;// 使得 imu_tracker 预测到time时刻AdvanceImuTracker(time, &imu_tracker);// 返回 imu_tracker 预测到的time时刻 的姿态return imu_tracker.orientation();
}PoseExtrapolator::EstimateGravityOrientation()用于预测得到time时刻 tracking frame 在 local 坐标系下的姿态（旋转）。
5.12 PoseExtrapolator::ExtrapolatePosesWithGravity()
PoseExtrapolator::ExtrapolationResult
PoseExtrapolator::ExtrapolatePosesWithGravity(const std::vector<common::Time>& times) {// 1. 初始化位姿容器，用于存储外推的位姿std::vector<transform::Rigid3f> poses;// 2. 批量外推位姿（除最后一个时间点）for (auto it = times.begin(); it != std::prev(times.end()); ++it) {poses.push_back(ExtrapolatePose(*it).cast<float>());}// 3. 确定当前速度const Eigen::Vector3d current_velocity = odometry_data_.size() < 2? linear_velocity_from_poses_: linear_velocity_from_odometry_;// 4. 构建并返回外推结果return ExtrapolationResult{poses, // 除最后一个时间点外的所有外推位姿ExtrapolatePose(times.back()), // 最后一个时间点的外推位姿current_velocity, // 当前有效的线速度（local坐标系下）EstimateGravityOrientation(times.back())}; // 最后一个时间点的姿态
} PoseExtrapolator::ExtrapolatePosesWithGravity 函数根据输入的多个目标时间点，批量外推每个时间点的位姿，返回一个包含完整外推结果的结构体。