From fc166fdefa10b6c1f8ddc505af11b188c6c2a254 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Thu, 8 Sep 2016 16:21:25 +0200 Subject: [PATCH] Adds MapBuilder and cleans up some configurations. (#23) --- cartographer/mapping/CMakeLists.txt | 28 ++++ cartographer/mapping/map_builder.cc | 125 ++++++++++++++++++ cartographer/mapping/map_builder.h | 91 +++++++++++++ cartographer/mapping/proto/CMakeLists.txt | 9 ++ .../mapping/proto/map_builder_options.proto | 34 +++++ configuration_files/map_builder.lua | 26 ++++ ..._builder.lua => trajectory_builder_2d.lua} | 2 +- 7 files changed, 314 insertions(+), 1 deletion(-) create mode 100644 cartographer/mapping/map_builder.cc create mode 100644 cartographer/mapping/map_builder.h create mode 100644 cartographer/mapping/proto/map_builder_options.proto create mode 100644 configuration_files/map_builder.lua rename configuration_files/{trajectory_builder.lua => trajectory_builder_2d.lua} (98%) diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index f2bdda6..c3ca6ee 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/cartographer/mapping/CMakeLists.txt @@ -28,6 +28,34 @@ google_library(mapping_global_trajectory_builder_interface sensor_point_cloud ) +google_library(mapping_map_builder + USES_CERES + USES_EIGEN + SRCS + map_builder.cc + HDRS + map_builder.h + DEPENDS + common_lua_parameter_dictionary + common_thread_pool + mapping_2d_global_trajectory_builder + mapping_2d_local_trajectory_builder + mapping_2d_sparse_pose_graph + mapping_2d_submaps + mapping_3d_global_trajectory_builder + mapping_3d_local_trajectory_builder_options + mapping_3d_proto_local_trajectory_builder_options + mapping_3d_sparse_pose_graph + mapping_global_trajectory_builder_interface + mapping_proto_map_builder_options + mapping_sparse_pose_graph + mapping_trajectory_node + sensor_laser + sensor_voxel_filter + transform_rigid_transform + transform_transform +) + google_library(mapping_probability_values USES_CERES SRCS diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc new file mode 100644 index 0000000..1eb04ef --- /dev/null +++ b/cartographer/mapping/map_builder.cc @@ -0,0 +1,125 @@ +/* + * 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 "cartographer/mapping/map_builder.h" + +#include +#include +#include +#include +#include + +#include "cartographer/common/make_unique.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" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/voxel_filter.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "glog/log_severity.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + 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( + parameter_dictionary->GetDictionary("sparse_pose_graph").get()); + CHECK_NE(options.use_trajectory_builder_2d(), + options.use_trajectory_builder_3d()); + return options; +} + +MapBuilder::MapBuilder(const proto::MapBuilderOptions& options, + std::deque* constant_data) + : options_(options), + thread_pool_(options.num_background_threads()) { + if (options.use_trajectory_builder_2d()) { + sparse_pose_graph_2d_ = common::make_unique( + options_.sparse_pose_graph_options(), &thread_pool_, constant_data); + sparse_pose_graph_ = sparse_pose_graph_2d_.get(); + } + if (options.use_trajectory_builder_3d()) { + sparse_pose_graph_3d_ = common::make_unique( + options_.sparse_pose_graph_options(), &thread_pool_, constant_data); + sparse_pose_graph_ = sparse_pose_graph_3d_.get(); + } +} + +MapBuilder::~MapBuilder() {} + +int MapBuilder::AddTrajectoryBuilder() { + if (options_.use_trajectory_builder_3d()) { + trajectory_builders_.push_back( + common::make_unique( + options_.trajectory_builder_3d_options(), + sparse_pose_graph_3d_.get())); + } else { + trajectory_builders_.push_back( + common::make_unique( + options_.trajectory_builder_2d_options(), + 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( + const int trajectory_id) const { + return trajectory_builders_.at(trajectory_id).get(); +} + +GlobalTrajectoryBuilderInterface* MapBuilder::GetTrajectoryBuilder( + const Submaps* trajectory) const { + return trajectory_builders_.at(GetTrajectoryId(trajectory)).get(); +} + +int MapBuilder::GetTrajectoryId(const Submaps* trajectory) const { + const auto trajectory_id = trajectory_ids_.find(trajectory); + CHECK(trajectory_id != trajectory_ids_.end()); + return trajectory_id->second; +} + +proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() { + return ToProto(sparse_pose_graph_->GetConnectedTrajectories(), + trajectory_ids_); +} + +int MapBuilder::num_trajectory_builders() const { + return trajectory_builders_.size(); +} + +SparsePoseGraph* MapBuilder::sparse_pose_graph() { return sparse_pose_graph_; } + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h new file mode 100644 index 0000000..512a594 --- /dev/null +++ b/cartographer/mapping/map_builder.h @@ -0,0 +1,91 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ + +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.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/trajectory_node.h" +#include "cartographer/mapping_2d/local_trajectory_builder.h" +#include "cartographer/mapping_2d/sparse_pose_graph.h" +#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/sparse_pose_graph.h" + +namespace cartographer { +namespace mapping { + +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) +// and a SparsePoseGraph for loop closure. +class MapBuilder { + public: + MapBuilder(const proto::MapBuilderOptions& options, + std::deque* constant_data); + ~MapBuilder(); + + MapBuilder(const MapBuilder&) = delete; + MapBuilder& operator=(const MapBuilder&) = delete; + + // Create a new trajectory and return its index. + int AddTrajectoryBuilder(); + + // Returns the TrajectoryBuilder corresponding to the specified + // 'trajectory_id' or 'trajectory' pointer. + mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder( + int trajectory_id) const; + mapping::GlobalTrajectoryBuilderInterface* GetTrajectoryBuilder( + const mapping::Submaps* trajectory) const; + + // Returns the trajectory ID for 'trajectory'. + int GetTrajectoryId(const mapping::Submaps* trajectory) const; + + // Returns the trajectory connectivity. + proto::TrajectoryConnectivity GetTrajectoryConnectivity(); + + int num_trajectory_builders() const; + + mapping::SparsePoseGraph* sparse_pose_graph(); + + private: + const proto::MapBuilderOptions options_; + common::ThreadPool thread_pool_; + + std::unique_ptr sparse_pose_graph_2d_; + std::unique_ptr sparse_pose_graph_3d_; + mapping::SparsePoseGraph* sparse_pose_graph_; + + std::vector> + trajectory_builders_; + std::unordered_map trajectory_ids_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ diff --git a/cartographer/mapping/proto/CMakeLists.txt b/cartographer/mapping/proto/CMakeLists.txt index 71e79d1..43c99c0 100644 --- a/cartographer/mapping/proto/CMakeLists.txt +++ b/cartographer/mapping/proto/CMakeLists.txt @@ -12,6 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +google_proto_library(mapping_proto_map_builder_options + SRCS + map_builder_options.proto + DEPENDS + mapping_2d_proto_local_trajectory_builder_options + mapping_3d_proto_local_trajectory_builder_options + mapping_proto_sparse_pose_graph_options +) + google_proto_library(mapping_proto_scan_matching_progress SRCS scan_matching_progress.proto diff --git a/cartographer/mapping/proto/map_builder_options.proto b/cartographer/mapping/proto/map_builder_options.proto new file mode 100644 index 0000000..d226ce7 --- /dev/null +++ b/cartographer/mapping/proto/map_builder_options.proto @@ -0,0 +1,34 @@ +// 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/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; + + // Number of threads to use for background computations. + optional int32 num_background_threads = 5; + optional SparsePoseGraphOptions sparse_pose_graph_options = 6; +} diff --git a/configuration_files/map_builder.lua b/configuration_files/map_builder.lua new file mode 100644 index 0000000..666ffdb --- /dev/null +++ b/configuration_files/map_builder.lua @@ -0,0 +1,26 @@ +-- 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" +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_2d.lua similarity index 98% rename from configuration_files/trajectory_builder.lua rename to configuration_files/trajectory_builder_2d.lua index bddab28..35d605d 100644 --- a/configuration_files/trajectory_builder.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -12,7 +12,7 @@ -- See the License for the specific language governing permissions and -- limitations under the License. -TRAJECTORY_BUILDER = { +TRAJECTORY_BUILDER_2D = { expect_imu_data = true, horizontal_laser_min_z = -0.8, horizontal_laser_max_z = 2.,