Use PoseExtrapolator in the 3D trajectory builder. (#450)

This replaces the Predict() logic in the 3D trajectory builder.

cartographer/mapping/pose_extrapolator.cc

@@ -30,6 +30,25 @@ PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
     : pose_queue_duration_(pose_queue_duration),
       gravity_time_constant_(gravity_time_constant) {}
 
+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;
+}
+
 common::Time PoseExtrapolator::GetLastPoseTime() const {
   if (timed_pose_queue_.empty()) {
     return common::Time::min();

cartographer/mapping/pose_extrapolator.h

@@ -40,6 +40,10 @@ class PoseExtrapolator {
   PoseExtrapolator(const PoseExtrapolator&) = delete;
   PoseExtrapolator& operator=(const PoseExtrapolator&) = delete;
 
+  static std::unique_ptr<PoseExtrapolator> InitializeWithImu(
+      common::Duration pose_queue_duration, double imu_gravity_time_constant,
+      const sensor::ImuData& imu_data);
+
   // Returns the time of the last added pose or Time::min() if no pose was added
   // yet.
   common::Time GetLastPoseTime() const;
@@ -49,6 +53,11 @@ class PoseExtrapolator {
   void AddOdometryData(const sensor::OdometryData& odometry_data);
   transform::Rigid3d ExtrapolatePose(common::Time time);
 
+  // Gravity alignment estimate.
+  Eigen::Quaterniond gravity_orientation() const {
+    return imu_tracker_->orientation();
+  }
+
  private:
   void UpdateVelocitiesFromPoses();
   void TrimImuData();

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -37,46 +37,43 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
               options_.real_time_correlative_scan_matcher_options())),
       ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
           options_.ceres_scan_matcher_options())),
-      odometry_state_tracker_(options_.num_odometry_states()),
       accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
 void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
-  const bool initial_imu_data = (imu_tracker_ == nullptr);
+  if (extrapolator_ != nullptr) {
-  if (initial_imu_data) {
+    extrapolator_->AddImuData(imu_data);
-    imu_tracker_ = common::make_unique<mapping::ImuTracker>(
+    return;
-        options_.imu_gravity_time_constant(), imu_data.time);
-  }
-  Predict(imu_data.time);
-  imu_tracker_->AddImuLinearAccelerationObservation(
-      imu_data.linear_acceleration);
-  imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);
-  if (initial_imu_data) {
-    // This uses the first accelerometer measurement to approximately align the
-    // first pose to gravity.
-    pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
   }
+  // 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;
+  extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu(
+      ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
+      options_.imu_gravity_time_constant(), imu_data);
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
                                            const Eigen::Vector3f& origin,
                                            const sensor::PointCloud& ranges) {
-  if (imu_tracker_ == nullptr) {
+  if (extrapolator_ == nullptr) {
-    LOG(INFO) << "ImuTracker not yet initialized.";
+    // Until we've initialized the extrapolator with our first IMU message, we
+    // cannot compute the orientation of the rangefinder.
+    LOG(INFO) << "IMU not yet initialized.";
     return nullptr;
   }
-
-  Predict(time);
   if (num_accumulated_ == 0) {
-    first_pose_estimate_ = pose_estimate_.cast<float>();
+    first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>();
     accumulated_range_data_ =
         sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
   }
 
   const transform::Rigid3f tracking_delta =
-      first_pose_estimate_.inverse() * pose_estimate_.cast<float>();
+      first_pose_estimate_.inverse() *
+      extrapolator_->ExtrapolatePose(time).cast<float>();
   const sensor::RangeData range_data_in_first_tracking =
       sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
                                  tracking_delta);
@@ -107,24 +104,6 @@ LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
   return nullptr;
 }
 
-void LocalTrajectoryBuilder::Predict(const common::Time time) {
-  CHECK(imu_tracker_ != nullptr);
-  CHECK_LE(time_, time);
-  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
-  imu_tracker_->Advance(time);
-  if (time_ > common::Time::min()) {
-    const double delta_t = common::ToSeconds(time - time_);
-    // Constant velocity model.
-    const Eigen::Vector3d translation =
-        pose_estimate_.translation() + delta_t * velocity_estimate_;
-    const Eigen::Quaterniond rotation = pose_estimate_.rotation() *
-                                        last_orientation.inverse() *
-                                        imu_tracker_->orientation();
-    pose_estimate_ = transform::Rigid3d(translation, rotation);
-  }
-  time_ = time;
-}
-
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time, const sensor::RangeData& range_data_in_tracking) {
@@ -140,11 +119,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
     return nullptr;
   }
 
-  Predict(time);
+  const transform::Rigid3d pose_prediction =
-  const transform::Rigid3d odometry_prediction =
+      extrapolator_->ExtrapolatePose(time);
-      pose_estimate_ * odometry_correction_;
-  const transform::Rigid3d model_prediction = pose_estimate_;
-  const transform::Rigid3d& pose_prediction = odometry_prediction;
 
   std::shared_ptr<const Submap> matching_submap =
       active_submaps_.submaps().front();
@@ -177,57 +153,26 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
        {&low_resolution_point_cloud_in_tracking,
         &matching_submap->low_resolution_hybrid_grid()}},
       &pose_observation_in_submap, &summary);
-  pose_estimate_ = matching_submap->local_pose() * pose_observation_in_submap;
+  const transform::Rigid3d pose_estimate =
-
+      matching_submap->local_pose() * pose_observation_in_submap;
-  odometry_correction_ = transform::Rigid3d::Identity();
+  extrapolator_->AddPose(time, pose_estimate);
-  if (!odometry_state_tracker_.empty()) {
-    // We add an odometry state, so that the correction from the scan matching
-    // is not removed by the next odometry data we get.
-    odometry_state_tracker_.AddOdometryState(
-        {time, odometry_state_tracker_.newest().odometer_pose,
-         odometry_state_tracker_.newest().state_pose *
-             odometry_prediction.inverse() * pose_estimate_});
-  }
-
-  // Improve the velocity estimate.
-  if (last_scan_match_time_ > common::Time::min() &&
-      time > last_scan_match_time_) {
-    const double delta_t = common::ToSeconds(time - last_scan_match_time_);
-    // This adds the observed difference in velocity that would have reduced the
-    // error to zero.
-    velocity_estimate_ +=
-        (pose_estimate_.translation() - model_prediction.translation()) /
-        delta_t;
-  }
-  last_scan_match_time_ = time_;
 
   last_pose_estimate_ = {
-      time, pose_estimate_,
+      time, pose_estimate,
       sensor::TransformPointCloud(filtered_range_data.returns,
-                                  pose_estimate_.cast<float>())};
+                                  pose_estimate.cast<float>())};
 
-  return InsertIntoSubmap(time, filtered_range_data, pose_estimate_);
+  return InsertIntoSubmap(time, filtered_range_data, pose_estimate);
 }
 
 void LocalTrajectoryBuilder::AddOdometerData(
     const common::Time time, const transform::Rigid3d& odometer_pose) {
-  if (imu_tracker_ == nullptr) {
+  if (extrapolator_ == nullptr) {
-    // Until we've initialized the IMU tracker we do not want to call Predict().
+    // Until we've initialized the extrapolator we cannot add odometry data.
-    LOG(INFO) << "ImuTracker not yet initialized.";
+    LOG(INFO) << "Extrapolator not yet initialized.";
     return;
   }
-
+  extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose});
-  Predict(time);
-  if (!odometry_state_tracker_.empty()) {
-    const auto& previous_odometry_state = odometry_state_tracker_.newest();
-    const transform::Rigid3d delta =
-        previous_odometry_state.odometer_pose.inverse() * odometer_pose;
-    const transform::Rigid3d new_pose =
-        previous_odometry_state.state_pose * delta;
-    odometry_correction_ = pose_estimate_.inverse() * new_pose;
-  }
-  odometry_state_tracker_.AddOdometryState(
-      {time, odometer_pose, pose_estimate_ * odometry_correction_});
 }
 
 const LocalTrajectoryBuilder::PoseEstimate&
@@ -251,7 +196,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
   active_submaps_.InsertRangeData(
       sensor::TransformRangeData(range_data_in_tracking,
                                  pose_observation.cast<float>()),
-      imu_tracker_->orientation());
+      extrapolator_->gravity_orientation());
   return std::unique_ptr<InsertionResult>(
       new InsertionResult{time, range_data_in_tracking, pose_observation,
                           std::move(insertion_submaps)});

cartographer/mapping_3d/local_trajectory_builder.h

@@ -21,8 +21,7 @@
 
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping/imu_tracker.h"
+#include "cartographer/mapping/pose_extrapolator.h"
-#include "cartographer/mapping/odometry_state_tracker.h"
 #include "cartographer/mapping_3d/motion_filter.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
@@ -64,8 +63,6 @@ class LocalTrajectoryBuilder {
   const PoseEstimate& pose_estimate() const;
 
  private:
-  void Predict(common::Time time);
-
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
       common::Time time, const sensor::RangeData& range_data_in_tracking);
 
@@ -83,17 +80,7 @@ class LocalTrajectoryBuilder {
       real_time_correlative_scan_matcher_;
   std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
 
-  // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'.
+  std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
-  common::Time time_ = common::Time::min();
-  transform::Rigid3d pose_estimate_ = transform::Rigid3d::Identity();
-  Eigen::Vector3d velocity_estimate_ = Eigen::Vector3d::Zero();
-  common::Time last_scan_match_time_ = common::Time::min();
-  // This is the difference between the model (constant velocity, IMU)
-  // prediction 'pose_estimate_' and the odometry prediction. To get the
-  // odometry prediction, right-multiply this to 'pose_estimate_'.
-  transform::Rigid3d odometry_correction_ = transform::Rigid3d::Identity();
-  std::unique_ptr<mapping::ImuTracker> imu_tracker_;
-  mapping::OdometryStateTracker odometry_state_tracker_;
 
   int num_accumulated_ = 0;
   transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();