Unify GlobalTrajectoryBuilder across 2D and 3D (#489)

This PR unifies GlobalTrajectoryBuilder across 2D and 3D by templating
it with the LocalTrajectoryBuilder and the SparsePoseGraph. It thus
includes small changes to interfaces in LocalTrajectoryBuilder and
SparsePoseGraph.

cartographer/mapping/global_trajectory_builder.h

@@ -0,0 +1,83 @@
+/*
+ * Copyright 2017 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_GLOBAL_TRAJECTORY_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
+
+#include "cartographer/mapping/global_trajectory_builder_interface.h"
+
+namespace cartographer {
+namespace mapping {
+
+template <typename LocalTrajectoryBuilder,
+          typename LocalTrajectoryBuilderOptions, typename SparsePoseGraph>
+class GlobalTrajectoryBuilder
+    : public mapping::GlobalTrajectoryBuilderInterface {
+ public:
+  GlobalTrajectoryBuilder(const LocalTrajectoryBuilderOptions& options,
+                          const int trajectory_id,
+                          SparsePoseGraph* const sparse_pose_graph)
+      : trajectory_id_(trajectory_id),
+        sparse_pose_graph_(sparse_pose_graph),
+        local_trajectory_builder_(options) {}
+  ~GlobalTrajectoryBuilder() override {}
+
+  GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
+  GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
+
+  const mapping::PoseEstimate& pose_estimate() const override {
+    return local_trajectory_builder_.pose_estimate();
+  }
+
+  void AddRangefinderData(const common::Time time,
+                          const Eigen::Vector3f& origin,
+                          const sensor::PointCloud& ranges) override {
+    std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
+        insertion_result = local_trajectory_builder_.AddRangeData(
+            time, sensor::RangeData{origin, ranges, {}});
+    if (insertion_result == nullptr) {
+      return;
+    }
+    sparse_pose_graph_->AddScan(
+        insertion_result->constant_data, insertion_result->pose_observation,
+        trajectory_id_, insertion_result->insertion_submaps);
+  }
+
+  void AddSensorData(const sensor::ImuData& imu_data) override {
+    local_trajectory_builder_.AddImuData(imu_data);
+    sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
+  }
+
+  void AddSensorData(const sensor::OdometryData& odometry_data) override {
+    local_trajectory_builder_.AddOdometerData(odometry_data);
+    sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
+  }
+
+  void AddSensorData(
+      const sensor::FixedFramePoseData& fixed_frame_pose) override {
+    sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
+  }
+
+ private:
+  const int trajectory_id_;
+  SparsePoseGraph* const sparse_pose_graph_;
+  LocalTrajectoryBuilder local_trajectory_builder_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_

cartographer/mapping/map_builder.cc

@@ -24,8 +24,9 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/mapping/collated_trajectory_builder.h"
-#include "cartographer/mapping_2d/global_trajectory_builder.h"
+#include "cartographer/mapping/global_trajectory_builder.h"
-#include "cartographer/mapping_3d/global_trajectory_builder.h"
+#include "cartographer/mapping_2d/local_trajectory_builder.h"
+#include "cartographer/mapping_3d/local_trajectory_builder.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/sensor/voxel_filter.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -76,7 +77,10 @@ int MapBuilder::AddTrajectoryBuilder(
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
             &sensor_collator_, trajectory_id, expected_sensor_ids,
-            common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
+            common::make_unique<mapping::GlobalTrajectoryBuilder<
+                mapping_3d::LocalTrajectoryBuilder,
+                mapping_3d::proto::LocalTrajectoryBuilderOptions,
+                mapping_3d::SparsePoseGraph>>(
                 trajectory_options.trajectory_builder_3d_options(),
                 trajectory_id, sparse_pose_graph_3d_.get())));
   } else {
@@ -84,7 +88,10 @@ int MapBuilder::AddTrajectoryBuilder(
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
             &sensor_collator_, trajectory_id, expected_sensor_ids,
-            common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
+            common::make_unique<mapping::GlobalTrajectoryBuilder<
+                mapping_2d::LocalTrajectoryBuilder,
+                mapping_2d::proto::LocalTrajectoryBuilderOptions,
+                mapping_2d::SparsePoseGraph>>(
                 trajectory_options.trajectory_builder_2d_options(),
                 trajectory_id, sparse_pose_graph_2d_.get())));
   }

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -1,66 +0,0 @@
-/*
- * 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_2d/global_trajectory_builder.h"
-
-namespace cartographer {
-namespace mapping_2d {
-
-GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
-    const proto::LocalTrajectoryBuilderOptions& options,
-    const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
-    : trajectory_id_(trajectory_id),
-      sparse_pose_graph_(sparse_pose_graph),
-      local_trajectory_builder_(options) {}
-
-GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
-
-void GlobalTrajectoryBuilder::AddRangefinderData(
-    const common::Time time, const Eigen::Vector3f& origin,
-    const sensor::PointCloud& ranges) {
-  std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
-      local_trajectory_builder_.AddHorizontalRangeData(
-          time, sensor::RangeData{origin, ranges, {}});
-  if (insertion_result == nullptr) {
-    return;
-  }
-  sparse_pose_graph_->AddScan(
-      insertion_result->constant_data, insertion_result->pose_estimate_2d,
-      trajectory_id_, insertion_result->insertion_submaps);
-}
-
-void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
-  local_trajectory_builder_.AddImuData(imu_data);
-  sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
-}
-
-void GlobalTrajectoryBuilder::AddSensorData(
-    const sensor::OdometryData& odometry_data) {
-  local_trajectory_builder_.AddOdometerData(odometry_data);
-  sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
-}
-
-void GlobalTrajectoryBuilder::AddSensorData(
-    const sensor::FixedFramePoseData& fixed_frame_pose) {
-  sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
-}
-
-const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
-  return local_trajectory_builder_.pose_estimate();
-}
-
-}  // namespace mapping_2d
-}  // namespace cartographer

cartographer/mapping_2d/global_trajectory_builder.h

@@ -1,59 +0,0 @@
-/*
- * 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_2D_GLOBAL_TRAJECTORY_BUILDER_H_
-#define CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_
-
-#include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_2d/local_trajectory_builder.h"
-#include "cartographer/mapping_2d/sparse_pose_graph.h"
-#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
-
-namespace cartographer {
-namespace mapping_2d {
-
-class GlobalTrajectoryBuilder
-    : public mapping::GlobalTrajectoryBuilderInterface {
- public:
-  GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
-                          int trajectory_id,
-                          SparsePoseGraph* sparse_pose_graph);
-  ~GlobalTrajectoryBuilder() override;
-
-  GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
-  GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
-
-  const mapping::PoseEstimate& pose_estimate() const override;
-
-  // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately
-  // parallel to the ground plane.
-  void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
-                          const sensor::PointCloud& ranges) override;
-  void AddSensorData(const sensor::ImuData& imu_data) override;
-  void AddSensorData(const sensor::OdometryData& odometry_data) override;
-  void AddSensorData(
-      const sensor::FixedFramePoseData& fixed_frame_pose) override;
-
- private:
-  const int trajectory_id_;
-  SparsePoseGraph* const sparse_pose_graph_;
-  LocalTrajectoryBuilder local_trajectory_builder_;
-};
-
-}  // namespace mapping_2d
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -82,7 +82,7 @@ void LocalTrajectoryBuilder::ScanMatch(
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
-LocalTrajectoryBuilder::AddHorizontalRangeData(
+LocalTrajectoryBuilder::AddRangeData(
     const common::Time time, const sensor::RangeData& range_data) {
   // Initialize extrapolator now if we do not ever use an IMU.
   if (!options_.use_imu_data()) {

cartographer/mapping_2d/local_trajectory_builder.h

@@ -42,7 +42,7 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    transform::Rigid2d pose_estimate_2d;
+    transform::Rigid2d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 
@@ -54,7 +54,9 @@ class LocalTrajectoryBuilder {
   LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
 
   const mapping::PoseEstimate& pose_estimate() const;
-  std::unique_ptr<InsertionResult> AddHorizontalRangeData(
+
+  // Range data must be approximately horizontal for 2D SLAM.
+  std::unique_ptr<InsertionResult> AddRangeData(
       common::Time, const sensor::RangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
   void AddOdometerData(const sensor::OdometryData& odometry_data);

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -1,64 +0,0 @@
-/*
- * 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_3d/global_trajectory_builder.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
-    const proto::LocalTrajectoryBuilderOptions& options,
-    const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
-    : trajectory_id_(trajectory_id),
-      sparse_pose_graph_(sparse_pose_graph),
-      local_trajectory_builder_(options) {}
-
-GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
-
-void GlobalTrajectoryBuilder::AddRangefinderData(
-    const common::Time time, const Eigen::Vector3f& origin,
-    const sensor::PointCloud& ranges) {
-  std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
-      local_trajectory_builder_.AddRangefinderData(time, origin, ranges);
-  if (insertion_result == nullptr) {
-    return;
-  }
-  sparse_pose_graph_->AddScan(
-      insertion_result->constant_data, insertion_result->pose_observation,
-      trajectory_id_, insertion_result->insertion_submaps);
-}
-
-void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
-  local_trajectory_builder_.AddImuData(imu_data);
-  sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
-}
-
-void GlobalTrajectoryBuilder::AddSensorData(
-    const sensor::OdometryData& odometry_data) {
-  local_trajectory_builder_.AddOdometerData(odometry_data);
-}
-
-void GlobalTrajectoryBuilder::AddSensorData(
-    const sensor::FixedFramePoseData& fixed_frame_pose) {
-  sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
-}
-
-const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
-  return local_trajectory_builder_.pose_estimate();
-}
-
-}  // namespace mapping_3d
-}  // namespace cartographer

cartographer/mapping_3d/global_trajectory_builder.h

@@ -1,57 +0,0 @@
-/*
- * 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_3D_GLOBAL_TRAJECTORY_BUILDER_H_
-#define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
-
-#include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_3d/local_trajectory_builder.h"
-#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
-#include "cartographer/mapping_3d/sparse_pose_graph.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-class GlobalTrajectoryBuilder
-    : public mapping::GlobalTrajectoryBuilderInterface {
- public:
-  GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
-                          int trajectory_id,
-                          SparsePoseGraph* sparse_pose_graph);
-  ~GlobalTrajectoryBuilder() override;
-
-  GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
-  GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
-
-  const mapping::PoseEstimate& pose_estimate() const override;
-
-  void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
-                          const sensor::PointCloud& ranges) override;
-  void AddSensorData(const sensor::ImuData& imu_data) override;
-  void AddSensorData(const sensor::OdometryData& odometry_data) override;
-  void AddSensorData(
-      const sensor::FixedFramePoseData& fixed_frame_pose) override;
-
- private:
-  const int trajectory_id_;
-  SparsePoseGraph* const sparse_pose_graph_;
-  LocalTrajectoryBuilder local_trajectory_builder_;
-};
-
-}  // namespace mapping_3d
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -58,9 +58,8 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
-LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
+LocalTrajectoryBuilder::AddRangeData(const common::Time time,
-                                           const Eigen::Vector3f& origin,
+                                     const sensor::RangeData& range_data) {
-                                           const sensor::PointCloud& ranges) {
   if (extrapolator_ == nullptr) {
     // Until we've initialized the extrapolator with our first IMU message, we
     // cannot compute the orientation of the rangefinder.
@@ -77,8 +76,7 @@ LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
       first_pose_estimate_.inverse() *
       extrapolator_->ExtrapolatePose(time).cast<float>();
   const sensor::RangeData range_data_in_first_tracking =
-      sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
+      sensor::TransformRangeData(range_data, tracking_delta);
-                                 tracking_delta);
   for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
     const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
     const float range = delta.norm();

cartographer/mapping_3d/local_trajectory_builder.h

@@ -54,9 +54,8 @@ class LocalTrajectoryBuilder {
   LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
 
   void AddImuData(const sensor::ImuData& imu_data);
-  std::unique_ptr<InsertionResult> AddRangefinderData(
+  std::unique_ptr<InsertionResult> AddRangeData(
-      common::Time time, const Eigen::Vector3f& origin,
+      common::Time time, const sensor::RangeData& range_data);
-      const sensor::PointCloud& ranges);
   void AddOdometerData(const sensor::OdometryData& odometry_data);
   const mapping::PoseEstimate& pose_estimate() const;
 

cartographer/mapping_3d/local_trajectory_builder_test.cc

@@ -239,8 +239,10 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
     for (const TrajectoryNode& node : expected_trajectory) {
       AddLinearOnlyImuObservation(node.time, node.pose);
       const auto range_data = GenerateRangeData(node.pose);
-      if (local_trajectory_builder_->AddRangefinderData(
+      if (local_trajectory_builder_->AddRangeData(
-              node.time, range_data.origin, range_data.returns) != nullptr) {
+              node.time,
+              sensor::RangeData{range_data.origin, range_data.returns, {}}) !=
+          nullptr) {
         const auto pose_estimate = local_trajectory_builder_->pose_estimate();
         EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
         ++num_poses;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -150,6 +150,13 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
   });
 }
 
+void SparsePoseGraph::AddOdometerData(
+    int trajectory_id,
+    const sensor::OdometryData& odometry_data) {
+  // TODO(cschuet): Add support for handling OdometryData in the optimization
+  // problem.
+}
+
 void SparsePoseGraph::AddFixedFramePoseData(
     const int trajectory_id,
     const sensor::FixedFramePoseData& fixed_frame_pose_data) {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -39,6 +39,7 @@
 #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_3d/submaps.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -76,6 +77,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_);
 
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(int trajectory_id,
+                       const sensor::OdometryData& odometry_data);
   void AddFixedFramePoseData(
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data);