这一章节呢，主要讲外推器。我们上一章节说到激光的畸变与矫正，最关键的是什么呢？其实最关键的就是关于每个发射点的发射位置的估计推测，这里就用到了外推器去推测每个发射点的位置。
local_trajectory_builder_2d.cc
if (synchronized_data.ranges.empty(std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data) {
......
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
......
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
sensor::RangefinderPoint hit_in_local =
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
......
}
}
......
}
其中可以看到每个点都有一个外推的pose------range_data_poses[i]。
再往前追溯，就可以知道，其实每个点都是外推器给推出来的，我们可以找到代码看一下。
local_trajectory_builder_2d.cc
for (const auto& range : synchronized_data.ranges) {
......
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}其中time_point是每一个激光点发射对应的时间戳。那么今天就仔细讲一下这个外推器是怎么进行位姿推算的。首先我们看ExtrapolatePose是怎么实现的。
pose_extrapolator.cc
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) {
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
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;
}下面进行逐行讲解
CHECK_GE(time, newest_timed_pose.time);
首先检查time是否大于等于外推最新的pose的时间，newest_timed_pose.time。其中，cached_extrapolated_pose_是外推器外推的pose.
if (cached_extrapolated_pose_.time != time) {这句的意思是在某个时间点上进行不重复的外推。也就是曾经没有外推过的时间，这都是一些时间上的检查。
我们既然要外推pose，那么就是两方面，一个是平移translation，一个是旋转rotation 先看平移。
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
那么这个平移是什么呢？其实就是在先前pose的基础上newest_timed_pose.pose.translation()，再加上一个
ExtrapolateTranslation(time)。
pose_extrapolator.cc
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);
if (odometry_data_.size() < 2) {
return extrapolation_delta * linear_velocity_from_poses_;
}
return extrapolation_delta * linear_velocity_from_odometry_;
}我们外推器里面存储了一系列的pose,linear_velocity_from_poses_。比如说滑动窗口，一个10个pose，新来了pose，就把最老的pose给干掉。然后我们用最新的减去最旧的，最后再除以时间差，得到这个速度，也就是linear_velocity_from_poses_，得到了之后呢。然后我们要用我们要外推需要的时间time，减去最新的pose的时间。其中要外推的时间肯定比最新的pose时间大，相减之后得到的时间是t，
extrapolation_delta，然后乘以从历史存储得到的速度，也就是linear_velocity_from_poses_，然后得到了在t时间内的平移。这是在里程计数据比较少的情况下，
if (odometry_data_.size() < 2)。
但是当里程计数据足够呢？
那么我们就用时间t乘以从里程计获得的速度。代码如下:
return extrapolation_delta * linear_velocity_from_odometry_;
其中linear_velocity_from_odometry_是用里程计算出的一个速度。
如果里程计数据足够，就用里程计做外推，如果没有，就之前存储的位姿来推算速度。
好了，平移说完了，那么就再说说旋转Rotation。
const Eigen::Quaterniond rotation =
newest_timed_pose.pose.rotation() *
ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
同样是最新的位姿的旋转向量newest_timed_pose.pose.rotation()的基础上，乘以
rotation,那么如何算
rotation，可以看到传入了两个向量，不仅有time，而且还有extrapolation_imu_tracker_.get()，那么extrapolation_imu_tracker_.get()这个是什么东西呢。我们展开来看一下。
pose_extrapolator.h
std::unique_ptr<ImuTracker> imu_tracker_;
std::unique_ptr<ImuTracker> odometry_imu_tracker_;
std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
我们可以看到有三个imu_tracker。一个是imu_tracker_，一个是odometry_imu_tracker_，还有一个是extrapolation_imu_tracker_。这具体是干什么的呢？后面我们再讲。我们先回归pose_extrapolator.cc到ExtrapolateRotation()这个函数中进行展开讲。
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker->orientation();
}
传入的imu_tracker是外推器imu_tracker------extrapolation_imu_tracker_
CHECK_GE(time, imu_tracker->time());
我们不会往历史时刻做外推，所以检查time大于等于imu_tracker->time()。AdvanceImuTracker(time, imu_tracker);这个先不展开讲，再看接下来做了什么。
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
首先是把这一次的imu_tracker_->orientation()作为上一次的last_orientation,也就是更新值，其中imu_tracker_是有一个下划线。从这个带下划线的imu_tracker_拿出一个一个朝向角，从这个朝向角乘以外推器的朝向角。他们做一个差，得到一个orientation。说明imu_tracker_是一直实时记录最新估计的imu的朝向角的。然后传入的imu_tracker是往前外推的imu_tracker的朝向角。所以是往前外推一下的orientation乘以上一次位姿的orientation的逆，也就是差，得到了
orientation。得到了这个旋转，最后再乘以最新的旋转newest_timed_pose.pose.rotation()。那么我们再来看一下，imu_tracker->orientation()是怎么计算的。也即是AdvanceImuTracker(time, imu_tracker);函数。
pose_extrapolator.cc
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const {
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_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;
}
if (imu_tracker->time() < imu_data_.front().time) {
// Advance to the beginning of 'imu_data_'.
imu_tracker->Advance(imu_data_.front().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);
}
接下来进行逐行解析吧，首先第一行。还是老套路，检查时间。
CHECK_GE(time, imu_tracker->time());
首先假设我们不直接使用imu，来做轨迹推算，直接用一个比较好的里程计来做推算。imu就是和编码器融合推演出一个比较好的里程计上面。不把imu直接用到cartographer上面。首先我们看这一种情况下是怎么操作的。
if (imu_data_.empty() || time < imu_data_.front().time)
首先是imu数据是空的，或者是外推的时间小于了imu的时间。那么我们再看看下面是怎么操作的。
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->Advance(time);是干什么的？
imu_tracker.cc
void ImuTracker::Advance(const common::Time time) {
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized();
gravity_vector_ = rotation.conjugate() * gravity_vector_;
time_ = time;
}从函数名Advance中就知道，就是前进到这个时刻time。其中有一个是time_，我们可能就比较困惑了，这个是从哪里来的呢，其实这是imu的编码规范，其实就是类的成员，至于这里是哪个类呢，显然是ImuTracker这个类的成员。这是time_具体是什么呢？其实就是我们一次一次Advance到的时间。可以看到代码最后一行time_ = time;，这就是我们最后的是时间，赋值给它。
const double delta_t = common::ToSeconds(time - time_);
而delta_t是我们将要赶到的时间点距离上一次赶到的时间点之间的差。
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
其中imu_angular_velocity_是imu的角速度。如果不用imu了，那么他就是0，那么rotation就是0。
orientation_ = (orientation_ * rotation).normalized();
orientation_就保持不变，乘以0还是原来的orientation_
gravity_vector_ = rotation.conjugate() * gravity_vector_;
gravity_vector_就是考虑重力对齐的关系。主要考虑的是imu航向角是否与水平面水平，如果不是，那么就要将重力划分到imu坐标系中。
回过头来，我们看一下Advance到底做了什么？
如果我们有imu数据的时候，也就是有了imu的角速度imu_angular_velocity_。通过这个角速度乘以时间t ，得到一个
rotation。有了imu自己的rotation，然后更新自己的重力向量。然后还有什么呢？这一些的ImuTracker Rotation其实就是在算
rotation。然后更新自己的orientation_。
现在imu_tracker->Advance()是干什么，就是更新朝向角，再更新一下重力向量的表示。
然后再接着回溯代码
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
其中imu_tracker是外推器的imu_tracker。这一步是添加imu的线速度加速度观测。因为我们假设我们没有imu数据，再进一步假设我们在2D平面上运行。那么我们只有一个在Z方向上的重力向量Eigen::Vector3d::UnitZ()。
imu_tracker.cc
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_ = 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);
}
虽然我们之前假设没有imu数据，但是当我们有imu数据的时候，我们添加imu线性加速度的作用是什么吧。我们先不管传入的是什么东西，我们先看这个函数吧。
void ImuTracker::AddImuLinearAccelerationObservation(
const Eigen::Vector3d& imu_linear_acceleration) {
......
}
首先填入进入的是一个线性加速度imu_linear_acceleration。主要是做了指数平均滤波，也就是互补滤波
指数平均滤波的权值是以指数式递减的
指数移动平均无论多旧的数据其总会被给予一定的权值
参考：javascript:void(0) 相关链接：https://zhuanlan.zhihu.com/p/76188487
首先是一个条件表达式，其中time_我们可以从之前讲的代码中得知已经更新到最新了，last_linear_acceleration_time_是上一次的线性加速度时间，两者想减得到，
last_linear_acceleration_time_ > common::Time::min()这个是验证时间的有效性。
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_;
下一步就是刚才引用的指数平均滤波的代码表示，其中imu_gravity_time_constant_是一个常数，可以知道，当越小的时候，
就越大。
const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
gravity_vector_ =
(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
其中imu_linear_acceleration为imu的测量值，如果,
IMU短时间内角速度还是比较准确的。 所以越大，我们越不相信之前的角速度
(1. - alpha) * gravity_vector_，就越相信当前的测量值alpha * imu_linear_acceleration。虽然不太准，但这就是重力向量在机器人体内，也就是机体内怎么表示的。
接下来呢，我们首先算一个旋转向量rotation。
const Eigen::Quaterniond rotation = FromTwoVectors(
gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
FromTwoVectors就是从一个向量转到另一个向量是怎么旋转的。第一个向量是重力向量gravity_vector_。第二个向量是什么呢？是orientation_（四元数）共轭 也就是旋转的逆。就是把这个四元数转换成向量的形式。Z轴上的向量Eigen::Vector3d::UnitZ()都旋转到 orientation_.conjugate()这个方向上。变成这么一个朝向上的向量。
这里我画图解释一下，首先假设我们有一个车，车体自己有一个坐标系以及航向。蓝色线为水平线
然后再画出我们已经算出来的重力向量。
我们假设算出的重力向量很准。然后呢，假设我们还知道航向与水平面的夹角。首先我们要知道，航向的四元数的共轭向量，本质就是旋转的逆。正向旋转是逆时针，反向旋转是顺时针，
如果这个旋转的逆，乘以这个UnitZ ,也就是意味着UnitZ 顺时针旋转角度。
绿色线为旋转之后的UnitZ。然后我们再求出UnitZ与重力向量直线的旋转向量，再将这个旋转向量乘以航向角，来纠正航向角，航向角向下旋转的角度就是新的UnitZ与重力向量之间的夹角。实现代码如下：
orientation_ = (orientation_ * rotation).normalized();
效果如下：紫色为纠正后的航向角
使用重力加速度来纠正我们估计的朝向。
怎么纠正呢，理论上来说，我们车体坐标系上的Z轴上的单位向量，乘以航向角的逆，也就是逆航向角进行旋转，应该是跟重量向量方向是重合的，如果不重合，那么就对航向角进行矫正，矫正的大小就是旋转后的Z轴与重力向量之间的旋转向量rotation。
这就是使用重力加速度来纠正估计航向的原理
CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
关于这两行代码呢。我有我自己的个人理解。可能不对，大家姑且一听，首先呢，orientation_ * gravity_vector_ 是对重力向量的矫正，让重力向量的精度更高，尽可能的垂直与水平面，之后，判断校正后的重力向量的z分量是否存在，存在是否大于0，理论上重力向量是朝上的，所以z的值是大于0的。为什么重力向量朝上，请看下面的引用。
重力向量为什么朝上，重力和器件之间作用力和反作用力，重力向下，测量值朝上。
这部分的代码我们讲完了，我们回溯一下，当前函数的名字是ImuTracker::AddImuLinearAccelerationObservation 这个是IMU加入线性加速度观测，加入观测的目的是什么呢？纠正当前的状态。我们之前利用角速度，求出朝向角。然后求出重力朝向，重力朝向去矫正航向角。
然后再回溯到上一层。
pose_extrapolator.cc
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
......
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
imu_tracker->AddImuAngularVelocityObservation(
odometry_data_.size() < 2 ? angular_velocity_from_poses_
: angular_velocity_from_odometry_);
return;
}
}
执行完IMU线速度观测，继续知行IMU角速度观测。也就是AddImuAngularVelocityObservation。
假设我们第一次进入AdvanceImuTracker这个函数，且有IMU数据，并且我们满足If条件中的time < imu_data_.front().time这个条件。在这个条件下，我们继续看AddImuAngularVelocityObservation这个函数是什么意思。这个函数是添加了IMU角速度观测值。
pose_extrapolator.cc
void ImuTracker::AddImuAngularVelocityObservation(
const Eigen::Vector3d& imu_angular_velocity) {
imu_angular_velocity_ = imu_angular_velocity;
}
直接就是IMU的角速度imu_angular_velocity就是最新的角速度，直接更新一下就好了。
那我们再分析AdvanceImuTracker这个函数的整体逻辑吧。再次回顾，形参里面的imu_tracker是什么呀？是extrapolator->imu_trackerpose_extrapolator.cc
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker->orientation();
}
再往回推ExtrapolateRotation这个函数。找到函数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) {
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
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;
}回溯完毕，我们继续掉回头去讲AdvanceImuTracker。如果不用IMU数据imu_data_.empty() 那么继续执行imu_tracker->Advance(time);。具体干了什么呢，就是更新了一下imu_tracker 记录的朝向。
void ImuTracker::Advance(const common::Time time) {
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized();
gravity_vector_ = rotation.conjugate() * gravity_vector_;
time_ = time;
}首先求出，在这个基础上乘以角速度，算出旋转，更新朝向，同时更新重力向量。用于后面加速度数据来了，用到重力向量
gravity_vector_上，再进一步更新朝向角。因为我们假设没有IMU数据，所以角速度是0，也没有旋转，所以重力向量是没有更新的。
接下来执行的代码是imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ()); 基于没有IMU数据，所以先给一个假数据Eigen::Vector3d::UnitZ()，也就是没有做矫正。接下来添加IMU的角速度观测。
imu_tracker->AddImuAngularVelocityObservation(
odometry_data_.size() < 2 ? angular_velocity_from_poses_
: angular_velocity_from_odometry_);
没有角速度
odometry_data_.size() < 2 ? angular_velocity_from_poses_
: angular_velocity_from_odometry_
如果里程计的数据很少，那么就从之前存储的位姿中计算出一个角速度，如果里程计速度够多，那么就从里程计中算出一个角速度。所以我们的IMU数据并不一定是0 ，有可能是历史的位姿上算出来的速度，有可能是里程计测算出来的速度。
imu_tracker.cc
void ImuTracker::Advance(const common::Time time) {
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized();
gravity_vector_ = rotation.conjugate() * gravity_vector_;
time_ = time;
}所以ImuTracker::Advance 里面的imu_angular_velocity_ 并不一定是零，因此，其他的变量也并不是不会改变的，当没有IMU数据的时候。rotation 并不是0，orientation_也是一直更新的。
以上是假设没有IMU数据的情况下。其实就是用历史位姿或者里程计更新朝向角和重力向量。
AdvanceImuTracker 重点就是更新形参imu_tracker的角度。
pose_extrapolator.cc
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker->orientation();
}
AdvanceImuTracker执行完之后，再看下一行
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker->orientation();
我们要算 就是拿
imu_tracker_的朝向角与imu_tracker作差就得到了。得到的这个旋转
ExtrapolateRotation(time, extrapolation_imu_tracker_.get())，再乘到最新的位姿newest_timed_pose.pose.rotation()上去。得到了最新的外推的位姿
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) {
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
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;
}其中，我们再求IMU角速度的时候，当没有IMU角速度，我们可以从历史位姿的角速度angular_velocity_from_poses_或者里程计的角速度angular_velocity_from_odometry_中推算出来，那么这两个速度是什么时候计算的呢？
首先看历史位姿角速度是在哪里更新的。
carto_ws/src/cartographer/cartographer/mapping/pose_extrapolator.h
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
......
angular_velocity_from_poses_ =
transform::RotationQuaternionToAngleAxisVector(
oldest_pose.rotation().inverse() * newest_pose.rotation()) /
queue_delta;
}就是从历史存储的位姿计算速度。
下面按行分析
if (timed_pose_queue_.size() < 2) {
// We need two poses to estimate velocities.
return;
}首先就是数据太少，就返回。再看下面就是一些变量的定义
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
const auto newest_time = newest_timed_pose.time;
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);
其中他也是用最新的时间减去最老的时间来算queue_delta。
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
拿出最新的位姿
const auto newest_time = newest_timed_pose.time;
同时拿出最新的时间
const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
拿出最老的位姿
const auto oldest_time = oldest_timed_pose.time;=
同时拿出最老的位姿。
linear_velocity_from_poses_ =
(newest_pose.translation() - oldest_pose.translation()) / queue_delta;
最新位姿和最老位姿作差，除以这个时间跨度。算出一个线速度
angular_velocity_from_poses_ =
transform::RotationQuaternionToAngleAxisVector(
oldest_pose.rotation().inverse() * newest_pose.rotation()) /
queue_delta;
再算一个角速度。
最后看一下这个函数UpdateVelocitiesFromPoses()是在哪里进行调用更新的。
void PoseExtrapolator::AddPose(const common::Time time,
const transform::Rigid3d& pose) {
......
UpdateVelocitiesFromPoses();
......
}
是在AddPose()中调用的，那么AddPose又在哪里调用的呢
src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
......
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
}
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
extrapolator_->AddPose(time, pose_estimate);
......
}
前端我们经过ScanMatch之后得到一个Posepose_estimate_2d我们再将这个Pose放到外推器里面。刚才不是讲了一个外推器里面的有一个位姿队列，然后就把这个位姿存储到位姿队列中去。
UpdateVelocitiesFromPoses();
然后计算更新后Pose的速度。
前端：我们看完了从pose里面算出来的速度，再看从odom算出来的角速度。
首先再回忆一下前端的整个过程，首先来了一帧激光，就用这一帧激光建立一个子图，然后就是运动，运动到哪个位置就用外推器计算出来大概的位置，大概是用里程计去计算大概的位置。计算出来之后，我们拿着当前的激光去跟之前建立的子图做匹配，进一步匹配精细化我跟刚刚匹配的位姿。匹配得到了位姿之后，就将位姿放到外推器里面，同时在这个位姿上将新的激光插入到子图上，这就是我们的前端了
可以看到ScanMatch修正之后得到的Pose
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
然后再将位姿放到外推器里面去，也就是调用AddPose函数。如下代码所示
const transform::Rigid3d pose_estimate =
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
extrapolator_->AddPose(time, pose_estimate);
然后再在AddPose（）中去调用UpdateVelocitiesFromPoses();函数，用Pose队列，去更新下速度。
接下来看用Odom算出来的角速度angular_velocity_from_odometry_是在哪里定义的。是在AddOdometryData中定义的。
void PoseExtrapolator::AddOdometryData(
const sensor::OdometryData& odometry_data) {
......
angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector(
odometry_pose_delta.rotation()) /
odometry_time_delta;
......
}
每次新来了Odom数据odometry_data之后，就用最新的队列去计算一下角速度。这里不仅计算角速度，而且还有ExtrapolateRotation(odometry_data_newest.time, odometry_imu_tracker_.get()); 值得一提的是，我们再通过里程计计算角速度的这个地方遇到了odometry_imu_tracker_，这里我们暂时提一下，后面会进行详细讲解。
接下来看一下AddOdometryData（）是在哪里调用的，
local_trajectory_builder_2d.cc
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);
}
主要就是我们前端在接受了里程计数据odometry_data之后，然后把它送到外推器里面去extrapolator_->AddOdometryData(odometry_data);。然后就是刚才提到的计算角速度。具体代码如下：
pose_extrapolator.cc
void PoseExtrapolator::AddOdometryData(
const sensor::OdometryData& odometry_data) {
CHECK(timed_pose_queue_.empty() ||
odometry_data.time >= timed_pose_queue_.back().time);
odometry_data_.push_back(odometry_data);
TrimOdometryData();
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.
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);
const transform::Rigid3d odometry_pose_delta =
odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;
angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector(
odometry_pose_delta.rotation()) /
odometry_time_delta;
if (timed_pose_queue_.empty()) {
return;
}
const Eigen::Vector3d
linear_velocity_in_tracking_frame_at_newest_odometry_time =
odometry_pose_delta.translation() / odometry_time_delta;
const Eigen::Quaterniond orientation_at_newest_odometry_time =
timed_pose_queue_.back().pose.rotation() *
ExtrapolateRotation(odometry_data_newest.time,
odometry_imu_tracker_.get());
linear_velocity_from_odometry_ =
orientation_at_newest_odometry_time *
linear_velocity_in_tracking_frame_at_newest_odometry_time;
}
下面进行逐行解析，首先是接收到一帧里程计数据odometry_data之后，首先是进行一个检查
src/cartographer/cartographer/mapping/pose_extrapolator.cc
CHECK(timed_pose_queue_.empty() ||
odometry_data.time >= timed_pose_queue_.back().time);
检查如果这个pose队列是空的，或者当前的里程计的时间大于pose队列最老的时间，满足这些条件我们才能进行下去。因为我们是往前推数据，而不是推已经存在的数据。然后有一个TrimOdometryData函数，会把time之前的odometry_data数据都给删掉。
void PoseExtrapolator::TrimOdometryData() {
while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
odometry_data_[1].time <= timed_pose_queue_.back().time) {
odometry_data_.pop_front();
}
}在保证里程计数据大于2的情况下，且位姿队列不为空的情况下，也就是说，直到位姿队列中只剩下一个位姿的时候，循环才会停止。并且，如果队列就剩下两个数据，timed_pose_queue_.back().time的时间就等于odometry_data_[1].time的时间，满足条件，就一直往外抛出数据。因为之前的数据都没有用了，我们要使用的是pose之前的数据，之后的没有用了，只保留2个就可以了。
然后继续回溯到之前的代码的位置。
src/cartographer/cartographer/mapping/pose_extrapolator.cc
if (odometry_data_.size() < 2) {
return;
}如果数据不够，就继续返回。
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);
const transform::Rigid3d odometry_pose_delta =
odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;
angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector(
odometry_pose_delta.rotation()) /
odometry_time_delta;
接下来是老套路了，都是拿到最老和最新的odom数据，然后再计算得到，根据最新和最老的位姿算出
。然后再求出角速度。
const Eigen::Vector3d
linear_velocity_in_tracking_frame_at_newest_odometry_time =
odometry_pose_delta.translation() / odometry_time_delta;
然后还有一串长名字linear_velocity_in_tracking_frame_at_newest_odometry_time。最新的里程计时间点上的tracking_frame上的线速度。为什么起这么个名字，我们还是要讲一下。
linear_velocity_from_odometry_ =
orientation_at_newest_odometry_time *
linear_velocity_in_tracking_frame_at_newest_odometry_time;
上面代码主要是让速度从机体坐标转化成local坐标系。
const Eigen::Quaterniond orientation_at_newest_odometry_time =
timed_pose_queue_.back().pose.rotation() *
ExtrapolateRotation(odometry_data_newest.time,
odometry_imu_tracker_.get());
上面代码算的是刚刚使用到的朝向，朝向怎么算的，跟之前的外推器计算的朝向差不多，就是外推器存储的最后的一个pose，timed_pose_queue_.back().pose.rotation(),在这个基础上，乘以一个
ExtrapolateRotation(odometry_data_newest.time, odometry_imu_tracker_.get())。 但是这个是怎么算的呢？他使用了一个
odometry_imu_tracker_。 当然这函数是一样的，只不过参数imu_tracker不一样。
我们为了通过里程计，计算出来线速度，我们需要把线速度转化到local坐标系里面去。计算这个速度的时候，本身就要外推一下，然后使用一个外推器，推出一个朝向角，计算出一个速度。然后再用这个速度去外推一个pose。然后计算里程计的速度去做一个外推，用到了imu_tracker,为什么又搞了一个新的呢？
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker->orientation();
}
可以看到我们的odometry_tracker 也是先通过AdvanceImuTracker 这个来得到一个朝向角。然后再从后缀_的imu_tracker_中得到上一次的朝向角。算出来一个。
我们什么时候要用外推器外推的时间？
比如一帧激光进来了，你要外推当前机器人的pose。要外推一次。
然后每一帧的激光点，都要给一个点外推一个发射原点。
我们的外推器，每来一个IMU数据，就要更新当前时刻的朝向。我们记录我们的，外推器记录外推器的，大家各自一套，分开就可以了。
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const {
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_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;
}
if (imu_tracker->time() < imu_data_.front().time) {
// Advance to the beginning of 'imu_data_'.
imu_tracker->Advance(imu_data_.front().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);
}
为了实时的把里程计的速度转化到local坐标系下，我们就单独用了odometry_imu_tracker。
我们再回顾一下imu_tracker。
src/cartographer/cartographer/mapping/pose_extrapolator.h
std::unique_ptr<ImuTracker> imu_tracker_;
std::unique_ptr<ImuTracker> odometry_imu_tracker_;
std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
extrapolation_imu_tracker_是用来做外推的主力。odometry_imu_tracker_只为了用于把里程计的速度转化为local坐标系里去。imu_tracker_还没讲到。但他时刻记录着过程中，起到的作用，记录最近一次pose的朝向角，我们总是在这个基础上，计算。
我们首先想想AddPose什么时候用的，就是ScanMatch结束之后，我们得到了一个精细的Pose，然后将这个Pose传入到AddPose函数中，放入到外推器里面的位姿队列中。
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_);
}
odometry_imu_tracker_的作用是只要来一帧里程计数据，就要更新里程计的速度，但是速度要转化到local坐标系里面去，怎么转化呢？就是算，再添加到最新的
pose上面去。
为什么两个位姿可以相减得到KaTeX parse error: Undefined control sequence: \Deltata at position 1: \̲D̲e̲l̲t̲a̲t̲a̲_{rotation}，因为每一次数据来之后，我们得到一个很精细的pose。完了我们把imu_tracker_记录的角度给统一到odometry_imu_tracker_，extrapolation_imu_tracker_中去。那么下次我们再计算出角度之后，就去减掉imu_tracker_。就相当于减掉自己的上一时刻的位姿。imu_tracker_记录的是每一次我们ScanMatch结束之后，我们记录的精细的角度。他始终记录的是结果。
odometry_imu_tracker_，extrapolation_imu_tracker_是两个工具人，始终在imu_tracker_的基础上去计算，然后再把
放置在存储pose队列里面最后的一个
pose上面去，得到所需要时刻的pose。