From de5937856d7e01ec100940043ffb3f01f893f503 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Wed, 17 Jan 2018 21:15:15 +0100
Subject: [PATCH] Collate_by_trajectory option (#828)

Adds an option to create a MapBuilder using TrajectoryCollator
(instead of Collator).

[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
---
 cartographer/mapping/map_builder.cc                 | 13 ++++++++++---
 cartographer/mapping/map_builder.h                  |  6 +++---
 .../mapping/proto/map_builder_options.proto         |  2 ++
 cartographer_grpc/map_builder_server_main.cc        |  4 ++++
 4 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index 4ecb5e1..aa56626 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -28,7 +28,9 @@
 #include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
 #include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
 #include "cartographer/mapping/collated_trajectory_builder.h"
+#include "cartographer/sensor/collator.h"
 #include "cartographer/sensor/range_data.h"
+#include "cartographer/sensor/trajectory_collator.h"
 #include "cartographer/sensor/voxel_filter.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -65,6 +67,11 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
         options_.pose_graph_options(), &thread_pool_);
     pose_graph_ = pose_graph_3d_.get();
   }
+  if (options.collate_by_trajectory()) {
+    sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>();
+  } else {
+    sensor_collator_ = common::make_unique<sensor::Collator>();
+  }
 }
 
 MapBuilder::~MapBuilder() {}
@@ -84,7 +91,7 @@ int MapBuilder::AddTrajectoryBuilder(
     }
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
-            &sensor_collator_, trajectory_id, expected_sensor_ids,
+            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
             common::make_unique<mapping::GlobalTrajectoryBuilder<
                 mapping_3d::LocalTrajectoryBuilder,
                 mapping_3d::proto::LocalTrajectoryBuilderOptions,
@@ -101,7 +108,7 @@ int MapBuilder::AddTrajectoryBuilder(
     }
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
-            &sensor_collator_, trajectory_id, expected_sensor_ids,
+            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
             common::make_unique<mapping::GlobalTrajectoryBuilder<
                 mapping_2d::LocalTrajectoryBuilder,
                 mapping_2d::proto::LocalTrajectoryBuilderOptions,
@@ -137,7 +144,7 @@ TrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
 }
 
 void MapBuilder::FinishTrajectory(const int trajectory_id) {
-  sensor_collator_.FinishTrajectory(trajectory_id);
+  sensor_collator_->FinishTrajectory(trajectory_id);
   pose_graph_->FinishTrajectory(trajectory_id);
 }
 
diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h
index c8343c4..3393cf1 100644
--- a/cartographer/mapping/map_builder.h
+++ b/cartographer/mapping/map_builder.h
@@ -27,7 +27,7 @@
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping_2d/pose_graph.h"
 #include "cartographer/mapping_3d/pose_graph.h"
-#include "cartographer/sensor/collator.h"
+#include "cartographer/sensor/collator_interface.h"
 
 namespace cartographer {
 namespace mapping {
@@ -39,7 +39,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
 // and a PoseGraph for loop closure.
 class MapBuilder : public MapBuilderInterface {
  public:
-  MapBuilder(const proto::MapBuilderOptions& options);
+  explicit MapBuilder(const proto::MapBuilderOptions& options);
   ~MapBuilder() override;
 
   MapBuilder(const MapBuilder&) = delete;
@@ -76,7 +76,7 @@ class MapBuilder : public MapBuilderInterface {
   std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_;
   mapping::PoseGraph* pose_graph_;
 
-  sensor::Collator sensor_collator_;
+  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
   std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
       trajectory_builders_;
 };
diff --git a/cartographer/mapping/proto/map_builder_options.proto b/cartographer/mapping/proto/map_builder_options.proto
index ebf37fb..48f1ef3 100644
--- a/cartographer/mapping/proto/map_builder_options.proto
+++ b/cartographer/mapping/proto/map_builder_options.proto
@@ -25,4 +25,6 @@ message MapBuilderOptions {
   // Number of threads to use for background computations.
   int32 num_background_threads = 3;
   PoseGraphOptions pose_graph_options = 4;
+  // Sort sensor input independently for each trajectory.
+  bool collate_by_trajectory = 5;
 }
diff --git a/cartographer_grpc/map_builder_server_main.cc b/cartographer_grpc/map_builder_server_main.cc
index f30160d..4b6f3cb 100644
--- a/cartographer_grpc/map_builder_server_main.cc
+++ b/cartographer_grpc/map_builder_server_main.cc
@@ -34,6 +34,10 @@ void Run(const std::string& configuration_directory,
   proto::MapBuilderServerOptions map_builder_server_options =
       LoadMapBuilderServerOptions(configuration_directory,
                                   configuration_basename);
+  // TODO(gaschler): Remove this override when parameter is imported from lua
+  // config.
+  map_builder_server_options.mutable_map_builder_options()
+      ->set_collate_by_trajectory(true);
   auto map_builder =
       cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
           map_builder_server_options.map_builder_options());