Cartorgrapher源码阅读04- PoseExtrapolator-前端匹配-位姿外插器 SLAM

接下来对前端匹配中的位姿外插器PoseExtrapolator进行解读，位姿外插器的主要作用是利用保存了一段时间的位姿计算角速度和线速度，并利用线速度和角速度进行位姿插值。也是前端匹配这一部分非常重要的一部分代码。
代码位于Cartographer/mapping/ pose_extrapolator.cpp
1.pose_extrapolator.h 从该类开头的注释我们可以知道，利用ScanMatch持续一段时间输出的PoseObservation，来估计角速度和线速度，并利用求得的速度来求解机器人的位姿。当有IMU和里程计的数据时，可将这些数据进行融合。
这个类的成员函数也正是三个数据的加入，以及外插的实现，另外还有重力方向的函数,此函数的作用仍然是实现平面投影。

//加入位姿数据. void AddPose(common::Time time, const transform::Rigid3d& pose); //加入IMU数据 void AddImuData(const sensor::ImuData& imu_data); //加入里程计数据. void AddOdometryData(const sensor::OdometryData& odometry_data); //得到time时刻的位姿--进行外插. transform::Rigid3d ExtrapolatePose(common::Time time); // Returns the current gravity alignment estimate as a rotation from // the tracking frame into a gravity aligned frame. // 得到重力向量的方向.有及速度数据，就可以求重力方向 Eigen::Quaterniond EstimateGravityOrientation(common::Time time);

成员变量有:

const common::Duration pose_queue_duration_; //标记持续时间的长短,微秒,0.1us// 从持续跟踪一段时间的位姿队列中估计出来的线速度，初始化为0 Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero(); // 从持续跟踪一段时间的位姿队列中估计出来的角速度，初始化为0 Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero()const double gravity_time_constant_; //重力的时间间隔std::deque::ImuData> imu_data_; //存储IMU数据的队列std::unique_ptr imu_tracker_; // imu_tracker_ 是存放由IMU数据得到的信息std::unique_ptr odometry_imu_tracker_; //存放由里程计得到的信息std::unique_ptr extrapolation_imu_tracker_; //是存放经过数据融合后的结果std::deque::OdometryData> odometry_data_; //里程计数据// 从里程计获取到的线速度，初始化为0 Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); // 从里程计获取到的角速度，初始化为0 Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();

从代码里可以注意到ImuTracker，这个接下来我们也会讲到，也是一个重要的类。接下来我们对extrapolator中的重要的成员函数进行解读。
2.AddPose 这部分的代码就是利用扫描匹配得到的一段位姿进行计算角速度和线速度，然后用速度进行插值。这段代码是核心。

//加入位姿. void PoseExtrapolator::AddPose(const common::Time time, const transform::Rigid3d& pose) { 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_ = absl::make_unique(gravity_time_constant_, tracker_start); }//把位姿和时间压入队列 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(); }//从位姿中估计速度. UpdateVelocitiesFromPoses(); //更新imu-tracker信息. AdvanceImuTracker(time, imu_tracker_.get()); //去除多余数据. TrimImuData(); TrimOdometryData(); //里程计和融合结果都以当前IMU的tracker为准。odometry_imu_tracker_ = absl::make_unique(*imu_tracker_); extrapolation_imu_tracker_ = absl::make_unique(*imu_tracker_); }

其中调用了更新速度UpdateVelocitiesFromPoses() 和更新imu数据的AdvanceImuTracker的两个函数
1.1更新速度：UpdateVelocitiesFromPoses
还是得注意，这里的更新速度是指得从位姿估计速度。
这部分的代码简单来说就是

Cartorgrapher源码阅读04- PoseExtrapolator-前端匹配-位姿外插器

文章图片

//用位姿队列中的数据来进行线速度和角速度的计算. 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); //如果时间差小于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; //估计线速度（速度之差除以时间） 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; }

1.2更新imu-tracker：AdvanceImuTracker
对imu-tracker进行更新,用imu队列中的数据进行更新,更新到time时刻，因为pose已经到了time时刻，因此imu数据也要更新得到这个时刻，老的数据就被删掉。
imutracker实际是用来求角速度和重力向量，因为imu对角速度的测量更加精确

void PoseExtrapolator::AdvanceImuTracker(const common::Time time, ImuTracker* const imu_tracker) const { 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.imu_tracker->Advance(time); //根据时间进行imu数据插值//给定一个理论上的重力向量 imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ()); //优先选择里程计得出的角速度给imu //当里程计的数据小于2个时，选择从pose；否则，选择里程计 //因为里程计的精度更高 imu_tracker->AddImuAngularVelocityObservation( odometry_data_.size() < 2 ? angular_velocity_from_poses_ : angular_velocity_from_odometry_); return; }//如果有imu数据时，此处调用了imu数据队列 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; }); //用imu数据队列进行imu-tracker的数据更新. //imu_tracker主要负责对角速度进行插值，并且估计重力向量 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); }

在后面，也会对imu_tracker进行分析
1.3修剪多余数据

// 删去队列中无用的IMU数据 void PoseExtrapolator::TrimImuData() { // 需要满足三个条件：IMU数据队列大于1，Pose的队列不为空， //IMU数据队列的第一个元素时间小于Pose队列的最后一个元素的时间 // 最后一个条件意味着当IMU数据的时间比一个最新的Pose的时间要早时， //说明这个IMU数据已经过期了。所以从队列中删掉就可以了。 // 知道IMU数据的时间要比最新的Pose时间晚，那么说明这时候这个数据还有用。 // 这种情况就不再删了，跳出循环，等待其他程序取出队列最开头的IMU数据进行融合 while (imu_data_.size() > 1 && !timed_pose_queue_.empty() && imu_data_[1].time <= timed_pose_queue_.back().time) { imu_data_.pop_front(); }

2.增加imu数据直接将imu数据压入imu数据队列，被imutracker调用

//加入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); //直接压入imu数据队列，被imutracker调用 TrimImuData(); }

3.增加里程计数据同样将里程计数据直接插入队列。

//加入里程计数据. 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; }

然后跟1.1由pose求角速度及线速度一样的道理。

// 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; }

4.对位姿进行插值ExtrapolatePose 对位移的插值分为了两步：

对位移，角度进行插值，得到的是增量
对位姿进行插值

4.1插值得到旋转增量

//用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(); }

4.2插值得到平移增量

//用线速度进行插值.--得到的是增量. 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 lation_delta * linear_velocity_from_odometry_;

4.3位姿插值

//对位姿进行插值,得到time时刻的位姿. transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) { //位姿队列中最新的队列. const TimedPose& newest_timed_pose = timed_pose_queue_.back(); //必须要比newest_timed_pose的时间大. CHECK_GE(time, newest_timed_pose.time); //如果缓存的Pose的时间不等于指定时间 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}}; } //返回插值之后的pose return cached_extrapolated_pose_.pose; }

5.计算重力向量计算重力向量,用imu-tracker来进行计算.

Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation( const common::Time time) { ImuTracker imu_tracker = *imu_tracker_; AdvanceImuTracker(time, &imu_tracker); return imu_tracker.orientation(); }

小节 【Cartorgrapher源码阅读04- PoseExtrapolator-前端匹配-位姿外插器】PoseExtrapolator的作用就是利用一段时间的位姿对角速度和线速度进行求解，并利用求解的速度对位姿进行插值，求解当前机器人的pose。
其中调用了比较重要的函数ImuTracker，因此接下来对ImuTracker进行解读。