Move GlobalTrajectoryBuilder to cc file (#923)

* Move GlobalTrajectoryBuilder to cc file.

This allows to instrument file-level static metrics.
Also, it is a cleaner interface.

* two create functions

* drop superfluous typename

cartographer/internal/mapping/global_trajectory_builder.cc

@@ -0,0 +1,155 @@
+/*
+ * Copyright 2018 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/internal/mapping/global_trajectory_builder.h"
+
+#include <memory>
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/mapping/local_slam_result_data.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+template <typename LocalTrajectoryBuilder, typename PoseGraph>
+class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
+ public:
+  // Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no
+  // 'TimedPointCloudData' may be added in that case.
+  GlobalTrajectoryBuilder(
+      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
+      const int trajectory_id, PoseGraph* const pose_graph,
+      const LocalSlamResultCallback& local_slam_result_callback)
+      : trajectory_id_(trajectory_id),
+        pose_graph_(pose_graph),
+        local_trajectory_builder_(std::move(local_trajectory_builder)),
+        local_slam_result_callback_(local_slam_result_callback) {}
+  ~GlobalTrajectoryBuilder() override {}
+
+  GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
+  GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
+
+  void AddSensorData(
+      const std::string& sensor_id,
+      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
+    CHECK(local_trajectory_builder_)
+        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
+    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
+        matching_result = local_trajectory_builder_->AddRangeData(
+            timed_point_cloud_data.time,
+            sensor::TimedRangeData{timed_point_cloud_data.origin,
+                                   timed_point_cloud_data.ranges,
+                                   {}});
+    if (matching_result == nullptr) {
+      // The range data has not been fully accumulated yet.
+      return;
+    }
+    std::unique_ptr<InsertionResult> insertion_result;
+    if (matching_result->insertion_result != nullptr) {
+      auto node_id = pose_graph_->AddNode(
+          matching_result->insertion_result->constant_data, trajectory_id_,
+          matching_result->insertion_result->insertion_submaps);
+      CHECK_EQ(node_id.trajectory_id, trajectory_id_);
+      insertion_result = common::make_unique<InsertionResult>(InsertionResult{
+          node_id, matching_result->insertion_result->constant_data,
+          std::vector<std::shared_ptr<const Submap>>(
+              matching_result->insertion_result->insertion_submaps.begin(),
+              matching_result->insertion_result->insertion_submaps.end())});
+    }
+    if (local_slam_result_callback_) {
+      local_slam_result_callback_(
+          trajectory_id_, matching_result->time, matching_result->local_pose,
+          std::move(matching_result->range_data_in_local),
+          std::move(insertion_result));
+    }
+  }
+
+  void AddSensorData(const std::string& sensor_id,
+                     const sensor::ImuData& imu_data) override {
+    if (local_trajectory_builder_) {
+      local_trajectory_builder_->AddImuData(imu_data);
+    }
+    pose_graph_->AddImuData(trajectory_id_, imu_data);
+  }
+
+  void AddSensorData(const std::string& sensor_id,
+                     const sensor::OdometryData& odometry_data) override {
+    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
+    if (local_trajectory_builder_) {
+      local_trajectory_builder_->AddOdometryData(odometry_data);
+    }
+    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
+  }
+
+  void AddSensorData(
+      const std::string& sensor_id,
+      const sensor::FixedFramePoseData& fixed_frame_pose) override {
+    if (fixed_frame_pose.pose.has_value()) {
+      CHECK(fixed_frame_pose.pose.value().IsValid())
+          << fixed_frame_pose.pose.value();
+    }
+    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
+  }
+
+  void AddSensorData(const std::string& sensor_id,
+                     const sensor::LandmarkData& landmark_data) override {
+    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
+  }
+
+  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
+                                  local_slam_result_data) override {
+    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
+                                         "local_trajectory_builder_ present.";
+    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
+  }
+
+ private:
+  const int trajectory_id_;
+  PoseGraph* const pose_graph_;
+  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
+  LocalSlamResultCallback local_slam_result_callback_;
+};
+
+}  // namespace
+
+std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
+    std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
+        local_trajectory_builder,
+    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
+    const TrajectoryBuilderInterface::LocalSlamResultCallback&
+        local_slam_result_callback) {
+  return common::make_unique<GlobalTrajectoryBuilder<
+      mapping_2d::LocalTrajectoryBuilder, mapping::PoseGraph2D>>(
+      std::move(local_trajectory_builder), trajectory_id, pose_graph,
+      local_slam_result_callback);
+}
+
+std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
+    std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
+        local_trajectory_builder,
+    const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
+    const TrajectoryBuilderInterface::LocalSlamResultCallback&
+        local_slam_result_callback) {
+  return common::make_unique<GlobalTrajectoryBuilder<
+      mapping_3d::LocalTrajectoryBuilder, mapping::PoseGraph3D>>(
+      std::move(local_trajectory_builder), trajectory_id, pose_graph,
+      local_slam_result_callback);
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/internal/mapping/global_trajectory_builder.h

@@ -17,113 +17,31 @@
 #ifndef CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
 #define CARTOGRAPHER_INTERNAL_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
 
-#include "cartographer/mapping/trajectory_builder_interface.h"
+#include <memory>
 
+#include "cartographer/internal/mapping_2d/local_trajectory_builder.h"
+#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
 #include "cartographer/mapping/local_slam_result_data.h"
-#include "glog/logging.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer/mapping_2d/pose_graph_2d.h"
+#include "cartographer/mapping_3d/pose_graph_3d.h"
 
 namespace cartographer {
 namespace mapping {
 
-template <typename LocalTrajectoryBuilder,
+std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
-          typename LocalTrajectoryBuilderOptions, typename PoseGraph>
+    std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
-class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
+        local_trajectory_builder,
- public:
+    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
-  // Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no
+    const TrajectoryBuilderInterface::LocalSlamResultCallback&
-  // 'TimedPointCloudData' may be added in that case.
+        local_slam_result_callback);
-  GlobalTrajectoryBuilder(
-      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
-      const int trajectory_id, PoseGraph* const pose_graph,
-      const LocalSlamResultCallback& local_slam_result_callback)
-      : trajectory_id_(trajectory_id),
-        pose_graph_(pose_graph),
-        local_trajectory_builder_(std::move(local_trajectory_builder)),
-        local_slam_result_callback_(local_slam_result_callback) {}
-  ~GlobalTrajectoryBuilder() override {}
 
-  GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
+std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
-  GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
+    std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
-
+        local_trajectory_builder,
-  void AddSensorData(
+    const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
-      const std::string& sensor_id,
+    const TrajectoryBuilderInterface::LocalSlamResultCallback&
-      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
+        local_slam_result_callback);
-    CHECK(local_trajectory_builder_)
-        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
-    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
-        matching_result = local_trajectory_builder_->AddRangeData(
-            timed_point_cloud_data.time,
-            sensor::TimedRangeData{timed_point_cloud_data.origin,
-                                   timed_point_cloud_data.ranges,
-                                   {}});
-    if (matching_result == nullptr) {
-      // The range data has not been fully accumulated yet.
-      return;
-    }
-    std::unique_ptr<InsertionResult> insertion_result;
-    if (matching_result->insertion_result != nullptr) {
-      auto node_id = pose_graph_->AddNode(
-          matching_result->insertion_result->constant_data, trajectory_id_,
-          matching_result->insertion_result->insertion_submaps);
-      CHECK_EQ(node_id.trajectory_id, trajectory_id_);
-      insertion_result = common::make_unique<InsertionResult>(InsertionResult{
-          node_id, matching_result->insertion_result->constant_data,
-          std::vector<std::shared_ptr<const Submap>>(
-              matching_result->insertion_result->insertion_submaps.begin(),
-              matching_result->insertion_result->insertion_submaps.end())});
-    }
-    if (local_slam_result_callback_) {
-      local_slam_result_callback_(
-          trajectory_id_, matching_result->time, matching_result->local_pose,
-          std::move(matching_result->range_data_in_local),
-          std::move(insertion_result));
-    }
-  }
-
-  void AddSensorData(const std::string& sensor_id,
-                     const sensor::ImuData& imu_data) override {
-    if (local_trajectory_builder_) {
-      local_trajectory_builder_->AddImuData(imu_data);
-    }
-    pose_graph_->AddImuData(trajectory_id_, imu_data);
-  }
-
-  void AddSensorData(const std::string& sensor_id,
-                     const sensor::OdometryData& odometry_data) override {
-    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
-    if (local_trajectory_builder_) {
-      local_trajectory_builder_->AddOdometryData(odometry_data);
-    }
-    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
-  }
-
-  void AddSensorData(
-      const std::string& sensor_id,
-      const sensor::FixedFramePoseData& fixed_frame_pose) override {
-    if (fixed_frame_pose.pose.has_value()) {
-      CHECK(fixed_frame_pose.pose.value().IsValid())
-          << fixed_frame_pose.pose.value();
-    }
-    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
-  }
-
-  void AddSensorData(const std::string& sensor_id,
-                     const sensor::LandmarkData& landmark_data) override {
-    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
-  }
-
-  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
-                                  local_slam_result_data) override {
-    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
-                                         "local_trajectory_builder_ present.";
-    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
-  }
-
- private:
-  const int trajectory_id_;
-  PoseGraph* const pose_graph_;
-  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
-  LocalSlamResultCallback local_slam_result_callback_;
-};
 
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/map_builder.cc

@@ -90,11 +90,9 @@ int MapBuilder::AddTrajectoryBuilder(
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
             sensor_collator_.get(), trajectory_id, expected_sensor_ids,
-            common::make_unique<mapping::GlobalTrajectoryBuilder<
+            CreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder),
-                mapping_3d::LocalTrajectoryBuilder,
+                                            trajectory_id, pose_graph_3d_.get(),
-                mapping_3d::proto::LocalTrajectoryBuilderOptions, PoseGraph3D>>(
+                                            local_slam_result_callback)));
-                std::move(local_trajectory_builder), trajectory_id,
-                pose_graph_3d_.get(), local_slam_result_callback)));
   } else {
     std::unique_ptr<mapping_2d::LocalTrajectoryBuilder>
         local_trajectory_builder;
@@ -106,11 +104,9 @@ int MapBuilder::AddTrajectoryBuilder(
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
             sensor_collator_.get(), trajectory_id, expected_sensor_ids,
-            common::make_unique<mapping::GlobalTrajectoryBuilder<
+            CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder),
-                mapping_2d::LocalTrajectoryBuilder,
+                                            trajectory_id, pose_graph_2d_.get(),
-                mapping_2d::proto::LocalTrajectoryBuilderOptions, PoseGraph2D>>(
+                                            local_slam_result_callback)));
-                std::move(local_trajectory_builder), trajectory_id,
-                pose_graph_2d_.get(), local_slam_result_callback)));
   }
   if (trajectory_options.pure_localization()) {
     constexpr int kSubmapsToKeep = 3;