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

 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 }

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