Change local 3D SLAM to not use a UKF. (#375)

This follows the 2D local SLAM in implementing a simple
constant velocity model and allows integrating odometry and
rotation information from an IMU.

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -32,7 +32,6 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions& options)
     : options_(options),
       active_submaps_(options.submaps_options()),
-      scan_matcher_pose_estimate_(transform::Rigid3d::Identity()),
       motion_filter_(options.motion_filter_options()),
       real_time_correlative_scan_matcher_(
           common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
@@ -40,8 +39,9 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
                   .real_time_correlative_scan_matcher_options())),
       ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
           options_.ceres_scan_matcher_options())),
-      num_accumulated_(0),
+      odometry_state_tracker_(options_.kalman_local_trajectory_builder_options()
-      first_pose_prediction_(transform::Rigid3f::Identity()),
+                                  .pose_tracker_options()
+                                  .num_odometry_states()),
       accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
@@ -49,36 +49,40 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 void LocalTrajectoryBuilder::AddImuData(
     const common::Time time, const Eigen::Vector3d& linear_acceleration,
     const Eigen::Vector3d& angular_velocity) {
-  if (!pose_tracker_) {
+  if (!imu_tracker_) {
-    pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
+    imu_tracker_ = common::make_unique<mapping::ImuTracker>(
         options_.kalman_local_trajectory_builder_options()
-            .pose_tracker_options(),
+            .pose_tracker_options()
+            .imu_gravity_time_constant(),
         time);
+    // Use the accelerometer to gravity align the first pose.
+    imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
+    pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
   }
 
-  pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
+  Predict(time);
-  pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
+  imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
+  imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
                                            const Eigen::Vector3f& origin,
                                            const sensor::PointCloud& ranges) {
-  if (!pose_tracker_) {
+  if (!imu_tracker_) {
-    LOG(INFO) << "PoseTracker not yet initialized.";
+    LOG(INFO) << "ImuTracker not yet initialized.";
     return nullptr;
   }
 
-  const transform::Rigid3d pose_prediction =
+  Predict(time);
-      pose_tracker_->GetPoseEstimateMean(time);
   if (num_accumulated_ == 0) {
-    first_pose_prediction_ = pose_prediction.cast<float>();
+    first_pose_estimate_ = pose_estimate_.cast<float>();
     accumulated_range_data_ =
         sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
   }
 
   const transform::Rigid3f tracking_delta =
-      first_pose_prediction_.inverse() * pose_prediction.cast<float>();
+      first_pose_estimate_.inverse() * pose_estimate_.cast<float>();
   const sensor::RangeData range_data_in_first_tracking =
       sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
                                  tracking_delta);
@@ -109,6 +113,24 @@ 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) {
@@ -124,8 +146,11 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
     return nullptr;
   }
 
-  const transform::Rigid3d pose_prediction =
+  Predict(time);
-      pose_tracker_->GetPoseEstimateMean(time);
+  const transform::Rigid3d odometry_prediction =
+      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();
@@ -152,46 +177,62 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   const sensor::PointCloud low_resolution_point_cloud_in_tracking =
       low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
   ceres_scan_matcher_->Match(
-      matching_submap->local_pose().inverse() * scan_matcher_pose_estimate_,
+      matching_submap->local_pose().inverse() * pose_prediction,
       initial_ceres_pose,
       {{&filtered_point_cloud_in_tracking,
         &matching_submap->high_resolution_hybrid_grid()},
        {&low_resolution_point_cloud_in_tracking,
         &matching_submap->low_resolution_hybrid_grid()}},
       &pose_observation_in_submap, &summary);
-  const transform::Rigid3d pose_observation =
+  pose_estimate_ = matching_submap->local_pose() * pose_observation_in_submap;
-      matching_submap->local_pose() * pose_observation_in_submap;
-  pose_tracker_->AddPoseObservation(
-      time, pose_observation,
-      options_.kalman_local_trajectory_builder_options()
-              .scan_matcher_variance() *
-          kalman_filter::PoseCovariance::Identity());
 
-  scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time);
+  odometry_correction_ = transform::Rigid3d::Identity();
+  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_);
+    velocity_estimate_ +=
+        (pose_estimate_.translation() - model_prediction.translation()) /
+        delta_t;
+  }
+  last_scan_match_time_ = time_;
 
   last_pose_estimate_ = {
-      time, scan_matcher_pose_estimate_,
+      time, pose_estimate_,
       sensor::TransformPointCloud(filtered_range_data.returns,
-                                  pose_observation.cast<float>())};
+                                  pose_estimate_.cast<float>())};
 
-  return InsertIntoSubmap(time, filtered_range_data, pose_observation);
+  return InsertIntoSubmap(time, filtered_range_data, pose_estimate_);
 }
 
-void LocalTrajectoryBuilder::AddOdometerData(const common::Time time,
+void LocalTrajectoryBuilder::AddOdometerData(
-                                             const transform::Rigid3d& pose) {
+    const common::Time time, const transform::Rigid3d& odometer_pose) {
-  if (!pose_tracker_) {
+  if (imu_tracker_ == nullptr) {
-    pose_tracker_.reset(new kalman_filter::PoseTracker(
+    // Until we've initialized the IMU tracker we do not want to call Predict().
-        options_.kalman_local_trajectory_builder_options()
+    LOG(INFO) << "ImuTracker not yet initialized.";
-            .pose_tracker_options(),
+    return;
-        time));
   }
-  pose_tracker_->AddOdometerPoseObservation(
+
-      time, pose,
+  Predict(time);
-      kalman_filter::BuildPoseCovariance(
+  if (!odometry_state_tracker_.empty()) {
-          options_.kalman_local_trajectory_builder_options()
+    const auto& previous_odometry_state = odometry_state_tracker_.newest();
-              .odometer_translational_variance(),
+    const transform::Rigid3d delta =
-          options_.kalman_local_trajectory_builder_options()
+        previous_odometry_state.odometer_pose.inverse() * odometer_pose;
-              .odometer_rotational_variance()));
+    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&
@@ -215,7 +256,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
   active_submaps_.InsertRangeData(
       sensor::TransformRangeData(range_data_in_tracking,
                                  pose_observation.cast<float>()),
-      pose_tracker_->gravity_orientation());
+      imu_tracker_->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

@@ -59,10 +59,13 @@ class LocalTrajectoryBuilder {
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
       const sensor::PointCloud& ranges);
-  void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
+  void AddOdometerData(common::Time time,
+                       const transform::Rigid3d& odometer_pose);
   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);
 
@@ -75,18 +78,25 @@ class LocalTrajectoryBuilder {
 
   PoseEstimate last_pose_estimate_;
 
-  // Pose of the last computed scan match.
-  transform::Rigid3d scan_matcher_pose_estimate_;
-
   MotionFilter motion_filter_;
   std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>
       real_time_correlative_scan_matcher_;
   std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
 
-  std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
+  // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'.
+  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_;
+  int num_accumulated_ = 0;
-  transform::Rigid3f first_pose_prediction_;
+  transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
   sensor::RangeData accumulated_range_data_;
 };
 

configuration_files/trajectory_builder_3d.lua

@@ -33,10 +33,10 @@ TRAJECTORY_BUILDER_3D = {
   },
 
   ceres_scan_matcher = {
-    occupied_space_weight_0 = 5.,
+    occupied_space_weight_0 = 1.,
-    occupied_space_weight_1 = 30.,
+    occupied_space_weight_1 = 6.,
-    translation_weight = 0.3,
+    translation_weight = 5.,
-    rotation_weight = 2e3,
+    rotation_weight = 4e2,
     only_optimize_yaw = false,
     ceres_solver_options = {
       use_nonmonotonic_steps = false,