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; }