Optional motion-filter for adding odometry data to the pose graph. (#1757)

This introduces an option to control the amount of odometry data that
enters the pose graph by motion-filtering it. This is very useful to
bound memory when standing still for longer periods of time.

If the new option is not configured, all odometry data goes unfiltered
into the pose graph as usual.

Signed-off-by: Michael Grupp <grupp@magazino.eu>

cartographer/mapping/internal/global_trajectory_builder.cc

@@ -19,7 +19,9 @@
 #include <memory>
 
 #include "absl/memory/memory.h"
+#include "absl/types/optional.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/metrics/family_factory.h"
 #include "glog/logging.h"
@@ -39,11 +41,13 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
   GlobalTrajectoryBuilder(
       std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
       const int trajectory_id, PoseGraph* const pose_graph,
-      const LocalSlamResultCallback& local_slam_result_callback)
+      const LocalSlamResultCallback& local_slam_result_callback,
+      const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter)
       : trajectory_id_(trajectory_id),
         pose_graph_(pose_graph),
         local_trajectory_builder_(std::move(local_trajectory_builder)),
-        local_slam_result_callback_(local_slam_result_callback) {}
+        local_slam_result_callback_(local_slam_result_callback),
+        pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {}
   ~GlobalTrajectoryBuilder() override {}
 
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
@@ -97,6 +101,14 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
     if (local_trajectory_builder_) {
       local_trajectory_builder_->AddOdometryData(odometry_data);
     }
+    // TODO(MichaelGrupp): Instead of having an optional filter on this level,
+    // odometry could be marginalized between nodes in the pose graph.
+    // Related issue: cartographer-project/cartographer/#1768
+    if (pose_graph_odometry_motion_filter_.has_value() &&
+        pose_graph_odometry_motion_filter_.value().IsSimilar(
+            odometry_data.time, odometry_data.pose)) {
+      return;
+    }
     pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
   }
 
@@ -127,6 +139,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
   PoseGraph* const pose_graph_;
   std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
   LocalSlamResultCallback local_slam_result_callback_;
+  absl::optional<MotionFilter> pose_graph_odometry_motion_filter_;
 };
 
 }  // namespace
@@ -135,22 +148,24 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
     std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
     const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
     const TrajectoryBuilderInterface::LocalSlamResultCallback&
-        local_slam_result_callback) {
+        local_slam_result_callback,
+    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
   return absl::make_unique<
       GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
       std::move(local_trajectory_builder), trajectory_id, pose_graph,
-      local_slam_result_callback);
+      local_slam_result_callback, pose_graph_odometry_motion_filter);
 }
 
 std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
     std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
     const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
     const TrajectoryBuilderInterface::LocalSlamResultCallback&
-        local_slam_result_callback) {
+        local_slam_result_callback,
+    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
   return absl::make_unique<
       GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
       std::move(local_trajectory_builder), trajectory_id, pose_graph,
-      local_slam_result_callback);
+      local_slam_result_callback, pose_graph_odometry_motion_filter);
 }
 
 void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory* factory) {

cartographer/mapping/internal/global_trajectory_builder.h

@@ -34,13 +34,15 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
     std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
     const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
     const TrajectoryBuilderInterface::LocalSlamResultCallback&
-        local_slam_result_callback);
+        local_slam_result_callback,
+    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter);
 
 std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
     std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
     const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
     const TrajectoryBuilderInterface::LocalSlamResultCallback&
-        local_slam_result_callback);
+        local_slam_result_callback,
+    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter);
 
 void GlobalTrajectoryBuilderRegisterMetrics(
     metrics::FamilyFactory* family_factory);

cartographer/mapping/map_builder.cc

@@ -17,6 +17,7 @@
 #include "cartographer/mapping/map_builder.h"
 
 #include "absl/memory/memory.h"
+#include "absl/types/optional.h"
 #include "cartographer/common/time.h"
 #include "cartographer/io/internal/mapping_state_serialization.h"
 #include "cartographer/io/proto_stream.h"
@@ -28,6 +29,7 @@
 #include "cartographer/mapping/internal/3d/pose_graph_3d.h"
 #include "cartographer/mapping/internal/collated_trajectory_builder.h"
 #include "cartographer/mapping/internal/global_trajectory_builder.h"
+#include "cartographer/mapping/internal/motion_filter.h"
 #include "cartographer/sensor/internal/collator.h"
 #include "cartographer/sensor/internal/trajectory_collator.h"
 #include "cartographer/sensor/internal/voxel_filter.h"
@@ -120,6 +122,14 @@ int MapBuilder::AddTrajectoryBuilder(
     const proto::TrajectoryBuilderOptions& trajectory_options,
     LocalSlamResultCallback local_slam_result_callback) {
   const int trajectory_id = trajectory_builders_.size();
+
+  absl::optional<MotionFilter> pose_graph_odometry_motion_filter;
+  if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
+    LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
+    pose_graph_odometry_motion_filter.emplace(
+        MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));
+  }
+
   if (options_.use_trajectory_builder_3d()) {
     std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
     if (trajectory_options.has_trajectory_builder_3d_options()) {
@@ -134,7 +144,7 @@ int MapBuilder::AddTrajectoryBuilder(
         CreateGlobalTrajectoryBuilder3D(
             std::move(local_trajectory_builder), trajectory_id,
             static_cast<PoseGraph3D*>(pose_graph_.get()),
-            local_slam_result_callback)));
+            local_slam_result_callback, pose_graph_odometry_motion_filter)));
   } else {
     std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
     if (trajectory_options.has_trajectory_builder_2d_options()) {
@@ -149,7 +159,7 @@ int MapBuilder::AddTrajectoryBuilder(
         CreateGlobalTrajectoryBuilder2D(
             std::move(local_trajectory_builder), trajectory_id,
             static_cast<PoseGraph2D*>(pose_graph_.get()),
-            local_slam_result_callback)));
+            local_slam_result_callback, pose_graph_odometry_motion_filter)));
   }
   MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                   pose_graph_.get());

cartographer/mapping/proto/trajectory_builder_options.proto

@@ -15,6 +15,7 @@
 syntax = "proto3";
 
 import "cartographer/transform/proto/transform.proto";
+import "cartographer/mapping/proto/motion_filter_options.proto";
 import "cartographer/mapping/proto/local_trajectory_builder_options_2d.proto";
 import "cartographer/mapping/proto/local_trajectory_builder_options_3d.proto";
 
@@ -41,6 +42,8 @@ message TrajectoryBuilderOptions {
 
   bool collate_fixed_frame = 7;
   bool collate_landmarks = 8;
+
+  MotionFilterOptions pose_graph_odometry_motion_filter = 9;
 }
 
 message SensorId {

cartographer/mapping/trajectory_builder_interface.cc

@@ -37,6 +37,23 @@ void PopulatePureLocalizationTrimmerOptions(
       options_dictionary->GetInt("max_submaps_to_keep"));
 }
 
+void PopulatePoseGraphOdometryMotionFilterOptions(
+    proto::TrajectoryBuilderOptions* const trajectory_builder_options,
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  constexpr char kDictionaryKey[] = "pose_graph_odometry_motion_filter";
+  if (!parameter_dictionary->HasKey(kDictionaryKey)) return;
+
+  auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey);
+  auto* options =
+      trajectory_builder_options->mutable_pose_graph_odometry_motion_filter();
+  options->set_max_time_seconds(
+      options_dictionary->GetDouble("max_time_seconds"));
+  options->set_max_distance_meters(
+      options_dictionary->GetDouble("max_distance_meters"));
+  options->set_max_angle_radians(
+      options_dictionary->GetDouble("max_angle_radians"));
+}
+
 }  // namespace
 
 proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
@@ -53,6 +70,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
   options.set_collate_landmarks(
       parameter_dictionary->GetBool("collate_landmarks"));
   PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary);
+  PopulatePoseGraphOdometryMotionFilterOptions(&options, parameter_dictionary);
   return options;
 }