From 3540996e91586e3ae1dd6024c35eeffb4884443e Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Mon, 2 Jul 2018 21:49:00 +0200 Subject: [PATCH] Add options to (not)collate fixed-frame/landmarks data. (#1224) --- .../internal/collated_trajectory_builder.cc | 15 +++++++++++---- .../internal/collated_trajectory_builder.h | 16 ++++++++++++++-- cartographer/mapping/map_builder.cc | 6 ++++-- .../proto/trajectory_builder_options.proto | 3 +++ .../mapping/trajectory_builder_interface.cc | 4 ++++ configuration_files/trajectory_builder.lua | 13 ++++++++++--- 6 files changed, 46 insertions(+), 11 deletions(-) diff --git a/cartographer/mapping/internal/collated_trajectory_builder.cc b/cartographer/mapping/internal/collated_trajectory_builder.cc index 3d607af..bfb73af 100644 --- a/cartographer/mapping/internal/collated_trajectory_builder.cc +++ b/cartographer/mapping/internal/collated_trajectory_builder.cc @@ -29,17 +29,26 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; } // namespace CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( + const proto::TrajectoryBuilderOptions& trajectory_options, sensor::CollatorInterface* const sensor_collator, const int trajectory_id, const std::set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder) : sensor_collator_(sensor_collator), + collate_landmarks_(trajectory_options.collate_landmarks()), + collate_fixed_frame_(trajectory_options.collate_fixed_frame()), trajectory_id_(trajectory_id), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), last_logging_time_(std::chrono::steady_clock::now()) { std::unordered_set expected_sensor_id_strings; for (const auto& sensor_id : expected_sensor_ids) { - // Landmark data does not need to be collated. - if (sensor_id.type == SensorId::SensorType::LANDMARK) continue; + if (sensor_id.type == SensorId::SensorType::LANDMARK && + !collate_landmarks_) { + continue; + } + if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE && + !collate_fixed_frame_) { + continue; + } expected_sensor_id_strings.insert(sensor_id.id); } sensor_collator_->AddTrajectory( @@ -49,8 +58,6 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( }); } -CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {} - void CollatedTrajectoryBuilder::AddData(std::unique_ptr data) { sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); } diff --git a/cartographer/mapping/internal/collated_trajectory_builder.h b/cartographer/mapping/internal/collated_trajectory_builder.h index e454b0e..0237f75 100644 --- a/cartographer/mapping/internal/collated_trajectory_builder.h +++ b/cartographer/mapping/internal/collated_trajectory_builder.h @@ -41,10 +41,11 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { using SensorId = TrajectoryBuilderInterface::SensorId; CollatedTrajectoryBuilder( + const proto::TrajectoryBuilderOptions& trajectory_options, sensor::CollatorInterface* sensor_collator, int trajectory_id, const std::set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder); - ~CollatedTrajectoryBuilder() override; + ~CollatedTrajectoryBuilder() override {} CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = @@ -69,11 +70,20 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { void AddSensorData( const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) override { - AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); + if (collate_landmarks_) { + AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data)); + return; + } + wrapped_trajectory_builder_->AddSensorData(sensor_id, + fixed_frame_pose_data); } void AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data) override { + if (collate_landmarks_) { + AddData(sensor::MakeDispatchable(sensor_id, landmark_data)); + return; + } wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data); } @@ -89,6 +99,8 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface { std::unique_ptr data); sensor::CollatorInterface* const sensor_collator_; + const bool collate_landmarks_; + const bool collate_fixed_frame_; const int trajectory_id_; std::unique_ptr wrapped_trajectory_builder_; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 9639281..528dbd4 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -111,7 +111,8 @@ int MapBuilder::AddTrajectoryBuilder( DCHECK(dynamic_cast(pose_graph_.get())); trajectory_builders_.push_back( common::make_unique( - sensor_collator_.get(), trajectory_id, expected_sensor_ids, + trajectory_options, sensor_collator_.get(), trajectory_id, + expected_sensor_ids, CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), @@ -126,7 +127,8 @@ int MapBuilder::AddTrajectoryBuilder( DCHECK(dynamic_cast(pose_graph_.get())); trajectory_builders_.push_back( common::make_unique( - sensor_collator_.get(), trajectory_id, expected_sensor_ids, + trajectory_options, sensor_collator_.get(), trajectory_id, + expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 227d905..b996cb3 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -42,6 +42,9 @@ message TrajectoryBuilderOptions { int32 max_submaps_to_keep = 1; } PureLocalizationTrimmerOptions pure_localization_trimmer = 6; + + bool collate_fixed_frame = 7; + bool collate_landmarks = 8; } message SensorId { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 2461347..c8fa31e 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -66,6 +66,10 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary); + options.set_collate_fixed_frame( + parameter_dictionary->GetBool("collate_fixed_frame")); + options.set_collate_landmarks( + parameter_dictionary->GetBool("collate_landmarks")); PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary); return options; } diff --git a/configuration_files/trajectory_builder.lua b/configuration_files/trajectory_builder.lua index 1fdc63c..ecd51d8 100644 --- a/configuration_files/trajectory_builder.lua +++ b/configuration_files/trajectory_builder.lua @@ -18,7 +18,14 @@ include "trajectory_builder_3d.lua" TRAJECTORY_BUILDER = { trajectory_builder_2d = TRAJECTORY_BUILDER_2D, trajectory_builder_3d = TRAJECTORY_BUILDER_3D, --- pure_localization_trimmer = { --- max_submaps_to_keep = 3, --- }, +-- pure_localization_trimmer = { +-- max_submaps_to_keep = 3, +-- }, +-- overlapping_submaps_trimmer_2d = { +-- fresh_submaps_count = 1, +-- min_covered_area = 2, +-- min_added_submaps_count = 5, +-- }, + collate_fixed_frame = true, + collate_landmarks = false, }