Cartographer源码阅读(6)：LocalTrajectoryBuilder和PoseExtrapolator

LocalTrajectoryBuilder意思是局部轨迹的构建，下面的类图中方法的参数没有画进去。

注意其中的三个类：PoseExtrapolator类，RealTimeCorrelativeScanMatcher类和CeresScanMatcher类。

（1）PoseExtrapolator类（如下图），Node类和LocalTrajectoryBuilder类都有用到PoseExtrapolator对象，好像两者之间并没有什么关系？

LocalTrajectoryBuilder中的PoseExtrapolator对象类似于运动模型。

（Node类中的可能是为了发布位姿信息用的，单独进行了位姿推算。先不管了。）

PoseExtrapolator的构造函数 VS 通过IMU初始化InitializeWithImu方法。

在LocalTrajectoryBuilder::InitializeExtrapolator中对其构造函数的调用：

 void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time)
 {
   if (extrapolator_ != nullptr) {
     return;
   }
   // We derive velocities from poses which are at least 1 ms apart for numerical
   // stability. Usually poses known to the extrapolator will be further apart
   // in time and thus the last two are used.
   constexpr double kExtrapolationEstimationTimeSec = 0.001;
   // TODO(gaschler): Consider using InitializeWithImu as 3D does.
   extrapolator_ = common::make_unique<mapping::PoseExtrapolator>(
       ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
       options_.imu_gravity_time_constant());
   extrapolator_->AddPose(time, transform::Rigid3d::Identity());
 }

PoseExtrapolator::InitializeWithImu方法：

 std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
     const common::Duration pose_queue_duration,
     const double imu_gravity_time_constant, const sensor::ImuData& imu_data)
 {
   auto extrapolator = common::make_unique<PoseExtrapolator>(pose_queue_duration, imu_gravity_time_constant);
   extrapolator->AddImuData(imu_data);
   extrapolator->imu_tracker_ =common::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);
   extrapolator->imu_tracker_->AddImuLinearAccelerationObservation(
       imu_data.linear_acceleration);
   extrapolator->imu_tracker_->AddImuAngularVelocityObservation(
       imu_data.angular_velocity);
   extrapolator->imu_tracker_->Advance(imu_data.time);
   extrapolator->AddPose(imu_data.time,transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation()));
   return extrapolator;
 }

LocalTrajectoryBuilder的AddImuData和AddOdometryData方法不赘述。

 void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
   CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
   InitializeExtrapolator(imu_data.time);
   extrapolator_->AddImuData(imu_data);
 }

 void LocalTrajectoryBuilder::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);
 }

如下查看LocalTrajectoryBuilder::AddRangeData方法。如果使用IMU数据，直接进入10行，如果不是用进入7行。

 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddRangeData(const common::Time time,
                                      const sensor::TimedRangeData& range_data)
 {
   // Initialize extrapolator now if we do not ever use an IMU.
   if (!options_.use_imu_data())
   {
     InitializeExtrapolator(time);
   }
   if (extrapolator_ == nullptr)
   {
     // Until we've initialized the extrapolator with our first IMU message, we
     // cannot compute the orientation of the rangefinder.
     LOG(INFO) << "Extrapolator not yet initialized.";
     return nullptr;
   }

   CHECK(!range_data.returns.empty());
   CHECK_EQ(range_data.returns.back()[], );
   const common::Time time_first_point =
       time + common::FromSeconds(range_data.returns.front()[]);
   if (time_first_point < extrapolator_->GetLastPoseTime()) {
     LOG(INFO) << "Extrapolator is still initializing.";
     return nullptr;
   }

   std::vector<transform::Rigid3f> range_data_poses;
   range_data_poses.reserve(range_data.returns.size());
   for (const Eigen::Vector4f& hit : range_data.returns) {
     const common::Time time_point = time + common::FromSeconds(hit[]);
     range_data_poses.push_back(
         extrapolator_->ExtrapolatePose(time_point).cast<float>());
   }

   if (num_accumulated_ == ) {
     // 'accumulated_range_data_.origin' is uninitialized until the last
     // accumulation.
     accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
   }

   // Drop any returns below the minimum range and convert returns beyond the
   // maximum range into misses.
   for (size_t i = ; i < range_data.returns.size(); ++i) {
     const Eigen::Vector4f& hit = range_data.returns[i];
     const Eigen::Vector3f origin_in_local =
         range_data_poses[i] * range_data.origin;
     const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<>();
     const Eigen::Vector3f delta = hit_in_local - origin_in_local;
     const float range = delta.norm();
     if (range >= options_.min_range()) {
       if (range <= options_.max_range()) {
         accumulated_range_data_.returns.push_back(hit_in_local);
       } else {
         accumulated_range_data_.misses.push_back(
             origin_in_local +
             options_.missing_data_ray_length() / range * delta);
       }
     }
   }
   ++num_accumulated_;

   if (num_accumulated_ >= options_.num_accumulated_range_data()) {
     num_accumulated_ = ;
     const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
         extrapolator_->EstimateGravityOrientation(time));
     accumulated_range_data_.origin =
         range_data_poses.back() * range_data.origin;
     return AddAccumulatedRangeData(
         time,
         TransformToGravityAlignedFrameAndFilter(
             gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
             accumulated_range_data_),
         gravity_alignment);
   }
   return nullptr;
 }

接着，LocalTrajectoryBuilder::AddAccumulatedRangeData代码如下，传入的参数为3个。

const common::Time time，const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& gravity_alignment

重力定向，定向后的深度数据和定向矩阵。

注意下面21行代码执行了扫描匹配的ScanMatch方法，之后代码29行调用的extrapolator_->AddPose()方法：

每次扫描匹配之后执行AddPose方法。

 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time,
     const sensor::RangeData& gravity_aligned_range_data,
     const transform::Rigid3d& gravity_alignment)
 {
   if (gravity_aligned_range_data.returns.empty())
   {
     LOG(WARNING) << "Dropped empty horizontal range data.";
     return nullptr;
   }

   // Computes a gravity aligned pose prediction.
   const transform::Rigid3d non_gravity_aligned_pose_prediction =
       extrapolator_->ExtrapolatePose(time);
   const transform::Rigid2d pose_prediction = transform::Project2D(
       non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

   // local map frame <- gravity-aligned frame
   std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
       ScanMatch(time, pose_prediction, gravity_aligned_range_data);
   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);

   sensor::RangeData range_data_in_local =
       TransformRangeData(gravity_aligned_range_data,
                          transform::Embed3D(pose_estimate_2d->cast<float>()));
   std::unique_ptr<InsertionResult> insertion_result =
       InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
                        pose_estimate, gravity_alignment.rotation());
   return common::make_unique<MatchingResult>(
       MatchingResult{time, pose_estimate, std::move(range_data_in_local),
                      std::move(insertion_result)});
 }

 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::InsertIntoSubmap(
     const common::Time time, const sensor::RangeData& range_data_in_local,
     const sensor::RangeData& gravity_aligned_range_data,
     const transform::Rigid3d& pose_estimate,
     const Eigen::Quaterniond& gravity_alignment)
 {
   if (motion_filter_.IsSimilar(time, pose_estimate))
   {
     return nullptr;
   }

   // Querying the active submaps must be done here before calling
   // InsertRangeData() since the queried values are valid for next insertion.
   std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps())
   {
     insertion_submaps.push_back(submap);
   }
   active_submaps_.InsertRangeData(range_data_in_local);

   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.loop_closure_adaptive_voxel_filter_options());
   const sensor::PointCloud filtered_gravity_aligned_point_cloud =
       adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);

   return common::make_unique<InsertionResult>(InsertionResult{
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::TrajectoryNode::Data{
               time,
               gravity_alignment,
               filtered_gravity_aligned_point_cloud,
               {},  // 'high_resolution_point_cloud' is only used in 3D.
               {},  // 'low_resolution_point_cloud' is only used in 3D.
               {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
               pose_estimate}),
       std::move(insertion_submaps)});
 }

LocalTrajectoryBuilder::AddAccumulatedRangeData

（2）RealTimeCorrelativeScanMatcher类，实时的扫描匹配，用的相关分析方法。