Integrate imu based extrapolator (#1732)

This integrates the imu based extrapolator implementation, adds lua configuration and conversions from lua to proto. Note that the parameters of the imu based extrapolator are not tuned yet and the default is still the constant-velocity extrapolator.

Signed-off-by: mschworer <mschworer@lyft.com>

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -108,8 +108,18 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
     extrapolator_->AddImuData(imu_data);
     return;
   }
+  std::vector<transform::TimestampedTransform> initial_poses;
+  for (const auto& pose_proto : options_.initial_poses()) {
+    initial_poses.push_back(transform::FromProto(pose_proto));
+  }
+  std::vector<sensor::ImuData> initial_imu_data;
+  for (const auto& imu : options_.initial_imu_data()) {
+    initial_imu_data.push_back(sensor::FromProto(imu));
+  }
+  initial_imu_data.push_back(imu_data);
+
   extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData(
-      options_.pose_extrapolator_options(), {imu_data});
+      options_.pose_extrapolator_options(), initial_imu_data, initial_poses);
 }
 
 std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>

cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc

@@ -98,10 +98,26 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
           rotational_histogram_size = 120,
           
           pose_extrapolator = {
+            use_imu_based = false,
             constant_velocity = {
               imu_gravity_time_constant = 10.,
               pose_queue_duration = 0.001,
             },
+            imu_based = {
+              pose_queue_duration = 5.,
+              gravity_constant = 9.806,
+              pose_translation_weight = 1.,
+              pose_rotation_weight = 1.,
+              imu_acceleration_weight = 1.,
+              imu_rotation_weight = 1.,
+              odometry_translation_weight = 1.,
+              odometry_rotation_weight = 1.,
+              solver_options = {
+                use_nonmonotonic_steps = false;
+                max_num_iterations = 10;
+                num_threads = 1;
+              },
+            },
           },
           
           submaps = {

cartographer/mapping/pose_extrapolator_interface.cc

@@ -16,7 +16,9 @@
 
 #include "cartographer/mapping/pose_extrapolator_interface.h"
 
+#include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/imu_based_pose_extrapolator.h"
 #include "cartographer/mapping/pose_extrapolator.h"
 
 namespace cartographer {
@@ -35,27 +37,64 @@ CreateConstantVelocityPoseExtrapolatorOptions(
   return options;
 }
 
+proto::ImuBasedPoseExtrapolatorOptions CreateImuBasedPoseExtrapolatorOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::ImuBasedPoseExtrapolatorOptions options;
+
+  options.set_pose_queue_duration(
+      parameter_dictionary->GetDouble("pose_queue_duration"));
+  options.set_gravity_constant(
+      parameter_dictionary->GetDouble("gravity_constant"));
+  options.set_pose_translation_weight(
+      parameter_dictionary->GetDouble("pose_translation_weight"));
+  options.set_pose_rotation_weight(
+      parameter_dictionary->GetDouble("pose_rotation_weight"));
+  options.set_imu_acceleration_weight(
+      parameter_dictionary->GetDouble("imu_acceleration_weight"));
+  options.set_imu_rotation_weight(
+      parameter_dictionary->GetDouble("imu_rotation_weight"));
+  options.set_odometry_rotation_weight(
+      parameter_dictionary->GetDouble("odometry_rotation_weight"));
+  options.set_odometry_translation_weight(
+      parameter_dictionary->GetDouble("odometry_translation_weight"));
+  *options.mutable_solver_options() = CreateCeresSolverOptionsProto(
+      parameter_dictionary->GetDictionary("solver_options").get());
+
+  return options;
+}
+
 }  // namespace
 
 proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::PoseExtrapolatorOptions options;
+  options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based"));
   *options.mutable_constant_velocity() =
       CreateConstantVelocityPoseExtrapolatorOptions(
           parameter_dictionary->GetDictionary("constant_velocity").get());
+  *options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions(
+      parameter_dictionary->GetDictionary("imu_based").get());
+
   return options;
 }
 
 std::unique_ptr<PoseExtrapolatorInterface>
 PoseExtrapolatorInterface::CreateWithImuData(
     const proto::PoseExtrapolatorOptions& options,
-    const std::vector<sensor::ImuData>& imu_data) {
+    const std::vector<sensor::ImuData>& imu_data,
+    const std::vector<transform::TimestampedTransform>& initial_poses) {
   CHECK(!imu_data.empty());
   // TODO(schwoere): Implement/integrate imu based extrapolator.
   CHECK(!options.use_imu_based()) << "Not implemented!";
-  return PoseExtrapolator::InitializeWithImu(
-      common::FromSeconds(options.constant_velocity().pose_queue_duration()),
-      options.constant_velocity().imu_gravity_time_constant(), imu_data.back());
+  if (options.use_imu_based()) {
+    return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(),
+                                                       imu_data, initial_poses);
+  } else {
+    return PoseExtrapolator::InitializeWithImu(
+        common::FromSeconds(options.constant_velocity().pose_queue_duration()),
+        options.constant_velocity().imu_gravity_time_constant(),
+        imu_data.back());
+  }
 }
 
 }  // namespace mapping

cartographer/mapping/pose_extrapolator_interface.h

@@ -45,13 +45,15 @@ class PoseExtrapolatorInterface {
   };
 
   PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete;
-  PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = delete;
+  PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) =
+      delete;
   virtual ~PoseExtrapolatorInterface() {}
 
   // TODO: Remove dependency cycle.
   static std::unique_ptr<PoseExtrapolatorInterface> CreateWithImuData(
       const proto::PoseExtrapolatorOptions& options,
-      const std::vector<sensor::ImuData>& imu_data);
+      const std::vector<sensor::ImuData>& imu_data,
+      const std::vector<transform::TimestampedTransform>& initial_poses);
 
   // Returns the time of the last added pose or Time::min() if no pose was added
   // yet.

cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto

@@ -60,12 +60,12 @@ message LocalTrajectoryBuilderOptions2D {
   MotionFilterOptions motion_filter_options = 13;
 
   // 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.	
-  // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and 
+  // 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.
+  // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and
   // was replaced by pose_extrapolator_options.
   double imu_gravity_time_constant = 17;
   mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21;

cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto

@@ -22,8 +22,10 @@ import "cartographer/mapping/proto/pose_extrapolator_options.proto";
 import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto";
 import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto";
 import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
+import "cartographer/sensor/proto/sensor.proto";
+import "cartographer/transform/proto/timestamped_transform.proto";
 
-// NEXT ID: 19
+// NEXT ID: 21
 message LocalTrajectoryBuilderOptions3D {
   // Rangefinder points outside these ranges will be dropped.
   float min_range = 1;
@@ -53,12 +55,12 @@ message LocalTrajectoryBuilderOptions3D {
   mapping.proto.MotionFilterOptions motion_filter_options = 7;
 
   // 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.	
-  // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and 
+  // 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.
+  // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and
   // was replaced by pose_extrapolator_options.
   double imu_gravity_time_constant = 15;
 
@@ -67,5 +69,8 @@ message LocalTrajectoryBuilderOptions3D {
 
   mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18;
 
+  repeated transform.proto.TimestampedTransform initial_poses = 19;
+  repeated sensor.proto.ImuData initial_imu_data = 20;
+
   SubmapsOptions3D submaps_options = 8;
 }

configuration_files/trajectory_builder_2d.lua

@@ -62,10 +62,26 @@ TRAJECTORY_BUILDER_2D = {
   -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
   imu_gravity_time_constant = 10.,
   pose_extrapolator = {
+    use_imu_based = false,
     constant_velocity = {
       imu_gravity_time_constant = 10.,
       pose_queue_duration = 0.001,
     },
+    imu_based = {
+      pose_queue_duration = 5.,
+      gravity_constant = 9.806,
+      pose_translation_weight = 1.,
+      pose_rotation_weight = 1.,
+      imu_acceleration_weight = 1.,
+      imu_rotation_weight = 1.,
+      odometry_translation_weight = 1.,
+      odometry_rotation_weight = 1.,
+      solver_options = {
+        use_nonmonotonic_steps = false;
+        max_num_iterations = 10;
+        num_threads = 1;
+      },
+    },
   },
 
   submaps = {

configuration_files/trajectory_builder_3d.lua

@@ -64,10 +64,27 @@ TRAJECTORY_BUILDER_3D = {
   -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
   imu_gravity_time_constant = 10.,
   pose_extrapolator = {
+    use_imu_based = false,
     constant_velocity = {
       imu_gravity_time_constant = 10.,
       pose_queue_duration = 0.001,
     },
+    -- TODO(wohe): Tune these parameters on the example datasets.
+    imu_based = {
+      pose_queue_duration = 5.,
+      gravity_constant = 9.806,
+      pose_translation_weight = 1.,
+      pose_rotation_weight = 1.,
+      imu_acceleration_weight = 1.,
+      imu_rotation_weight = 1.,
+      odometry_translation_weight = 1.,
+      odometry_rotation_weight = 1.,
+      solver_options = {
+        use_nonmonotonic_steps = false;
+        max_num_iterations = 10;
+        num_threads = 1;
+      },
+    },
   },
 
   submaps = {