Enable multiple configuration for multi-trajectory (#248)

cartographer/common/configuration_files_test.cc

@@ -42,5 +42,21 @@ TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) {
   });
 }
 
+TEST(ConfigurationFilesTest, ValidateTrajectoryBuilderOptions) {
+  const string kCode = R"text(
+      include "trajectory_builder.lua"
+      TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false
+      return TRAJECTORY_BUILDER)text";
+  EXPECT_NO_FATAL_FAILURE({
+    auto file_resolver = ::cartographer::common::make_unique<
+        ::cartographer::common::ConfigurationFileResolver>(
+        std::vector<string>{string(::cartographer::common::kSourceDirectory) +
+                            "/configuration_files"});
+    ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
+        kCode, std::move(file_resolver));
+    ::cartographer::mapping::CreateTrajectoryBuilderOptions(
+         &lua_parameter_dictionary);
+    });
+}
 }  // namespace
 }  // namespace cartographer_ros

cartographer/mapping/map_builder.cc

@@ -41,14 +41,8 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
   proto::MapBuilderOptions options;
   options.set_use_trajectory_builder_2d(
       parameter_dictionary->GetBool("use_trajectory_builder_2d"));
-  *options.mutable_trajectory_builder_2d_options() =
-      mapping_2d::CreateLocalTrajectoryBuilderOptions(
-          parameter_dictionary->GetDictionary("trajectory_builder_2d").get());
   options.set_use_trajectory_builder_3d(
       parameter_dictionary->GetBool("use_trajectory_builder_3d"));
-  *options.mutable_trajectory_builder_3d_options() =
-      mapping_3d::CreateLocalTrajectoryBuilderOptions(
-          parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
   options.set_num_background_threads(
       parameter_dictionary->GetNonNegativeInt("num_background_threads"));
   *options.mutable_sparse_pose_graph_options() = CreateSparsePoseGraphOptions(
@@ -58,6 +52,18 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
   return options;
 }
 
+proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::TrajectoryBuilderOptions options;
+  *options.mutable_trajectory_builder_2d_options() =
+      mapping_2d::CreateLocalTrajectoryBuilderOptions(
+          parameter_dictionary->GetDictionary("trajectory_builder_2d").get());
+  *options.mutable_trajectory_builder_3d_options() =
+      mapping_3d::CreateLocalTrajectoryBuilderOptions(
+          parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
+  return options;
+}
+
 MapBuilder::MapBuilder(
     const proto::MapBuilderOptions& options,
     std::deque<TrajectoryNode::ConstantData>* const constant_data)
@@ -77,21 +83,24 @@ MapBuilder::MapBuilder(
 MapBuilder::~MapBuilder() {}
 
 int MapBuilder::AddTrajectoryBuilder(
-    const std::unordered_set<string>& expected_sensor_ids) {
+    const std::unordered_set<string>& expected_sensor_ids,
+    const proto::TrajectoryBuilderOptions& trajectory_options) {
   const int trajectory_id = trajectory_builders_.size();
   if (options_.use_trajectory_builder_3d()) {
+    CHECK(trajectory_options.has_trajectory_builder_3d_options());
     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(),
+                trajectory_options.trajectory_builder_3d_options(),
                 sparse_pose_graph_3d_.get())));
   } else {
+    CHECK(trajectory_options.has_trajectory_builder_2d_options());
     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(),
+                trajectory_options.trajectory_builder_2d_options(),
                 sparse_pose_graph_2d_.get())));
   }
   trajectory_ids_.emplace(trajectory_builders_.back()->submaps(),

cartographer/mapping/map_builder.h

@@ -30,6 +30,7 @@
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_builder.h"
@@ -43,6 +44,8 @@ namespace mapping {
 
 proto::MapBuilderOptions CreateMapBuilderOptions(
     common::LuaParameterDictionary* const parameter_dictionary);
+proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
+    common::LuaParameterDictionary* const parameter_dictionary);
 
 // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
 // and a SparsePoseGraph for loop closure.
@@ -57,7 +60,8 @@ class MapBuilder {
 
   // Create a new trajectory and return its index.
   int AddTrajectoryBuilder(
-      const std::unordered_set<string>& expected_sensor_ids);
+      const std::unordered_set<string>& expected_sensor_ids,
+      const proto::TrajectoryBuilderOptions& trajectory_options);
 
   // Returns the TrajectoryBuilder corresponding to the specified
   // 'trajectory_id'.

cartographer/mapping/proto/map_builder_options.proto

@@ -15,20 +15,14 @@
 syntax = "proto2";
 
 import "cartographer/mapping/proto/sparse_pose_graph_options.proto";
-import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
-import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
 
 package cartographer.mapping.proto;
 
 message MapBuilderOptions {
   optional bool use_trajectory_builder_2d = 1;
-  optional mapping_2d.proto.LocalTrajectoryBuilderOptions
-      trajectory_builder_2d_options = 2;
-  optional bool use_trajectory_builder_3d = 3;
-  optional mapping_3d.proto.LocalTrajectoryBuilderOptions
-      trajectory_builder_3d_options = 4;
+  optional bool use_trajectory_builder_3d = 2;
 
   // Number of threads to use for background computations.
-  optional int32 num_background_threads = 5;
-  optional SparsePoseGraphOptions sparse_pose_graph_options = 6;
+  optional int32 num_background_threads = 3;
+  optional SparsePoseGraphOptions sparse_pose_graph_options = 4;
 }

cartographer/mapping/proto/trajectory_builder_options.proto

@@ -0,0 +1,27 @@
+// Copyright 2016 The Cartographer Authors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+syntax = "proto2";
+
+import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
+import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
+
+package cartographer.mapping.proto;
+
+message TrajectoryBuilderOptions {
+  optional mapping_2d.proto.LocalTrajectoryBuilderOptions
+      trajectory_builder_2d_options = 1;
+  optional mapping_3d.proto.LocalTrajectoryBuilderOptions
+      trajectory_builder_3d_options = 2;
+}

configuration_files/map_builder.lua

@@ -12,15 +12,11 @@
 -- See the License for the specific language governing permissions and
 -- limitations under the License.
 
-include "trajectory_builder_2d.lua"
-include "trajectory_builder_3d.lua"
 include "sparse_pose_graph.lua"
 
 MAP_BUILDER = {
   use_trajectory_builder_2d = false,
-  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
   use_trajectory_builder_3d = false,
-  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
   num_background_threads = 4,
   sparse_pose_graph = SPARSE_POSE_GRAPH,
 }

configuration_files/trajectory_builder.lua

@@ -0,0 +1,21 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "trajectory_builder_2d.lua"
+include "trajectory_builder_3d.lua"
+
+TRAJECTORY_BUILDER = {
+  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
+  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
+}