本文初始架构转自Cartographer源码阅读11—LocalTrajectoryBuilder2D成员变量解读
PoseExtrapolator定义在/mapping/pose_extrapolator.h中。其主要作用是对IMU、里程计数据进行融合，估计机器人的实时位姿。
从cartographer算法的流程框图(见Cartographer源码阅读1——整体框架介绍(https://zhuanlan.zhihu.com/p/48010119)中第一个图)我们可以知道，PoseExtrapolator的输入有三个：从里程计传来的数据、经过ImuTracker处理过的对重力进行aligned过的IMU数据、从Scan Matching输出的Pose Observation。PoseExtrapolator对这三个输入进行融合后输出估计出来的PoseEstimate。
从该类开头的一段注释我们也可以知道，该类通过从Scan Matching输出的PoseObservation持续一段时间来跟踪Poses，从而估计机器人的线速度和角速度。通过速度来解算机器人的运动。当IMU或里程计数据可用时，可与这些数据融合来提升计算结果。
一、成员变量
<cartographer>/cartographer/mapping/pose_extrapolator.h
------
class PoseExtrapolator : public PoseExtrapolatorInterface {
// Duration是在/common/time.h中定义的别名
// using Duration = UniversalTimeScaleClock::duration; //微秒,0.1us
// UniversalTimeScaleClock类实现c++11的clock接口，以0.1us为时间精度。
// 在rose_ros，值1000。0.001秒，即1毫秒。这时间基本就是timed_pose_queue_中最多2个单元。
const common::Duration pose_queue_duration_;
// 带时间的Pose
struct TimedPose {
common::Time time;
transform::Rigid3d pose;
};
// 带时间的Pose队列，存储要持续跟踪的Poses，应该是从ScanMatching输出的PoseObservation
std::deque<TimedPose> timed_pose_queue_;
// 从持续跟踪一段时间的Pose队列中估计出来的线速度，初始值为0
Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
// 从持续跟踪一段时间的Pose队列中估计出来的线速度，初始值为0
Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();
//重力的时间间隔？
const double gravity_time_constant_;
//存储IMU数据的队列
std::deque<sensor::ImuData> imu_data_;
//ImuTracker,定义在/mapping/imu_tracker.h中。存放由IMU数据得到的信息
std::unique_ptr<ImuTracker> imu_tracker_;
// 存放由里程计得到的信息
std::unique_ptr<ImuTracker> odometry_imu_tracker_;
// 存放经过数据融合后的结果
std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
// 缓存的一个带时间的Pose
TimedPose cached_extrapolated_pose_;
//里程计数据
std::deque<sensor::OdometryData> odometry_data_;
// 从里程计获取到的线速度，初始化为0
Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
// 从里程计获取到的角速度，初始化为0
Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
};
可以看到，与该类的功能相对应，成员变量里包括了：标记持续时间的长短的pose_queue_duration_、记录这段时间的Pose的一个队列timed_pose_queue_、从pose队列中估计出来的线速度和角速度，以及分别从IMU和里程计获得的信息。
根据上面，首先分析下LocalTrajectoryBuilder2D::AddRangeData要调用的ExtrapolatePose。
二、ExtrapolatePose
ExtrapolatePose用于推算指定时刻的Pose。time一般指的是点云中各个点的采集时间，对此次点云的最后一个点，time就是零时刻。
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
// 取出Pose队列中的最新值(收到第k次点云的零时刻pose)：47.56414
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
// 当前time一定要比队列中的时间更新
CHECK_GE(time, newest_timed_pose.time);
if (cached_extrapolated_pose_.time != time) {
// 如果缓存的Pose的时间不等于指定时间
// 最新translation等于最新Pose的translation再加上这段时间的平移增长量
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
// 最新旋转等于最新Pose的rotation再加上这段时间的旋转增长量。47.56414 + 4.45687 = 52.02102。
// 4.45687是(time - k-1次点云的零时刻)这段时间的imu旋转变化量
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;
}会经常调用ExtrapolatePose(time)，而且连着数次参数time往往相同，为提高效率，Extrapolator就用了一个缓存变量cached_extrapolated_pose_，缓存着指定time时的位姿。这样对指定time，只要第一次计算，后面就可直接取cached_extrapolated_pose_.pose，直到来了新的time。
ExtrapolatePose怎么计算位姿？来自两处变量：一是timed_pose_queue_队列最新单元中的TimedPose，二是ExtrapolateTranslation和ExtrapolateRotation。
首先看timed_pose_queue_存的是什么。AddPose是入队timed_pose_queue_的唯一入口函数，负责在时刻time往Pose队列中添加一个Pose。
三、AddPose
void PoseExtrapolator::AddPose(const common::Time time,
const transform::Rigid3d& pose) {
if (imu_tracker_ == nullptr) {
// 如果ImuTracker还没有建立，则需要建立一个ImuTracker
common::Time tracker_start = time;
if (!imu_data_.empty()) {
// 如果IMU数据队列不为空，则以当前时间和IMU数据中的最早时刻的较小值为初始时刻建立一个ImuTracker
tracker_start = std::min(tracker_start, imu_data_.front().time);
}
// 生成ImuTracker
imu_tracker_ =
absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
}
// 把Pose压入队列
timed_pose_queue_.push_back(TimedPose{time, pose});
// Pose队列大于2，并且时间间隔已经大于我们设定的pose_queue_duration_时，则把队列之前的元素删除
while (timed_pose_queue_.size() > 2 &&
timed_pose_queue_[1].time <= time - pose_queue_duration_) {
timed_pose_queue_.pop_front();
}
// 计算机器人的瞬时线速度(linear_velocity_from_poses_)和瞬时角速度(angular_velocity_from_poses_)
UpdateVelocitiesFromPoses();
// 更新ImuTracker
AdvanceImuTracker(time, imu_tracker_.get());
//更新IMU数据队列
TrimImuData();
//更新里程计数据队列
TrimOdometryData();
// 让两个临时ImuTracker和imu_tracker_保持一致
odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}目前不必理会AddPose中自UpdateVelocitiesFromPoses()后的那段逻辑。看是什么时候调用AddPose。当没用imu时，就两处。
- LocalTrajectoryBuilder2D::InitializeExtrapolator。保证在调用ExtrapolatePose(time)时，队列中至少有一个“0”值。
- LocalTrajectoryBuilder2D::AddAccumulatedRangeData。ScanMatch之后结果，再考虑重力后的Pose。
综上所述，在timed_pose_queue_存储的是那些个Scan插入Submap的最优Pose，然后考虑重力方向，将2d的pose变成3d的pose。它指示了机器人在time时的位姿。
上面出现了三个ImuTracker：imu_tracker_、odometry_imu_tracker_和extrapolation_imu_tracker_。在一次LocalTrajectoryBuilder2D::AddRangeData过程中，imu_tracker_中状态只在最后(AddPose)时才会变换。odometry_imu_tracker_、extrapolation_imu_tracker_则是在外推点云中的每个点时就要变换一次，e有时也就叫临时ImuTracker。计算出一次local_pose后，后两个就要和imu_tracker_保持一致，即下次点云来时，是以上一次imu_tracker_状态为基准。
接下看ExtrapolateRotation和ExtrapolateTranslation，它们分别计算旋转和平移的增长量
四、ExtrapolateRotation和ExtrapolateTranslation
ExtrapolateRotation功能是要算出(time - k-1次点云的零时刻)这段时间的旋转变化量。参数imu_tracker不会是imu_tracker_，往往是extrapolation_imu_tracker_。
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
//检查指定时间是否大于等于ImuTracker的时间。
CHECK_GE(time, imu_tracker->time());
//更新ImuTracker到指定的time
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
// 求取旋转变化量：上一时刻旋转的逆乘以当前的旋转。
// -43.22239 + 47.67926 = 4.45687
// 43.22239: imu_tracker_.orientation_要等到AddPose后才会更新，目前值就imu_tracker在k-1次点云零时刻的orientation_。
// 47.67926: time时刻imu_tracker的orientation_。
return last_orientation.inverse() * imu_tracker->orientation();
}计算(time - newest_timed_pose.time)这段时间平移的变换量
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
// 取出Pose队列中最新的时刻
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) {
// 没有里程计数据，从Pose队列中估计的线速度乘以时间。rose_ros进这入口。
return extrapolation_delta * linear_velocity_from_poses_;
}
// 有里程计数据，则更信任里程计速度，直接把从里程计处获得的线速度乘以时间
return extrapolation_delta * linear_velocity_from_odometry_;
}从上面两个函数可以看出，对于rotation而言，更信任IMU的数据，如果有IMU数据则用IMU数据；对于Translation而言，更信任里程计数据，如果有里程计数据则优先使用里程计数据。没有的话就用Pose队列估计出来的线速度。而对于IMU的线速度并不信任。这跟IMU的传感器特性是相关的。
不论平移还是旋转，它们都由两部分构成，一是oldest到newest的变化量，二是newest到time的变化量。对oldest到newest的变化量，是UpdateVelocitiesFromPoses在进行计算。
五、UpdateVelocitiesFromPoses
该函数的主要工作就是取出timed_pose_queue_这个队列中最老(oldest)和最新(newest)的两个Pose做差，然后除以这两个点的时间间隔得到机器人的瞬时线速度(linear_velocity_from_poses_)和瞬时角速度(angular_velocity_from_poses_)。
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
if (timed_pose_queue_.size() < 2) {
// timed_pose_queue_至少得存在两个pose，因为要由首、尾两个pose算出瞬时角/线速度。
// 只有一个的话，做为除数的时间间隔是0，就没意义了。
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);
// 如果时间差小于1ms，则估计不准，弹出警告信息
if (queue_delta < common::ToSeconds(pose_queue_duration_)) {
LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
<< queue_delta << " s";
return;
}
// 获取两个时刻各自的Pose
const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
// 线速度即为两个Pose的translation部分相减后除以间隔时间
linear_velocity_from_poses_ =
(newest_pose.translation() - oldest_pose.translation()) / queue_delta;
// 角速度是两个Pose的rotation部分的差
angular_velocity_from_poses_ =
transform::RotationQuaternionToAngleAxisVector(
oldest_pose.rotation().inverse() * newest_pose.rotation()) /
queue_delta;
}算出的角速度放在angular_velocity_from_poses_，类型是Eigen::Vector3d，但它其实是个旋转向量。对如何表示旋转向量，Eigen往往用Eigen::AngleAxisd，cartogrphaer没用它，而是Eigen::Vector3d，它的[0]、[1]、[2]分别对应旋转向量的roll、pitch、yaw。carotographer自实现的RotationQuaternionToAngleAxisVector用于把四元数转成旋转向量，AngleAxisVectorToRotationQuaternion则把旋转向量转成四元数。
六、AdvanceImuTracker
PoseExtrapolator::AdvanceImuTracker的主要作用就是从IMU数据队列中取出最新的数据，更新ImuTracker的状态到指定的时间time。
// 从IMU数据队列中取出最新的数据，更新ImuTracker的状态到指定的时间time
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const {
//当前时间是否晚于ImuTracker的时间,如果是，需要更新ImuTracker
CHECK_GE(time, imu_tracker->time());
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数据队列为空，或当前时间要比IMU数据队列中最早期的时间还要早。说明没有可用的IMU数据。
// 这时候根据时间推算。对于角速度，是用从local-pose中估计的角速度或从里程计获得的角速度更新ImuTracker的角速度
imu_tracker->Advance(time);
// 用一个fake的重力方向更新线加速度。这个fake的重力方向就是[0,0,1]。理想情况。
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
imu_tracker->AddImuAngularVelocityObservation(
odometry_data_.size() < 2 ? angular_velocity_from_poses_
: angular_velocity_from_odometry_);
return;
}
// 如果ImuTracker维护的时间早于IMU数据队列最早期的时间
if (imu_tracker->time() < imu_data_.front().time) {
// Advance to the beginning of 'imu_data_'.
// 先把ImuTracker更新到IMU数据来临的那一刻
imu_tracker->Advance(imu_data_.front().time);
}
//然后依次取出IMU数据队列中的数据，更新ImuTracker，直到IMU数据的时间比指定时间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;
});
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);
}ImuTracker的主要作用就是根据IMU的读数维护传感器当前的姿态、线加速度(经过重力校正的)、当前姿态、重力方向、角速度等量。这些量都是以ImuTracker刚建立时的那一时刻IMU本身的坐标系为基准坐标系。具体函数的实现代码如下。
ImuTracker::Advance功能是把ImuTracker状态更新到指定时刻time，状态包括指示旋转值的orientation_，以及time_、gravity_vector_。函数注解见“计算位姿”中“二、更新旋转值”。
七、ImuTracker::AddImuLinearAccelerationObservation
功能是根据读数更新线加速度。这里的线加速度是经过重力校正的。
void ImuTracker::AddImuLinearAccelerationObservation(
const Eigen::Vector3d& imu_linear_acceleration) {
// Update the 'gravity_vector_' with an exponential moving average using the
// 'imu_gravity_time_constant'.
// 求时间差
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_;
last_linear_acceleration_time_ = time_;
const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
gravity_vector_ =
(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
// Change the 'orientation_' so that it agrees with the current
// 'gravity_vector_'.
const Eigen::Quaterniond rotation = FromTwoVectors(
gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
orientation_ = (orientation_ * rotation).normalized();
CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}
根据读数更新角速度
void ImuTracker::AddImuAngularVelocityObservation(
const Eigen::Vector3d& imu_angular_velocity) {
imu_angular_velocity_ = imu_angular_velocity;
}
重力校正这一块的公式我每太看懂，但由于时间关系，就不再深入了解了。读者如有兴趣可自行查看。
ImuTracker的主要作用就是根据IMU的读数维护传感器当前的姿态、线加速度(经过重力校正的)、当前姿态、重力方向、角速度等量。这些量都是以ImuTracker刚建立时的那一时刻IMU本身的坐标系为基准坐标系。具体函数的实现代码如下。为全面，再补上ImuTracker构造函数。
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()),
//初始姿态为I
orientation_(Eigen::Quaterniond::Identity()),
//重力方向初始化为[0,0,1]
gravity_vector_(Eigen::Vector3d::UnitZ()),
//角速度初始化为0
imu_angular_velocity_(Eigen::Vector3d::Zero()) {}