Add options to (not)collate fixed-frame/landmarks data. (#1224)

master
Alexander Belyaev 2018-07-02 21:49:00 +02:00 committed by GitHub
parent 0981620d8f
commit 3540996e91
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 46 additions and 11 deletions

View File

@ -29,17 +29,26 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
} // namespace } // namespace
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* const sensor_collator, const int trajectory_id, sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids, const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder) std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator), : sensor_collator_(sensor_collator),
collate_landmarks_(trajectory_options.collate_landmarks()),
collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
trajectory_id_(trajectory_id), trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) { last_logging_time_(std::chrono::steady_clock::now()) {
std::unordered_set<std::string> expected_sensor_id_strings; std::unordered_set<std::string> expected_sensor_id_strings;
for (const auto& sensor_id : expected_sensor_ids) { for (const auto& sensor_id : expected_sensor_ids) {
// Landmark data does not need to be collated. if (sensor_id.type == SensorId::SensorType::LANDMARK &&
if (sensor_id.type == SensorId::SensorType::LANDMARK) continue; !collate_landmarks_) {
continue;
}
if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
!collate_fixed_frame_) {
continue;
}
expected_sensor_id_strings.insert(sensor_id.id); expected_sensor_id_strings.insert(sensor_id.id);
} }
sensor_collator_->AddTrajectory( sensor_collator_->AddTrajectory(
@ -49,8 +58,6 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
}); });
} }
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) { void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data)); sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
} }

View File

@ -41,10 +41,11 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
using SensorId = TrajectoryBuilderInterface::SensorId; using SensorId = TrajectoryBuilderInterface::SensorId;
CollatedTrajectoryBuilder( CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* sensor_collator, int trajectory_id, sensor::CollatorInterface* sensor_collator, int trajectory_id,
const std::set<SensorId>& expected_sensor_ids, const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder); std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder);
~CollatedTrajectoryBuilder() override; ~CollatedTrajectoryBuilder() override {}
CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete;
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
@ -69,11 +70,20 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
void AddSensorData( void AddSensorData(
const std::string& sensor_id, const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override { 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, void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) override { 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); wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
} }
@ -89,6 +99,8 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
std::unique_ptr<sensor::Data> data); std::unique_ptr<sensor::Data> data);
sensor::CollatorInterface* const sensor_collator_; sensor::CollatorInterface* const sensor_collator_;
const bool collate_landmarks_;
const bool collate_fixed_frame_;
const int trajectory_id_; const int trajectory_id_;
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_; std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;

View File

@ -111,7 +111,8 @@ int MapBuilder::AddTrajectoryBuilder(
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get())); DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
trajectory_builders_.push_back( trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>( common::make_unique<CollatedTrajectoryBuilder>(
sensor_collator_.get(), trajectory_id, expected_sensor_ids, trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder3D( CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id, std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph3D*>(pose_graph_.get()), static_cast<PoseGraph3D*>(pose_graph_.get()),
@ -126,7 +127,8 @@ int MapBuilder::AddTrajectoryBuilder(
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())); DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
trajectory_builders_.push_back( trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>( common::make_unique<CollatedTrajectoryBuilder>(
sensor_collator_.get(), trajectory_id, expected_sensor_ids, trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D( CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id, std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()), static_cast<PoseGraph2D*>(pose_graph_.get()),

View File

@ -42,6 +42,9 @@ message TrajectoryBuilderOptions {
int32 max_submaps_to_keep = 1; int32 max_submaps_to_keep = 1;
} }
PureLocalizationTrimmerOptions pure_localization_trimmer = 6; PureLocalizationTrimmerOptions pure_localization_trimmer = 6;
bool collate_fixed_frame = 7;
bool collate_landmarks = 8;
} }
message SensorId { message SensorId {

View File

@ -66,6 +66,10 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
CreateLocalTrajectoryBuilderOptions3D( CreateLocalTrajectoryBuilderOptions3D(
parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary); 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); PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary);
return options; return options;
} }

View File

@ -18,7 +18,14 @@ include "trajectory_builder_3d.lua"
TRAJECTORY_BUILDER = { TRAJECTORY_BUILDER = {
trajectory_builder_2d = TRAJECTORY_BUILDER_2D, trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
trajectory_builder_3d = TRAJECTORY_BUILDER_3D, trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
-- pure_localization_trimmer = { -- pure_localization_trimmer = {
-- max_submaps_to_keep = 3, -- 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,
} }