From 522b37979ae5a19d1813644e026bb6233234be61 Mon Sep 17 00:00:00 2001 From: Yutaka Takaoka Date: Mon, 8 May 2017 02:58:53 -0700 Subject: [PATCH] Enable multiple configuration for multi-trajectory (#248) --- .../common/configuration_files_test.cc | 16 +++++++++++ cartographer/mapping/map_builder.cc | 27 ++++++++++++------- cartographer/mapping/map_builder.h | 6 ++++- .../mapping/proto/map_builder_options.proto | 12 +++------ .../proto/trajectory_builder_options.proto | 27 +++++++++++++++++++ configuration_files/map_builder.lua | 4 --- configuration_files/trajectory_builder.lua | 21 +++++++++++++++ 7 files changed, 90 insertions(+), 23 deletions(-) create mode 100644 cartographer/mapping/proto/trajectory_builder_options.proto create mode 100644 configuration_files/trajectory_builder.lua diff --git a/cartographer/common/configuration_files_test.cc b/cartographer/common/configuration_files_test.cc index dec4683..b35fddf 100644 --- a/cartographer/common/configuration_files_test.cc +++ b/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(::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 diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 4c68b24..f8d5b5a 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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* const constant_data) @@ -77,21 +83,24 @@ MapBuilder::MapBuilder( MapBuilder::~MapBuilder() {} int MapBuilder::AddTrajectoryBuilder( - const std::unordered_set& expected_sensor_ids) { + const std::unordered_set& 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( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique( - 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( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique( - options_.trajectory_builder_2d_options(), + trajectory_options.trajectory_builder_2d_options(), sparse_pose_graph_2d_.get()))); } trajectory_ids_.emplace(trajectory_builders_.back()->submaps(), diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index f2c7e1d..8e29935 100644 --- a/cartographer/mapping/map_builder.h +++ b/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& expected_sensor_ids); + const std::unordered_set& expected_sensor_ids, + const proto::TrajectoryBuilderOptions& trajectory_options); // Returns the TrajectoryBuilder corresponding to the specified // 'trajectory_id'. diff --git a/cartographer/mapping/proto/map_builder_options.proto b/cartographer/mapping/proto/map_builder_options.proto index d226ce7..18a6cac 100644 --- a/cartographer/mapping/proto/map_builder_options.proto +++ b/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; } diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto new file mode 100644 index 0000000..98d50f5 --- /dev/null +++ b/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; +} diff --git a/configuration_files/map_builder.lua b/configuration_files/map_builder.lua index 666ffdb..5ec2c51 100644 --- a/configuration_files/map_builder.lua +++ b/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, } diff --git a/configuration_files/trajectory_builder.lua b/configuration_files/trajectory_builder.lua new file mode 100644 index 0000000..75d91ad --- /dev/null +++ b/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, +}