Cartographer源码阅读：LocalTrajectoryBuilder和PoseExtrapolator

Posted 2020-10-25 岁寒然后知松柏之后凋也！

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

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

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

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

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

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

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

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

PoseExtrapolator::InitializeWithImu方法：

 1 std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
 2     const common::Duration pose_queue_duration,
 3     const double imu_gravity_time_constant, const sensor::ImuData& imu_data)
 4 {
 5   auto extrapolator = common::make_unique<PoseExtrapolator>(pose_queue_duration, imu_gravity_time_constant);
 6   extrapolator->AddImuData(imu_data);
 7   extrapolator->imu_tracker_ =common::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);
 8   extrapolator->imu_tracker_->AddImuLinearAccelerationObservation(
 9       imu_data.linear_acceleration);
10   extrapolator->imu_tracker_->AddImuAngularVelocityObservation(
11       imu_data.angular_velocity);
12   extrapolator->imu_tracker_->Advance(imu_data.time);
13   extrapolator->AddPose(imu_data.time,transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation()));
14   return extrapolator;
15 }

 LocalTrajectoryBuilder的AddImuData和AddOdometryData方法不赘述。

 1 void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
 2   CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
 3   InitializeExtrapolator(imu_data.time);
 4   extrapolator_->AddImuData(imu_data);
 5 }
 6 
 7 void LocalTrajectoryBuilder::AddOdometryData(
 8     const sensor::OdometryData& odometry_data) {
 9   if (extrapolator_ == nullptr) {
10     // Until we\'ve initialized the extrapolator we cannot add odometry data.
11     LOG(INFO) << "Extrapolator not yet initialized.";
12     return;
13   }
14   extrapolator_->AddOdometryData(odometry_data);
15 }

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

 1 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 2 LocalTrajectoryBuilder::AddRangeData(const common::Time time,
 3                                      const sensor::TimedRangeData& range_data) 
 4 {
 5   // Initialize extrapolator now if we do not ever use an IMU.
 6   if (!options_.use_imu_data()) 
 7   {
 8     InitializeExtrapolator(time);
 9   }
10   if (extrapolator_ == nullptr) 
11   {
12     // Until we\'ve initialized the extrapolator with our first IMU message, we
13     // cannot compute the orientation of the rangefinder.
14     LOG(INFO) << "Extrapolator not yet initialized.";
15     return nullptr;
16   }
17 
18   CHECK(!range_data.returns.empty());
19   CHECK_EQ(range_data.returns.back()[3], 0);
20   const common::Time time_first_point =
21       time + common::FromSeconds(range_data.returns.front()[3]);
22   if (time_first_point < extrapolator_->GetLastPoseTime()) {
23     LOG(INFO) << "Extrapolator is still initializing.";
24     return nullptr;
25   }
26 
27   std::vector<transform::Rigid3f> range_data_poses;
28   range_data_poses.reserve(range_data.returns.size());
29   for (const Eigen::Vector4f& hit : range_data.returns) {
30     const common::Time time_point = time + common::FromSeconds(hit[3]);
31     range_data_poses.push_back(
32         extrapolator_->ExtrapolatePose(time_point).cast<float>());
33   }
34 
35   if (num_accumulated_ == 0) {
36     // \'accumulated_range_data_.origin\' is uninitialized until the last
37     // accumulation.
38     accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
39   }
40 
41   // Drop any returns below the minimum range and convert returns beyond the
42   // maximum range into misses.
43   for (size_t i = 0; i < range_data.returns.size(); ++i) {
44     const Eigen::Vector4f& hit = range_data.returns[i];
45     const Eigen::Vector3f origin_in_local =
46         range_data_poses[i] * range_data.origin;
47     const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
48     const Eigen::Vector3f delta = hit_in_local - origin_in_local;
49     const float range = delta.norm();
50     if (range >= options_.min_range()) {
51       if (range <= options_.max_range()) {
52         accumulated_range_data_.returns.push_back(hit_in_local);
53       } else {
54         accumulated_range_data_.misses.push_back(
55             origin_in_local +
56             options_.missing_data_ray_length() / range * delta);
57       }
58     }
59   }
60   ++num_accumulated_;
61 
62   if (num_accumulated_ >= options_.num_accumulated_range_data()) {
63     num_accumulated_ = 0;
64     const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
65         extrapolator_->EstimateGravityOrientation(time));
66     accumulated_range_data_.origin =
67         range_data_poses.back() * range_data.origin;
68     return AddAccumulatedRangeData(
69         time,
70         TransformToGravityAlignedFrameAndFilter(
71             gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
72             accumulated_range_data_),
73         gravity_alignment);
74   }
75   return nullptr;
76 }

 接着，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方法。

 1 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
 2 LocalTrajectoryBuilder::AddAccumulatedRangeData(
 3     const common::Time time,
 4     const sensor::RangeData& gravity_aligned_range_data,
 5     const transform::Rigid3d& gravity_alignment) 
 6 {
 7   if (gravity_aligned_range_data.returns.empty())
 8   {
 9     LOG(WARNING) << "Dropped empty horizontal range data.";
10     return nullptr;
11   }
12 
13   // Computes a gravity aligned pose prediction.
14   const transform::Rigid3d non_gravity_aligned_pose_prediction =
15       extrapolator_->ExtrapolatePose(time);
16   const transform::Rigid2d pose_prediction = transform::Project2D(
17       non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
18 
19   // local map frame <- gravity-aligned frame
20   std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
21       ScanMatch(time, pose_prediction, gravity_aligned_range_data);
22   if (pose_estimate_2d == nullptr) 
23   {
24     LOG(WARNING) << "Scan matching failed.";
25     return nullptr;
26   }
27   const transform::Rigid3d pose_estimate =
28       transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
29   extrapolator_->AddPose(time, pose_estimate);
30 
31   sensor::RangeData range_data_in_local =
32       TransformRangeData(gravity_aligned_range_data,
33                          transform::Embed3D(pose_estimate_2d->cast<float>()));
34   std::unique_ptr<InsertionResult> insertion_result =
35       InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
36                        pose_estimate, gravity_alignment.rotation());
37   return common::make_unique<MatchingResult>(
38       MatchingResult{time, pose_estimate, std::move(range_data_in_local),
39                      std::move(insertion_result)});
40 }
41 
42 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
43 LocalTrajectoryBuilder::InsertIntoSubmap(
44     const common::Time time, const sensor::RangeData& range_data_in_local,
45     const sensor::RangeData& gravity_aligned_range_data,
46     const transform::Rigid3d& pose_estimate,
47     const Eigen::Quaterniond& gravity_alignment) 
48 {
49   if (motion_filter_.IsSimilar(time, pose_estimate))
50   {
51     return nullptr;
52   }
53 
54   // Querying the active submaps must be done here before calling
55   // InsertRangeData() since the queried values are valid for next insertion.
56   std::vector<std::shared_ptr<const Submap>> insertion_submaps;
57   for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) 
58   {
59     insertion_submaps.push_back(submap);
60   }
61   active_submaps_.InsertRangeData(range_data_in_local);
62 
63   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
64       options_.loop_closure_adaptive_voxel_filter_options());
65   const sensor::PointCloud filtered_gravity_aligned_point_cloud =
66       adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
67 
68   return common::make_unique<InsertionResult>(InsertionResult{
69       std::make_shared<const mapping::TrajectoryNode::Data>(
70           mapping::TrajectoryNode::Data{
71               time,
72               gravity_alignment,
73               filtered_gravity_aligned_point_cloud,
74               {},  // \'high_resolution_point_cloud\' is only used in 3D.
75               {},  // \'low_resolution_point_cloud\' is only used in 3D.
76               {},  // \'rotational_scan_matcher_histogram\' is only used in 3D.
77               pose_estimate}),
78       std::move(insertion_submaps)});
79 }

LocalTrajectoryBuilder::AddAccumulatedRangeData

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