Collate sensor data in the MapBuilder. (#85)

cartographer/mapping/CMakeLists.txt

@@ -68,17 +68,20 @@ google_library(mapping_map_builder
   DEPENDS
     common_lua_parameter_dictionary
     common_make_unique
+    common_port
     common_thread_pool
     mapping_2d_global_trajectory_builder
     mapping_2d_sparse_pose_graph
     mapping_3d_global_trajectory_builder
     mapping_3d_local_trajectory_builder_options
     mapping_3d_sparse_pose_graph
-    mapping_global_trajectory_builder_interface
+    mapping_collated_trajectory_builder
     mapping_proto_map_builder_options
     mapping_sparse_pose_graph
     mapping_submaps
+    mapping_trajectory_builder
     mapping_trajectory_node
+    sensor_collator
     sensor_laser
     sensor_voxel_filter
     transform_rigid_transform

cartographer/mapping/map_builder.cc

@@ -23,6 +23,7 @@
 #include <utility>
 
 #include "cartographer/common/make_unique.h"
+#include "cartographer/mapping/collated_trajectory_builder.h"
 #include "cartographer/mapping_2d/global_trajectory_builder.h"
 #include "cartographer/mapping_3d/global_trajectory_builder.h"
 #include "cartographer/mapping_3d/local_trajectory_builder_options.h"
@@ -57,8 +58,9 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
   return options;
 }
 
-MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
+MapBuilder::MapBuilder(
-                       std::deque<TrajectoryNode::ConstantData>* constant_data)
+    const proto::MapBuilderOptions& options,
+    std::deque<TrajectoryNode::ConstantData>* const constant_data)
     : options_(options), thread_pool_(options.num_background_threads()) {
   if (options.use_trajectory_builder_2d()) {
     sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
@@ -74,35 +76,39 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
 
 MapBuilder::~MapBuilder() {}
 
-int MapBuilder::AddTrajectoryBuilder() {
+int MapBuilder::AddTrajectoryBuilder(
+    const std::unordered_set<string>& expected_sensor_ids) {
+  const int trajectory_id = trajectory_builders_.size();
   if (options_.use_trajectory_builder_3d()) {
     trajectory_builders_.push_back(
+        common::make_unique<CollatedTrajectoryBuilder>(
+            &sensor_collator_, trajectory_id, expected_sensor_ids,
             common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
                 options_.trajectory_builder_3d_options(),
-            sparse_pose_graph_3d_.get()));
+                sparse_pose_graph_3d_.get())));
   } else {
     trajectory_builders_.push_back(
+        common::make_unique<CollatedTrajectoryBuilder>(
+            &sensor_collator_, trajectory_id, expected_sensor_ids,
             common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
                 options_.trajectory_builder_2d_options(),
-            sparse_pose_graph_2d_.get()));
+                sparse_pose_graph_2d_.get())));
   }
-  const int trajectory_id = trajectory_builders_.size() - 1;
   trajectory_ids_.emplace(trajectory_builders_.back()->submaps(),
                           trajectory_id);
   return trajectory_id;
 }
 
-GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
+TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
     const int trajectory_id) const {
   return trajectory_builders_.at(trajectory_id).get();
 }
 
-GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder(
+void MapBuilder::FinishTrajectory(const int trajectory_id) {
-    const Submaps* trajectory) const {
+  sensor_collator_.FinishTrajectory(trajectory_id);
-  return trajectory_builders_.at(GetTrajectoryId(trajectory)).get();
 }
 
-int MapBuilder::GetTrajectoryId(const Submaps* trajectory) const {
+int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const {
   const auto trajectory_id = trajectory_ids_.find(trajectory);
   CHECK(trajectory_id != trajectory_ids_.end());
   return trajectory_id->second;

cartographer/mapping/map_builder.h

@@ -19,19 +19,23 @@
 
 #include <deque>
 #include <memory>
+#include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include "Eigen/Geometry"
 #include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
 #include "cartographer/common/thread_pool.h"
-#include "cartographer/mapping/global_trajectory_builder_interface.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/submaps.h"
+#include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/mapping_2d/sparse_pose_graph.h"
 #include "cartographer/mapping_3d/sparse_pose_graph.h"
+#include "cartographer/sensor/collator.h"
 
 namespace cartographer {
 namespace mapping {
@@ -51,14 +55,16 @@ class MapBuilder {
   MapBuilder& operator=(const MapBuilder&) = delete;
 
   // Create a new trajectory and return its index.
-  int AddTrajectoryBuilder();
+  int AddTrajectoryBuilder(
+      const std::unordered_set<string>& expected_sensor_ids);
 
   // Returns the TrajectoryBuilder corresponding to the specified
-  // 'trajectory_id' or 'trajectory' pointer.
+  // 'trajectory_id'.
-  mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder(
+  mapping::TrajectoryBuilder* GetTrajectoryBuilder(int trajectory_id) const;
-      int trajectory_id) const;
+
-  mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder(
+  // Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,
-      const mapping::Submaps* trajectory) const;
+  // i.e. no further sensor data is expected.
+  void FinishTrajectory(int trajectory_id);
 
   // Returns the trajectory ID for 'trajectory'.
   int GetTrajectoryId(const mapping::Submaps* trajectory) const;
@@ -78,8 +84,8 @@ class MapBuilder {
   std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
   mapping::SparsePoseGraph* sparse_pose_graph_;
 
-  std::vector<std::unique_ptr<mapping::GlobalTrajectoryBuilderInterface>>
+  sensor::Collator sensor_collator_;
-      trajectory_builders_;
+  std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_;
   std::unordered_map<const mapping::Submaps*, int> trajectory_ids_;
 };