12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer/mapping/internal/collated_trajectory_builder.cc
View File

@ -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<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> 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<std::string> 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<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

14
cartographer/mapping/internal/collated_trajectory_builder.h
View File

@ -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<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> 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 {
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<sensor::Data> data);
sensor::CollatorInterface* const sensor_collator_;
const bool collate_landmarks_;
const bool collate_fixed_frame_;
const int trajectory_id_;
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;

6
cartographer/mapping/map_builder.cc
View File

@ -111,7 +111,8 @@ int MapBuilder::AddTrajectoryBuilder(
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
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<PoseGraph3D*>(pose_graph_.get()),
@ -126,7 +127,8 @@ int MapBuilder::AddTrajectoryBuilder(
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
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<PoseGraph2D*>(pose_graph_.get()),

3
cartographer/mapping/proto/trajectory_builder_options.proto
View File

@ -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 {

4
cartographer/mapping/trajectory_builder_interface.cc
View File

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

7
configuration_files/trajectory_builder.lua
View File

@ -21,4 +21,11 @@ TRAJECTORY_BUILDER = {
-- 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,
}