use extrapolator interface (#1730)

This integrates the extrapolator interface into the 3D trajectory builder. The construction is now done within the interface and local trajectory builder 3D just makes use of the interface. No functional changes.

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

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -319,14 +319,15 @@ void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
   if (extrapolator_ != nullptr) {
     return;
   }
-  // We derive velocities from poses which are at least 1 ms apart for numerical
+  CHECK(!options_.pose_extrapolator_options().use_imu_based());
-  // 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;
   // TODO(gaschler): Consider using InitializeWithImu as 3D does.
   extrapolator_ = absl::make_unique<PoseExtrapolator>(
-      ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
+      ::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
-      options_.imu_gravity_time_constant());
+                                              .constant_velocity()
+                                              .pose_queue_duration()),
+      options_.pose_extrapolator_options()
+          .constant_velocity()
+          .imu_gravity_time_constant());
   extrapolator_->AddPose(time, transform::Rigid3d::Identity());
 }
 

cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc

@@ -20,6 +20,7 @@
 #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping/pose_extrapolator_interface.h"
 #include "cartographer/sensor/internal/voxel_filter.h"
 
 namespace cartographer {
@@ -58,6 +59,8 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D(
           parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
   *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions(
       parameter_dictionary->GetDictionary("motion_filter").get());
+  *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions(
+      parameter_dictionary->GetDictionary("pose_extrapolator").get());
   options.set_imu_gravity_time_constant(
       parameter_dictionary->GetDouble("imu_gravity_time_constant"));
   *options.mutable_submaps_options() = CreateSubmapsOptions2D(

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

@@ -108,13 +108,8 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
     extrapolator_->AddImuData(imu_data);
     return;
   }
-  // We derive velocities from poses which are at least 1 ms apart for numerical
+  extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData(
-  // stability. Usually poses known to the extrapolator will be further apart
+      options_.pose_extrapolator_options(), {imu_data});
-  // 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<LocalTrajectoryBuilder3D::MatchingResult>

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

@@ -26,7 +26,7 @@
 #include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/mapping/internal/range_data_collator.h"
-#include "cartographer/mapping/pose_extrapolator.h"
+#include "cartographer/mapping/pose_extrapolator_interface.h"
 #include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h"
 #include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/imu_data.h"
@@ -103,7 +103,7 @@ class LocalTrajectoryBuilder3D {
       real_time_correlative_scan_matcher_;
   std::unique_ptr<scan_matching::CeresScanMatcher3D> ceres_scan_matcher_;
 
-  std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
+  std::unique_ptr<mapping::PoseExtrapolatorInterface> extrapolator_;
 
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;

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

@@ -97,6 +97,13 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
           imu_gravity_time_constant = 1.,
           rotational_histogram_size = 120,
           
+          pose_extrapolator = {
+            constant_velocity = {
+              imu_gravity_time_constant = 10.,
+              pose_queue_duration = 0.001,
+            },
+          },
+          
           submaps = {
             high_resolution = 0.2,
             high_resolution_max_range = 50.,

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

@@ -20,6 +20,7 @@
 #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping/pose_extrapolator_interface.h"
 #include "cartographer/sensor/internal/voxel_filter.h"
 #include "glog/logging.h"
 
@@ -57,6 +58,8 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
           parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
   *options.mutable_motion_filter_options() = CreateMotionFilterOptions(
       parameter_dictionary->GetDictionary("motion_filter").get());
+  *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions(
+      parameter_dictionary->GetDictionary("pose_extrapolator").get());
   options.set_imu_gravity_time_constant(
       parameter_dictionary->GetDouble("imu_gravity_time_constant"));
   options.set_rotational_histogram_size(

cartographer/mapping/pose_extrapolator_interface.cc

@@ -35,7 +35,7 @@ CreateConstantVelocityPoseExtrapolatorOptions(
   return options;
 }
 
-}
+}  // namespace
 
 proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
@@ -51,11 +51,12 @@ PoseExtrapolatorInterface::CreateWithImuData(
     const proto::PoseExtrapolatorOptions& options,
     const std::vector<sensor::ImuData>& imu_data) {
   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()),
+      common::FromSeconds(options.constant_velocity().pose_queue_duration()),
-        options.constant_velocity().imu_gravity_time_constant(),
+      options.constant_velocity().imu_gravity_time_constant(), imu_data.back());
-        imu_data.back());
 }
 
-}
+}  // namespace mapping
-}
+}  // namespace cartographer

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

@@ -17,11 +17,13 @@ syntax = "proto3";
 package cartographer.mapping.proto;
 
 import "cartographer/mapping/proto/motion_filter_options.proto";
+import "cartographer/mapping/proto/pose_extrapolator_options.proto";
 import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
 import "cartographer/mapping/proto/2d/submaps_options_2d.proto";
 import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto";
 import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto";
 
+// NEXT ID: 22
 message LocalTrajectoryBuilderOptions2D {
   // Rangefinder points outside these ranges will be dropped.
   float min_range = 14;
@@ -63,7 +65,10 @@ message LocalTrajectoryBuilderOptions2D {
   // 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;
 
   SubmapsOptions2D submaps_options = 11;
 

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

@@ -18,11 +18,12 @@ package cartographer.mapping.proto;
 
 import "cartographer/mapping/proto/3d/submaps_options_3d.proto";
 import "cartographer/mapping/proto/motion_filter_options.proto";
+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";
 
-// NEXT ID: 18
+// NEXT ID: 19
 message LocalTrajectoryBuilderOptions3D {
   // Rangefinder points outside these ranges will be dropped.
   float min_range = 1;
@@ -57,10 +58,14 @@ message LocalTrajectoryBuilderOptions3D {
   // 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;
 
   // Number of histogram buckets for the rotational scan matcher.
   int32 rotational_histogram_size = 17;
 
+  mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18;
+
   SubmapsOptions3D submaps_options = 8;
 }

configuration_files/trajectory_builder_2d.lua

@@ -59,7 +59,14 @@ TRAJECTORY_BUILDER_2D = {
     max_angle_radians = math.rad(1.),
   },
 
+  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
   imu_gravity_time_constant = 10.,
+  pose_extrapolator = {
+    constant_velocity = {
+      imu_gravity_time_constant = 10.,
+      pose_queue_duration = 0.001,
+    },
+  },
 
   submaps = {
     num_range_data = 90,

configuration_files/trajectory_builder_3d.lua

@@ -59,9 +59,17 @@ TRAJECTORY_BUILDER_3D = {
     max_angle_radians = 0.004,
   },
 
-  imu_gravity_time_constant = 10.,
   rotational_histogram_size = 120,
 
+  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
+  imu_gravity_time_constant = 10.,
+  pose_extrapolator = {
+    constant_velocity = {
+      imu_gravity_time_constant = 10.,
+      pose_queue_duration = 0.001,
+    },
+  },
+
   submaps = {
     high_resolution = 0.10,
     high_resolution_max_range = 20.,