Adds MapBuilder and cleans up some configurations. (#23)

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

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 <cmath>
+#include <limits>
+#include <memory>
+#include <unordered_map>
+#include <utility>
+
+#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<TrajectoryNode::ConstantData>* 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>(
+        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<mapping_3d::SparsePoseGraph>(
+        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<mapping_3d::GlobalTrajectoryBuilder>(
+            options_.trajectory_builder_3d_options(),
+            sparse_pose_graph_3d_.get()));
+  } else {
+    trajectory_builders_.push_back(
+        common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
+            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

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 <deque>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#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<mapping::TrajectoryNode::ConstantData>* 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<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_;
+  std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
+  mapping::SparsePoseGraph* sparse_pose_graph_;
+
+  std::vector<std::unique_ptr<mapping::GlobalTrajectoryBuilderInterface>>
+      trajectory_builders_;
+  std::unordered_map<const mapping::Submaps*, int> trajectory_ids_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_MAP_BUILDER_H_

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

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;
+}

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,
+}

configuration_files/trajectory_builder.lua → 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.,