其實也就是包括兩個方面的內容：類似於運動模型的位姿估計和掃描匹配，因為需要計算速度，所以時間就有必要了！

1. PoseExtrapolator解決了IMU資料、里程計和位姿資訊進行融合的問題。

該類定義了三個佇列。

1 std::deque<TimedPose> timed_pose_queue_;
2 std::deque<sensor::ImuData> imu_data_;
3 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)通過里程計計算線速度和角速度物件

1 Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
2 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資料，里程計資料。

 1 void PoseExtrapolator::AddPose(const common::Time time,
 2                                const transform::Rigid3d& pose) {
 3   if (imu_tracker_ == nullptr) {
 4     common::Time tracker_start = time;
 5     if (!imu_data_.empty()) {
 6       tracker_start = std::min(tracker_start, imu_data_.front().time);
 
 7     }
 8     imu_tracker_ =
 9         common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
10   }
11   timed_pose_queue_.push_back(TimedPose{time, pose});
12   while (timed_pose_queue_.size() > 2 &&
13          timed_pose_queue_[1].time <= time - pose_queue_duration_) {
14     timed_pose_queue_.pop_front();
15   }
16   UpdateVelocitiesFromPoses();
17   AdvanceImuTracker(time, imu_tracker_.get());
18   TrimImuData();
19   TrimOdometryData();
20   odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
21   extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
22 }

 第16行，更新了根據Pose計算的線速度和角速度。

 1 void PoseExtrapolator::UpdateVelocitiesFromPoses() 
 2 {
 3   if (timed_pose_queue_.size() < 2)
 4   {
 5     // We need two poses to estimate velocities.
 6     return;
 7   }
 8   CHECK(!timed_pose_queue_.empty());
 9   const TimedPose& newest_timed_pose = timed_pose_queue_.back();
10   const auto newest_time = newest_timed_pose.time;
11   const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
12   const auto oldest_time = oldest_timed_pose.time;
13   const double queue_delta = common::ToSeconds(newest_time - oldest_time);
14   if (queue_delta < 0.001) {  // 1 ms
15     LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
16                  << queue_delta << " ms";
17     return;
18   }
19   const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
20   const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
21   linear_velocity_from_poses_ =
22       (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
23   angular_velocity_from_poses_ =
24       transform::RotationQuaternionToAngleAxisVector(
25           oldest_pose.rotation().inverse() * newest_pose.rotation()) /
26       queue_delta;
27 }

17行執行了PoseExtrapolator::AdvanceImuTracker方法，當不使用IMU資料時，將 angular_velocity_from_poses_ 或者 angular_velocity_from_odometry_ 資料傳入了imu_tracker.

 1 void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
 2                                          ImuTracker* const imu_tracker) const 
 3 {
 4   CHECK_GE(time, imu_tracker->time());
 5   if (imu_data_.empty() || time < imu_data_.front().time) 
 6   {
 7     // There is no IMU data until 'time', so we advance the ImuTracker and use
 8     // the angular velocities from poses and fake gravity to help 2D stability.
 9     imu_tracker->Advance(time);
10     imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
11     imu_tracker->AddImuAngularVelocityObservation(
12         odometry_data_.size() < 2 ? angular_velocity_from_poses_
13                                   : angular_velocity_from_odometry_);
14     return;
15   }
16   if (imu_tracker->time() < imu_data_.front().time) {
17     // Advance to the beginning of 'imu_data_'.
18     imu_tracker->Advance(imu_data_.front().time);
19   }
20   auto it = std::lower_bound(
21       imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
22       [](const sensor::ImuData& imu_data, const common::Time& time) {
23         return imu_data.time < time;
24       });
25   while (it != imu_data_.end() && it->time < time) {
26     imu_tracker->Advance(it->time);
27     imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
28     imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
29     ++it;
30   }
31   imu_tracker->Advance(time);
32 }

 在執行ExtrapolatePose()，推測某一時刻的位姿的時候，呼叫了 ExtrapolateTranslation 和 ExtrapolateRotation 方法。

 1 transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
 2   const TimedPose& newest_timed_pose = timed_pose_queue_.back();
 3   CHECK_GE(time, newest_timed_pose.time);
 4   if (cached_extrapolated_pose_.time != time) {
 5     const Eigen::Vector3d translation =
 6         ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
 7     const Eigen::Quaterniond rotation =
 8         newest_timed_pose.pose.rotation() *
 9         ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
10     cached_extrapolated_pose_ =
11         TimedPose{time, transform::Rigid3d{translation, rotation}};
12   }
13   return cached_extrapolated_pose_.pose;
14 }

可以看到使用的是：（1）旋轉，imu_tracker的方位角角的變化量；（2）平移，里程計或者位姿線速度計算的移動量。

 1 Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
 2     const common::Time time, ImuTracker* const imu_tracker) const {
 3   CHECK_GE(time, imu_tracker->time());
 4   AdvanceImuTracker(time, imu_tracker);
 5   const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
 6   return last_orientation.inverse() * imu_tracker->orientation();
 7 }
 8 
 9 Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
10   const TimedPose& newest_timed_pose = timed_pose_queue_.back();
11   const double extrapolation_delta =
12       common::ToSeconds(time - newest_timed_pose.time);
13   if (odometry_data_.size() < 2) {
14     return extrapolation_delta * linear_velocity_from_poses_;
15   }
16   return extrapolation_delta * linear_velocity_from_odometry_;
17 }

 2）使用IMU和里程計

　　IMU頻率最高，假設訊息進入的先後順序是IMU、里程計，最後是鐳射訊息。

2. RealTimeCorrelativeScanMatcher解決了Scan和子圖的掃描匹配的問題。

通過 real_time_correlative_scan_matcher_ 和 ceres_scan_matcher_ 實現的。

 1 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
 2     const common::Time time, const transform::Rigid2d& pose_prediction,
 3     const sensor::RangeData& gravity_aligned_range_data) 
 4 {
 5   std::shared_ptr<const Submap> matching_submap =
 6       active_submaps_.submaps().front();
 7   // The online correlative scan matcher will refine the initial estimate for
 8   // the Ceres scan matcher.
 9   transform::Rigid2d initial_ceres_pose = pose_prediction;
10   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
11       options_.adaptive_voxel_filter_options());
12   const sensor::PointCloud filtered_gravity_aligned_point_cloud =
13       adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
14   if (filtered_gravity_aligned_point_cloud.empty())
15   {
16     return nullptr;
17   }
18   if (options_.use_online_correlative_scan_matching()) 
19   {
20     real_time_correlative_scan_matcher_.Match(
21         pose_prediction, filtered_gravity_aligned_point_cloud,
22         matching_submap->probability_grid(), &initial_ceres_pose);
23   }
24 
25   auto pose_observation = common::make_unique<transform::Rigid2d>();
26   ceres::Solver::Summary summary;
27   ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
28                             filtered_gravity_aligned_point_cloud,
29                             matching_submap->probability_grid(),
30                             pose_observation.get(), &summary);
31   return pose_observation;
32 }