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!";
+  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());
+        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/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;
@@ -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 = {