(02)Cartographer源码无死角解析-(39) PoseExtrapolator→ExtrapolatePose()、ExtrapolatePosesWithGravity() 等成员函数
讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
通过前面的博客，已经对 src/cartographer/cartographer/mapping/pose_extrapolator.cc 文件中的如下函数进行了讲解：
void PoseExtrapolator::AddPose() //添加pose数据
void PoseExtrapolator::AddImuData() //添加Imu数据
void PoseExtrapolator::AddOdometryData() //添加里程计数据
这个三种数据，都包含姿态与位置信息。首先能够想到的问题是，来自于这么多种不同类型的数据，但是都是表述姿态的信息，应该以那个为助，那个为辅呢？这些都是后话，后续再进行详细的讨论，这里主要提及一下，源码中应该是以 Imu 获取的位姿为基准的。
了解了如上三个函数之后，还有身下的函数没有进行分析，如下所示：
// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time){......}
// 预测得到time时刻 tracking frame 在 local 坐标系下的姿态
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(const common::Time time) {......}
void PoseExtrapolator::AdvanceImuTracker(const common::Time time, ImuTracker* const imu_tracker){......}
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(const common::Time time, ImuTracker* const imu_tracker){......}
PoseExtrapolator::ExtrapolateTranslation(common::Time time){......}
// 获取一段时间内的预测位姿的结果
PoseExtrapolator::ExtrapolationResult PoseExtrapolator::ExtrapolatePosesWithGravity(const std::vector<common::Time>& times) {......}
看起来比较吓人奥，怎么还有这么多的函数没有分析呢？起始这些函数又很多相同的地方，理解了一两个之后，剩下的理解起来就比较简单。另外，他们之间还存在相互调用的关系。
二、ExtrapolatePose()
E x t r a p o l a t e P o s e ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ExtrapolatePose()======================================================================================================================================================}: ExtrapolatePose()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 指定一个时间点，然后返回该时间机器人(tracking frame) 在local 坐标系下的位姿。
输入 : {\color{Purple} 输入}: 输入: 【参数①time】→指定一个时间点。
返回 : {\color{Purple} 返回}: 返回:【transform::Rigid3d】 返回指定时间点，机器人(tracking frame) 在local 坐标系下的位姿。
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ================================================================================================================================================================}: ================================================================================================================================================================:
该函数的主要流程如下：
( 1 ) : \color{blue} (1): (1): 获得 timed_pose_queue_ 队列中最后的一个数据，然后检查传入的时间 time 参数，是否比最后一个数据时间点大，如果不是则报错。
( 2 ) : \color{blue} (2): (2): 判断 time 时间点的位姿是否已经推断过，如果已经推断过，则无需再次进行推断，直接返回缓存在 cached_extrapolated_pose_ 的位姿即可，否则需要进行位姿推断。
( 3 ) : \color{blue} (3): (3): 平移预测→首先调用 ExtrapolateTranslation() 函数，预测 time 时间点相对于 newest_timed_pose.time 时间点的平移，然后再在加上 newest_timed_pose.time 时间点的位移。
( 4 ) : \color{blue} (4): (4): 旋转预测→首先调用 ExtrapolateRotation() 函数，预测 time 时间点相对于 newest_timed_pose.pose 时间点的旋转，然后在左乘 newest_timed_pose.pose 时间点的旋转。调用 ExtrapolateRotation() 函数时，除 time 参数外，还传入了参数 extrapolation_imu_tracker_.get()，后面会进行兴详细讲解。
( 5 ) : \color{blue} (5): (5): 预测出来的 translation 与 rotation 存储于 cached_extrapolated_pose_ 中并返回。
代码注释如下：
// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿
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;
}
三、ExtrapolateTranslation()
从上面可以了解到，ExtrapolatePose() 并没有直接进行位姿的推断，而是调用了 ExtrapolateTranslation() 与 ExtrapolateRotation()，首先来看看 ExtrapolateTranslation() 函数。
E x t r a p o l a t e T r a n s l a t i o n ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ExtrapolateTranslation()======================================================================================================================================================}: ExtrapolateTranslation()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 指定一个时间点，然后返回该时间点，机器人(tracking frame) 相对于 timed_pose_queue_ 中最后一个数据时间点的平移，这里是基于local坐标系。
输入 : {\color{Purple} 输入}: 输入: 【参数①time】→指定一个时间点。
返回 : {\color{Purple} 返回}: 返回:【Eigen::Vector3】 返回指定时间点，机器人相对于 timed_pose_queue_ 最后一个数据平平移。
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} =================================================================================================================================================================}: =================================================================================================================================================================:
代码逻辑：简单的说，就是使用 newest_timed_pose.time 到 time 的时间段，然后乘以线速度即可。不过呢？有两种线速度进行选择：①来自于点云匹配的pose；②来自于里程计的。从代码层面来分析，如果使用了 odome 里程计，则会优先使用其平移。也就是说，认为 odome 的平移更加精准。
代码注释如下，公式比较简单，就不再进行推导了，就是单纯的时间乘以速度等于位移而已、
// 返回从最后一个位姿的时间 到time时刻 的tracking frame在local坐标系下的平移量
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_;
}
四、ExtrapolateRotation()
在 ExtrapolatePose() 函数中调用 ExtrapolateRotation() 的代码如下：
const Eigen::Quaterniond rotation = newest_timed_pose.pose.rotation() * ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
可知其传入给ExtrapolateRotation()的的实参为 extrapolation_imu_tracker_.get() 裸指针，通过前面对 PoseExtrapolator::AddPose() 函数的讲解。如下代码：
// 用于根据里程计数据计算线速度时姿态的预测
odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
// 用于位姿预测时的姿态预测
extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
也就是说，odometry_imu_tracker_ 与 extrapolation_imu_tracker_ 都是来自于 imu_tracker_ 对象的拷贝。
E x t r a p o l a t e R o t a t i o n ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ExtrapolateRotation()======================================================================================================================================================}: ExtrapolateRotation()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 指定一个时间点，返回该时间相对于 imu_tracker 最新姿态的旋转变化
输入 : {\color{Purple} 输入}: 输入: 【参数①time】→指定一个时间点。【参数②imu_tracker】→用于姿态推断的ImuTracker对象
返回 : {\color{Purple} 返回}: 返回:【Eigen::Quaterniond】 time 时间点机器人相对于 imu_tracker 最新位姿的旋转变化。
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ==================================================================================================================================================================}: ==================================================================================================================================================================:
该函数的代码逻辑比较简单
( 1 ) : \color{blue} (1): (1): 首先调用 PoseExtrapolator::AdvanceImuTracker() 函数，通过 imu_tracker 预测 time 时刻的姿态，这里记录为： o r i e n t a t i o n t i m e i n i t \mathbf {orientation}^{init}_{time} orientationtimeinit
( 2 ) : \color{blue} (2): (2): 通过 imu_tracker_->orientation() 获取上一次校准时的姿态，这里记为 o r i e n t a t i o n t i m e _ i n i t \mathbf {orientation}^{init}_{time\_} orientationtime_init
( 3 ) : \color{blue} (3): (3): 通过 o r i e n t a t i o n t i m e i n i t \mathbf {orientation}^{init}_{time} orientationtimeinit 与 o r i e n t a t i o n t i m e _ i n i t \mathbf {orientation}^{init}_{time\_} orientationtime_init 即可推导出 time_ 到 time 期间的姿态(方向)变化，数学矩阵形式可以表示如下： r o t a t i o n t i m e _ t i m e = [ o r i e n t a t i o n t i m e i n i t ] − 1 ∗ o r i e n t a t i o n t i m e _ i n i t (01) \color{Green} \tag{01} \mathbf {rotation}^{time}_{time\_}=[\mathbf {orientation}^{init}_{time}]^{-1}*\mathbf {orientation}^{init}_{time\_} rotationtime_time=[orientationtimeinit]−1∗orientationtime_init(01)
代码注释如下：
// 计算从imu_tracker到time时刻的姿态变化量
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
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();
}
五、AdvanceImuTracker()
在上面的 ExtrapolateRotation() 函数中调用了 AdvanceImuTracker()，其主要功能是把形参 imu_tracker 的状态更新到 time 时刻，或者说预测 time 时刻姿态。
A d v a n c e I m u T r a c k e r ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} AdvanceImuTracker()======================================================================================================================================================}: AdvanceImuTracker()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 指定一个时间点，使用 imu_tracker 推测该时间点姿态，然后返回
输入 : {\color{Purple} 输入}: 输入: 【参数①time】→指定一个时间点。【参数②imu_tracker】→用于姿态推断的ImuTracker对象
返回 : {\color{Purple} 返回}: 返回:【Eigen::Quaterniond】 imu_tracker 推断出 time 时刻的位姿
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ==================================================================================================================================================================}: ==================================================================================================================================================================:
该函数实际上是比较复杂的，不过经过上一篇博客对 ImuTracker 的分析，现在分析起来就比较简单了。
( 1 ) : \color{blue} (1): (1): 如果目前 imu_data_ 的数据为空，或者 time 时间点小于 imu_data_ 第一个数据的时间点。那么此时会利用来自于 poses 或者 odometry 的 angular_velocity_from_poses_ 与 angular_velocity_from_odometry_ 数据对 imu_tracker 进行更新，然后直接返回。该更新主要执行如下3个步骤。
( 1.1 ) : \color{blue} (1.1): (1.1): 调用 imu_tracker->Advance(time) 函数，预测当前时刻的姿态与重力方向。该函数预测主要使用到 ImuTracker 中的 orientation_ 与 gravity_vector_ 成员变量，这两个变量在构造函数中就进行了初始化。
( 1.2 ) : \color{blue} (1.2): (1.2): 调用 imu_tracker->AddImuLinearAccelerationObservation() ，添加Imu线性加速度数据。不过此时Imu数据都没有，那么 Imu 线性加速度当然也没有，所以通过 Eigen::Vector3d::UnitZ() 模拟了一个重力加速度。
( 1.3 ) : \color{blue} (1.3): (1.3): 调用imu_tracker->AddImuAngularVelocityObservation() 对角速度更新，不过同上面一样，使用的是来自于 poses 或这 odometry 的角速度。这里是以 odometry 里程计的角速度优先的。
( 2 ) : \color{blue} (2): (2): 如果目前 imu_data_ 的数据不为空， 则判断形参 imu_tracker->time() 是否小于 imu_data_ 第一个数据的时间。如果小于，那么就把 imu_tracker 的状态更新到 imu_data_ 第一个数据时间的姿态。
( 4 ) : \color{blue} (4): (4): 调用 std::lower_bound 函数，在 imu_data_ 队列中找到时间小于 time 的第一个数据，迭代器it指向该数据。
( 5 ) : \color{blue} (5): (5): 使用while循环遍历 imu_data_ 中所有时间低于 time 的数据，然后使用 imu_tracker 对所有数据的时间位姿进行预测，每次预测之后都会使用 Imu 数据的加速度与角速度对预测出来的姿态进行校准。
( 6 ) : \color{blue} (6): (6): 最后返回 imu_tracker 对 time 时刻位姿的预测结果。
总结 : \color{red} 总结: 总结: 该函数主要利用 imu_data_ 所有数据，对 imu_tracker 进行校正。然后再用校正好的 imu_tracker 预测 time 时刻的姿态。代码注释如下：
/**
* @brief 更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻
*
* @param[in] time 要预测到的时刻
* @param[in] imu_tracker 给定的先验状态
*/
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const {
// 检查指定时间是否大于等于 ImuTracker 的时间
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.
// 在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的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间
if (imu_tracker->time() < imu_data_.front().time) {
// Advance to the beginning of 'imu_data_'.
imu_tracker->Advance(imu_data_.front().time);
}
// c++11: std::lower_bound() 是在区间内找到第一个大于等于 value 的值的位置并返回, 如果没找到就返回 end() 位置
// 在第四个参数位置可以自定义比较规则,在区域内查找第一个 **不符合** comp 规则的元素
// 在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);
}
六、EstimateGravityOrientation()
到目前为止，对于 ExtrapolatePose() 函数及其内部调用到的函数都讲解完成了，下面来看看 EstimateGravityOrientation() 函数。
E s t i m a t e G r a v i t y O r i e n t a t i o n ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} EstimateGravityOrientation()======================================================================================================================================================}: EstimateGravityOrientation()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 指定一个时间点，返回该时间相对于 imu_tracker_ 预测出的 time 时刻姿态(推断出来的重力方向)。
输入 : {\color{Purple} 输入}: 输入: 【参数①time】→指定一个时间点。
返回 : {\color{Purple} 返回}: 返回:【Eigen::Quaterniond】 预测的 time 时刻姿态(推断出来的重力方向)。
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ======================================================================================================================================================================}: ======================================================================================================================================================================:
其与 ExtrapolateRotation() 十分相似，ExtrapolateRotation() 预测的是变化量，且能够通过参数指定 ImuTracker对象， EstimateGravityOrientation() 预测的是 time 时刻的位姿，固定使用 imu_tracker_ 进行预测。代码比较简单，注释如下，
// 预测得到time时刻 tracking frame 在 local 坐标系下的姿态
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();
}
七、ExtrapolatePosesWithGravity()
E x t r a p o l a t e P o s e s W i t h G r a v i t y ( ) = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ExtrapolatePosesWithGravity()======================================================================================================================================================}: ExtrapolatePosesWithGravity()======================================================================================================================================================:
功能 : {\color{Purple} 功能}: 功能: 指定一个n个时间点，返回 PoseExtrapolator::ExtrapolationResult 结构体，其包含了 n 个时间点位姿信息，以及当前的线速度，及估算出来的重力方向。
输入 : {\color{Purple} 输入}: 输入: 【参数①times】→ vector形式存储的多个时间点
返回 : {\color{Purple} 返回}: 返回:【PoseExtrapolator::ExtrapolationResult】含n个时间点，推测出来的位姿，以及当前线速度，及估算出来的重力方向。
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = : {\color{Purple} ====================================================================================================================================================================}: ====================================================================================================================================================================:
该函数比较简单，就是调用前面讲解过的函数，其返回的线速度是以里程计为主的，也就是说其认为里程计的线速度比来自于 poses 的线速度更准。
代码注释如下：
// 获取一段时间内的预测位姿的结果
PoseExtrapolator::ExtrapolationResult
PoseExtrapolator::ExtrapolatePosesWithGravity(
const std::vector<common::Time>& times) {
std::vector<transform::Rigid3f> poses;
// c++11: std::prev 获取一个距离指定迭代器 n 个元素的迭代器,而不改变输入迭代器的值
// 默认 n 为1,当 n 为正数时, 其返回的迭代器将位于 it 左侧；
// 反之, 当 n 为负数时, 其返回的迭代器位于 it 右侧
// 获取 [0, n-1] 范围的预测位姿
for (auto it = times.begin(); it != std::prev(times.end()); ++it) {
poses.push_back(ExtrapolatePose(*it).cast<float>());
}
// 进行当前线速度的预测
const Eigen::Vector3d current_velocity = odometry_data_.size() < 2
? linear_velocity_from_poses_
: linear_velocity_from_odometry_;
return ExtrapolationResult{poses, ExtrapolatePose(times.back()),
current_velocity,
EstimateGravityOrientation(times.back())};
}
八、结语
到目为止对于 PoseExtrapolator 的成员函数，可以说是讲解完成了。但是对于其具体的用法和作用还是比较懵逼的。比如说，最前面提到的：
void PoseExtrapolator::AddPose() //添加pose数据
void PoseExtrapolator::AddImuData() //添加Imu数据
void PoseExtrapolator::AddOdometryData() //添加里程计数据
三个函数，可以往推测器中添加各种数据，那么其又是如何取舍，或者以谁为主的呢？这些疑问，在下一篇博客再进行揭秘。