Cartographer源码阅读(7)：轨迹推算和位姿推算的原理

其实也就是包括两个方面的内容：类似于运动模型的位姿估计和扫描匹配，因为需要计算速度，所以时间就有必要了！

1. PoseExtrapolator解决了IMU数据、里程计和位姿信息进行融合的问题。

该类定义了三个队列。

 std::deque<TimedPose> timed_pose_queue_;
 std::deque<sensor::ImuData> imu_data_;
 std::deque<sensor::OdometryData> odometry_data_;

定义了(a)通过位姿计算线速度和角速度对象

  Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();

和(b)通过里程计计算线速度和角速度对象

 Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
 Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();

轮询处理三类消息 IMU消息、里程计消息，激光测距消息。有如下情况：

1）不使用IMU和里程计

　　只执行AddPose，注意12-15行的代码，$ time{\rm{ }} - timed\_pose\_queue\_\left[ 1 \right].time \ge pose\_queue\_duration\_ $，队列中最前面的数据的时间，当距离当前时间超过一定间隔时执行，作用是将较早时间的数据剔除。接着，根据位姿计算运动速度。对齐IMU数据，里程计数据。

 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_ =
         common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
   }
   timed_pose_queue_.push_back(TimedPose{time, pose});
   while (timed_pose_queue_.size() >  &&
          timed_pose_queue_[].time <= time - pose_queue_duration_) {
     timed_pose_queue_.pop_front();
   }
   UpdateVelocitiesFromPoses();
   AdvanceImuTracker(time, imu_tracker_.get());
   TrimImuData();
   TrimOdometryData();
   odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
   extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
 }

第16行，更新了根据Pose计算的线速度和角速度。

 void PoseExtrapolator::UpdateVelocitiesFromPoses()
 {
   if (timed_pose_queue_.size() < )
   {
     // We need two poses to estimate velocities.
     return;
   }
   CHECK(!timed_pose_queue_.empty());
   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);
   if (queue_delta < 0.001) {  // 1 ms
     LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
                  << queue_delta << " ms";
     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;
 }

PoseExtrapolator::UpdateVelocitiesFromPoses()

17行执行了PoseExtrapolator::AdvanceImuTracker方法，当不使用IMU数据时，将 angular_velocity_from_poses_ 或者 angular_velocity_from_odometry_ 数据传入了imu_tracker.

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

在执行ExtrapolatePose()，推测某一时刻的位姿的时候，调用了 ExtrapolateTranslation 和 ExtrapolateRotation 方法。

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

可以看到使用的是：（1）旋转，imu_tracker的方位角角的变化量；（2）平移，里程计或者位姿线速度计算的移动量。

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

 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() < ) {
     return extrapolation_delta * linear_velocity_from_poses_;
   }
   return extrapolation_delta * linear_velocity_from_odometry_;
 }

2）使用IMU和里程计

　　IMU频率最高，假设消息进入的先后顺序是IMU、里程计，最后是激光消息。

2. RealTimeCorrelativeScanMatcher解决了Scan和子图的扫描匹配的问题。

通过 real_time_correlative_scan_matcher_ 和 ceres_scan_matcher_ 实现的。

 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
     const common::Time time, const transform::Rigid2d& pose_prediction,
     const sensor::RangeData& gravity_aligned_range_data)
 {
   std::shared_ptr<const Submap> matching_submap =
       active_submaps_.submaps().front();
   // The online correlative scan matcher will refine the initial estimate for
   // the Ceres scan matcher.
   transform::Rigid2d initial_ceres_pose = pose_prediction;
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.adaptive_voxel_filter_options());
   const sensor::PointCloud filtered_gravity_aligned_point_cloud =
       adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
   if (filtered_gravity_aligned_point_cloud.empty())
   {
     return nullptr;
   }
   if (options_.use_online_correlative_scan_matching())
   {
     real_time_correlative_scan_matcher_.Match(
         pose_prediction, filtered_gravity_aligned_point_cloud,
         matching_submap->probability_grid(), &initial_ceres_pose);
   }

   auto pose_observation = common::make_unique<transform::Rigid2d>();
   ceres::Solver::Summary summary;
   ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                             filtered_gravity_aligned_point_cloud,
                             matching_submap->probability_grid(),
                             pose_observation.get(), &summary);
   return pose_observation;
 }