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

一、前言

接下来就是对 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& 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)\color{blue}{(1)}(1) imu_data_ 中的元素个数大于1 → imu_data_.size() > 1

(2)\color{blue}{(2)}(2) timed_pose_queue_ 队列不为空 → !timed_pose_queue_.empty()

(3)\color{blue}{(3)}(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_ 最后一个元素的时间，也会停止循环。

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

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

四、AddOdometryData()→逻辑讲解

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

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

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

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

(2)\color{blue}{(2)}(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 表示基于