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

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

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<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 {
-    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<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_;
 

cartographer/mapping/map_builder.cc

@@ -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()),

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 {

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

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