Remove the UKF from local 2D SLAM. (#140)

Changes default configuration to log loop closure matches.

cartographer/mapping_2d/CMakeLists.txt

@@ -54,13 +54,14 @@ google_library(mapping_2d_local_trajectory_builder
     common_lua_parameter_dictionary
     common_make_unique
     common_time
-    kalman_filter_pose_tracker
     mapping_2d_proto_local_trajectory_builder_options
     mapping_2d_scan_matching_ceres_scan_matcher
     mapping_2d_scan_matching_real_time_correlative_scan_matcher
     mapping_2d_submaps
     mapping_3d_motion_filter
     mapping_global_trajectory_builder_interface
+    mapping_imu_tracker
+    mapping_odometry_state_tracker
     sensor_configuration
     sensor_laser
     sensor_voxel_filter

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -53,16 +53,14 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
   *options.mutable_motion_filter_options() =
       mapping_3d::CreateMotionFilterOptions(
           parameter_dictionary->GetDictionary("motion_filter").get());
-  *options.mutable_pose_tracker_options() =
-      kalman_filter::CreatePoseTrackerOptions(
-          parameter_dictionary->GetDictionary("pose_tracker").get());
+  options.set_imu_gravity_time_constant(
+      parameter_dictionary->GetDouble("imu_gravity_time_constant"));
+  options.set_num_odometry_states(
+      parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
+  CHECK_GT(options.num_odometry_states(), 0);
   *options.mutable_submaps_options() = CreateSubmapsOptions(
       parameter_dictionary->GetDictionary("submaps").get());
   options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data"));
-  options.set_odometer_translational_variance(
-      parameter_dictionary->GetDouble("odometer_translational_variance"));
-  options.set_odometer_rotational_variance(
-      parameter_dictionary->GetDouble("odometer_rotational_variance"));
   return options;
 }
 
@@ -70,11 +68,11 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions& options)
     : options_(options),
       submaps_(options.submaps_options()),
-      scan_matcher_pose_estimate_(transform::Rigid3d::Identity()),
       motion_filter_(options_.motion_filter_options()),
       real_time_correlative_scan_matcher_(
           options_.real_time_correlative_scan_matcher_options()),
-      ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
+      ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
+      odometry_state_tracker_(options_.num_odometry_states()) {}
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
@@ -135,45 +133,42 @@ void LocalTrajectoryBuilder::ScanMatch(
   transform::Rigid2d tracking_2d_to_map;
   kalman_filter::Pose2DCovariance covariance_observation_2d;
   ceres::Solver::Summary summary;
-  ceres_scan_matcher_.Match(
-      transform::Project2D(scan_matcher_pose_estimate_ *
-                           tracking_to_tracking_2d.inverse()),
-      initial_ceres_pose, filtered_point_cloud_in_tracking_2d, probability_grid,
-      &tracking_2d_to_map, &covariance_observation_2d, &summary);
+  ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose,
+                            filtered_point_cloud_in_tracking_2d,
+                            probability_grid, &tracking_2d_to_map,
+                            &covariance_observation_2d, &summary);
 
-  CHECK(pose_tracker_ != nullptr);
-
-  *pose_observation = transform::Embed3D(tracking_2d_to_map);
+  *pose_observation =
+      transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d;
   // This covariance is used for non-yaw rotation and the fake height of 0.
   constexpr double kFakePositionCovariance = 1.;
   constexpr double kFakeOrientationCovariance = 1.;
   *covariance_observation =
       kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance,
                              kFakeOrientationCovariance);
-  pose_tracker_->AddPoseObservation(
-      time, (*pose_observation) * tracking_to_tracking_2d,
-      *covariance_observation);
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddHorizontalLaserFan(
     const common::Time time, const sensor::LaserFan& laser_fan) {
-  // Initialize pose tracker now if we do not ever use an IMU.
+  // Initialize IMU tracker now if we do not ever use an IMU.
   if (!options_.use_imu_data()) {
-    InitializePoseTracker(time);
+    InitializeImuTracker(time);
   }
 
-  if (pose_tracker_ == nullptr) {
-    // Until we've initialized the UKF with our first IMU message, we cannot
-    // compute the orientation of the laser scanner.
-    LOG(INFO) << "PoseTracker not yet initialized.";
+  if (imu_tracker_ == nullptr) {
+    // Until we've initialized the IMU tracker with our first IMU message, we
+    // cannot compute the orientation of the laser scanner.
+    LOG(INFO) << "ImuTracker not yet initialized.";
     return nullptr;
   }
 
-  transform::Rigid3d pose_prediction;
-  kalman_filter::PoseCovariance covariance_prediction;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_prediction,
-                                                  &covariance_prediction);
+  Predict(time);
+  const transform::Rigid3d odometry_prediction =
+      pose_estimate_ * odometry_correction_;
+  const transform::Rigid3d model_prediction = pose_estimate_;
+  // TODO(whess): Prefer IMU over odom orientation if configured?
+  const transform::Rigid3d pose_prediction = odometry_prediction;
 
   // Computes the rotation without yaw, as defined by GetYaw().
   const transform::Rigid3d tracking_to_tracking_2d =
@@ -190,30 +185,42 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
     return nullptr;
   }
 
-  transform::Rigid3d pose_observation;
   kalman_filter::PoseCovariance covariance_observation;
   ScanMatch(time, pose_prediction, tracking_to_tracking_2d,
-            laser_fan_in_tracking_2d, &pose_observation,
-            &covariance_observation);
+            laser_fan_in_tracking_2d, &pose_estimate_, &covariance_observation);
+  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_});
+  }
 
-  kalman_filter::PoseCovariance covariance_estimate;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(
-      time, &scan_matcher_pose_estimate_, &covariance_estimate);
+  // Improve the velocity estimate.
+  if (last_scan_match_time_ > common::Time::min()) {
+    const double delta_t = common::ToSeconds(time - last_scan_match_time_);
+    velocity_estimate_ += (pose_estimate_.translation().head<2>() -
+                           model_prediction.translation().head<2>()) /
+                          delta_t;
+  }
+  last_scan_match_time_ = time_;
 
   // Remove the untracked z-component which floats around 0 in the UKF.
-  const auto translation = scan_matcher_pose_estimate_.translation();
-  scan_matcher_pose_estimate_ = transform::Rigid3d(
+  const auto translation = pose_estimate_.translation();
+  pose_estimate_ = transform::Rigid3d(
       transform::Rigid3d::Vector(translation.x(), translation.y(), 0.),
-      scan_matcher_pose_estimate_.rotation());
+      pose_estimate_.rotation());
 
   const transform::Rigid3d tracking_2d_to_map =
-      scan_matcher_pose_estimate_ * tracking_to_tracking_2d.inverse();
+      pose_estimate_ * tracking_to_tracking_2d.inverse();
   last_pose_estimate_ = {
       time,
-      {pose_prediction, covariance_prediction},
-      {pose_observation, covariance_observation},
-      {scan_matcher_pose_estimate_, covariance_estimate},
-      scan_matcher_pose_estimate_,
+      {pose_prediction, covariance_observation},
+      {pose_estimate_, covariance_observation},
+      {pose_estimate_, covariance_observation},
+      pose_estimate_,
       sensor::TransformPointCloud(laser_fan_in_tracking_2d.returns,
                                   tracking_2d_to_map.cast<float>())};
 
@@ -229,13 +236,14 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
   for (int insertion_index : submaps_.insertion_indices()) {
     insertion_submaps.push_back(submaps_.Get(insertion_index));
   }
-  submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d,
-                                            tracking_2d_to_map.cast<float>()));
+  submaps_.InsertLaserFan(
+      TransformLaserFan(laser_fan_in_tracking_2d,
+                        transform::Embed3D(pose_estimate_2d.cast<float>())));
 
   return common::make_unique<InsertionResult>(InsertionResult{
       time, &submaps_, matching_submap, insertion_submaps,
       tracking_to_tracking_2d, tracking_2d_to_map, laser_fan_in_tracking_2d,
-      pose_estimate_2d, covariance_estimate});
+      pose_estimate_2d, covariance_observation});
 }
 
 const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
@@ -248,19 +256,15 @@ void LocalTrajectoryBuilder::AddImuData(
     const Eigen::Vector3d& angular_velocity) {
   CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
 
-  InitializePoseTracker(time);
-  pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
-  pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
-
-  transform::Rigid3d pose_estimate;
-  kalman_filter::PoseCovariance unused_covariance_estimate;
-  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate,
-                                                  &unused_covariance_estimate);
+  InitializeImuTracker(time);
+  Predict(time);
+  imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
+  imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
 
   // Log a warning if the backpack inclination exceeds 20 degrees. In these
   // cases, it's very likely that 2D SLAM will fail.
   const Eigen::Vector3d gravity_direction =
-      Eigen::Quaterniond(pose_estimate.rotation()) * Eigen::Vector3d::UnitZ();
+      imu_tracker_->orientation() * Eigen::Vector3d::UnitZ();
   const double inclination = std::acos(gravity_direction.z());
   constexpr double kMaxInclination = common::DegToRad(20.);
   LOG_IF_EVERY_N(WARNING, inclination > kMaxInclination, 1000)
@@ -268,26 +272,56 @@ void LocalTrajectoryBuilder::AddImuData(
       << common::RadToDeg(kMaxInclination);
 }
 
-void LocalTrajectoryBuilder::AddOdometerData(const common::Time time,
-                                             const transform::Rigid3d& pose) {
-  if (pose_tracker_ == nullptr) {
-    // Until we've initialized the UKF with our first IMU message, we cannot
-    // process odometer poses.
-    LOG_EVERY_N(INFO, 100) << "PoseTracker not yet initialized.";
-  } else {
-    pose_tracker_->AddOdometerPoseObservation(
-        time, pose, kalman_filter::BuildPoseCovariance(
-                        options_.odometer_translational_variance(),
-                        options_.odometer_rotational_variance()));
+void LocalTrajectoryBuilder::AddOdometerData(
+    const common::Time time, const transform::Rigid3d& odometer_pose) {
+  if (imu_tracker_ == nullptr) {
+    // Until we've initialized the IMU tracker we do not want to call Predict().
+    LOG(INFO) << "ImuTracker not yet initialized.";
+    return;
+  }
+
+  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_});
+}
+
+void LocalTrajectoryBuilder::InitializeImuTracker(const common::Time time) {
+  if (imu_tracker_ == nullptr) {
+    imu_tracker_ = common::make_unique<mapping::ImuTracker>(
+        options_.imu_gravity_time_constant(), time);
   }
 }
 
-void LocalTrajectoryBuilder::InitializePoseTracker(const common::Time time) {
-  if (pose_tracker_ == nullptr) {
-    pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
-        options_.pose_tracker_options(),
-        kalman_filter::PoseTracker::ModelFunction::k2D, time);
+void LocalTrajectoryBuilder::Predict(const common::Time time) {
+  CHECK(imu_tracker_ != nullptr);
+  CHECK_LE(time_, time);
+  const double last_yaw = transform::GetYaw(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 *
+            Eigen::Vector3d(velocity_estimate_.x(), velocity_estimate_.y(), 0.);
+    // Use the current IMU tracker roll and pitch for gravity alignment, and
+    // apply its change in yaw.
+    const Eigen::Quaterniond rotation =
+        Eigen::AngleAxisd(
+            transform::GetYaw(pose_estimate_.rotation()) - last_yaw,
+            Eigen::Vector3d::UnitZ()) *
+        imu_tracker_->orientation();
+    pose_estimate_ = transform::Rigid3d(translation, rotation);
   }
+  time_ = time;
 }
 
 }  // namespace mapping_2d

cartographer/mapping_2d/local_trajectory_builder.h

@@ -21,8 +21,9 @@
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping/imu_tracker.h"
+#include "cartographer/mapping/odometry_state_tracker.h"
 #include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
@@ -84,22 +85,33 @@ class LocalTrajectoryBuilder {
                  transform::Rigid3d* pose_observation,
                  kalman_filter::PoseCovariance* covariance_observation);
 
-  // Lazily constructs a PoseTracker.
-  void InitializePoseTracker(common::Time time);
+  // Lazily constructs an ImuTracker.
+  void InitializeImuTracker(common::Time time);
+
+  // Updates the current estimate to reflect the given 'time'.
+  void Predict(common::Time time);
 
   const proto::LocalTrajectoryBuilderOptions options_;
   Submaps submaps_;
   mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_;
 
-  // Pose of the last computed scan match.
-  transform::Rigid3d scan_matcher_pose_estimate_;
+  // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'.
+  common::Time time_ = common::Time::min();
+  transform::Rigid3d pose_estimate_ = transform::Rigid3d::Identity();
+  Eigen::Vector2d velocity_estimate_ = Eigen::Vector2d::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();
 
   mapping_3d::MotionFilter motion_filter_;
   scan_matching::RealTimeCorrelativeScanMatcher
       real_time_correlative_scan_matcher_;
   scan_matching::CeresScanMatcher ceres_scan_matcher_;
 
-  std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
+  std::unique_ptr<mapping::ImuTracker> imu_tracker_;
+  mapping::OdometryStateTracker odometry_state_tracker_;
 };
 
 }  // namespace mapping_2d

cartographer/mapping_2d/proto/CMakeLists.txt

@@ -21,7 +21,6 @@ google_proto_library(mapping_2d_proto_local_trajectory_builder_options
   SRCS
     local_trajectory_builder_options.proto
   DEPENDS
-    kalman_filter_proto_pose_tracker_options
     mapping_2d_proto_submaps_options
     mapping_2d_scan_matching_proto_ceres_scan_matcher_options
     mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options

cartographer/mapping_2d/proto/local_trajectory_builder_options.proto

@@ -16,7 +16,6 @@ syntax = "proto2";
 
 package cartographer.mapping_2d.proto;
 
-import "cartographer/kalman_filter/proto/pose_tracker_options.proto";
 import "cartographer/mapping_3d/proto/motion_filter_options.proto";
 import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
 import "cartographer/mapping_2d/proto/submaps_options.proto";
@@ -51,12 +50,20 @@ message LocalTrajectoryBuilderOptions {
   optional scan_matching.proto.CeresScanMatcherOptions
       ceres_scan_matcher_options = 8;
   optional mapping_3d.proto.MotionFilterOptions motion_filter_options = 13;
-  optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 10;
+
+  // Time constant in seconds for the orientation moving average based on
+  // observed gravity via the IMU. It should be chosen so that the error
+  // 1. from acceleration measurements not due to gravity (which gets worse when
+  // the constant is reduced) and
+  // 2. from integration of angular velocities (which gets worse when the
+  // constant is increased) is balanced.
+  optional double imu_gravity_time_constant = 17;
+
+  // Maximum number of previous odometry states to keep.
+  optional int32 num_odometry_states = 18;
+
   optional mapping_2d.proto.SubmapsOptions submaps_options = 11;
 
   // True if IMU data should be expected and used.
   optional bool use_imu_data = 12;
-
-  optional double odometer_translational_variance = 17;
-  optional double odometer_rotational_variance = 18;
 }

cartographer/sensor/CMakeLists.txt

@@ -60,6 +60,7 @@ google_library(sensor_data
     common_time
     kalman_filter_pose_tracker
     sensor_laser
+    sensor_point_cloud
     transform_rigid_transform
 )
 

cartographer/transform/rigid_transform.h

@@ -201,7 +201,7 @@ Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                             const Rigid3<FloatType>& rhs) {
   return Rigid3<FloatType>(
       lhs.rotation() * rhs.translation() + lhs.translation(),
-      lhs.rotation() * rhs.rotation());
+      (lhs.rotation() * rhs.rotation()).normalized());
 }
 
 template <typename FloatType>

cartographer/transform/transform.h

@@ -36,14 +36,21 @@ FloatType GetAngle(const Rigid3<FloatType>& transform) {
                                    std::abs(transform.rotation().w()));
 }
 
+// Returns the yaw component in radians of the given 3D 'rotation'. Assuming
+// 'rotation' is composed of three rotations around X, then Y, then Z, returns
+// the angle of the Z rotation.
+template <typename T>
+T GetYaw(const Eigen::Quaternion<T>& rotation) {
+  const Eigen::Matrix<T, 3, 1> direction =
+      rotation * Eigen::Matrix<T, 3, 1>::UnitX();
+  return atan2(direction.y(), direction.x());
+}
+
 // Returns the yaw component in radians of the given 3D transformation
-// 'transform'. Assuming three rotations around X, then Y, then Z axis resulted
-// in the rotational component, the yaw is the angle of the Z rotation.
+// 'transform'.
 template <typename T>
 T GetYaw(const Rigid3<T>& transform) {
-  const Eigen::Matrix<T, 3, 1> direction =
-      transform.rotation() * Eigen::Matrix<T, 3, 1>::UnitX();
-  return atan2(direction.y(), direction.x());
+  return GetYaw(transform.rotation());
 }
 
 // Returns an angle-axis vector (a vector with the length of the rotation angle

configuration_files/sparse_pose_graph.lua

@@ -25,7 +25,7 @@ SPARSE_POSE_GRAPH = {
     min_score = 0.55,
     global_localization_min_score = 0.6,
     lower_covariance_eigenvalue_bound = 1e-11,
-    log_matches = false,
+    log_matches = true,
     fast_correlative_scan_matcher = {
       linear_search_window = 7.,
       angular_search_window = math.rad(30.),

configuration_files/trajectory_builder_2d.lua

@@ -20,8 +20,6 @@ TRAJECTORY_BUILDER_2D = {
   laser_max_z = 2.,
   laser_missing_echo_ray_length = 5.,
   laser_voxel_filter_size = 0.025,
-  odometer_translational_variance = 1e-7,
-  odometer_rotational_variance = 1e-7,
 
   use_online_correlative_scan_matching = false,
   adaptive_voxel_filter = {
@@ -38,13 +36,13 @@ TRAJECTORY_BUILDER_2D = {
   },
 
   ceres_scan_matcher = {
-    occupied_space_weight = 20.,
-    translation_weight = 1.,
+    occupied_space_weight = 1e1,
+    translation_weight = 1e1,
     rotation_weight = 1e2,
-    covariance_scale = 2.34e-4,
+    covariance_scale = 1e-2,
     ceres_solver_options = {
-      use_nonmonotonic_steps = true,
-      max_num_iterations = 50,
+      use_nonmonotonic_steps = false,
+      max_num_iterations = 20,
       num_threads = 1,
     },
   },
@@ -55,14 +53,8 @@ TRAJECTORY_BUILDER_2D = {
     max_angle_radians = math.rad(1.),
   },
 
-  pose_tracker = {
-    orientation_model_variance = 5e-4,
-    position_model_variance = 0.000654766,
-    velocity_model_variance = 0.053926,
   imu_gravity_time_constant = 10.,
-    imu_gravity_variance = 1e-6,
   num_odometry_states = 1000,
-  },
 
   submaps = {
     resolution = 0.05,