From 6715afe70c70753767c1510e0a35955c0af6cc3e Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 4 Nov 2020 14:55:13 +0100 Subject: [PATCH] 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 --- .../internal/global_trajectory_builder.cc | 27 ++++++++++++++----- .../internal/global_trajectory_builder.h | 6 +++-- cartographer/mapping/map_builder.cc | 14 ++++++++-- .../proto/trajectory_builder_options.proto | 3 +++ .../mapping/trajectory_builder_interface.cc | 18 +++++++++++++ 5 files changed, 58 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index 3260f05..f68645f 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -19,7 +19,9 @@ #include #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 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& 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 local_trajectory_builder_; LocalSlamResultCallback local_slam_result_callback_; + absl::optional pose_graph_odometry_motion_filter_; }; } // namespace @@ -135,22 +148,24 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr 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& pose_graph_odometry_motion_filter) { return absl::make_unique< GlobalTrajectoryBuilder>( 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 CreateGlobalTrajectoryBuilder3D( std::unique_ptr 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& pose_graph_odometry_motion_filter) { return absl::make_unique< GlobalTrajectoryBuilder>( 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) { diff --git a/cartographer/mapping/internal/global_trajectory_builder.h b/cartographer/mapping/internal/global_trajectory_builder.h index 754912d..4669436 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.h +++ b/cartographer/mapping/internal/global_trajectory_builder.h @@ -34,13 +34,15 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr 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& pose_graph_odometry_motion_filter); std::unique_ptr CreateGlobalTrajectoryBuilder3D( std::unique_ptr 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& pose_graph_odometry_motion_filter); void GlobalTrajectoryBuilderRegisterMetrics( metrics::FamilyFactory* family_factory); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 7809e6f..208d9e2 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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 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 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(pose_graph_.get()), - local_slam_result_callback))); + local_slam_result_callback, pose_graph_odometry_motion_filter))); } else { std::unique_ptr 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(pose_graph_.get()), - local_slam_result_callback))); + local_slam_result_callback, pose_graph_odometry_motion_filter))); } MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get()); diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index fe20db0..13f0344 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/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 { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 8ed8377..991dcc4 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/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; }