Remove the LocalTrajectoryBuilderInterface in 3D. (#371)

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -238,7 +238,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
-  const int num_trimmed_submaps = optimization_problem_.num_trimmed_submaps(trajectory_id);
+  const int num_trimmed_submaps =
+      optimization_problem_.num_trimmed_submaps(trajectory_id);
   const transform::Rigid2d optimized_pose =
       optimization_problem_.submap_data()
           .at(matching_id.trajectory_id)
@@ -461,8 +462,8 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
     const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
-  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
+  return ComputeLocalToGlobalTransform(
-                                       num_trimmed_submaps_at_last_optimization_,
+      optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
       trajectory_id);
 }
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -134,7 +134,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
 
   // Set the starting point.
   // TODO(hrapp): Move ceres data into SubmapData.
-  std::vector<std::vector<std::array<double, 3>>> C_submaps(submap_data_.size());
+  std::vector<std::vector<std::array<double, 3>>> C_submaps(
+      submap_data_.size());
   std::vector<std::vector<std::array<double, 3>>> C_nodes(node_data_.size());
   bool first_submap = true;
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
@@ -221,7 +222,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
     for (size_t submap_data_index = 0;
-         submap_data_index != submap_data_[trajectory_id].size(); ++submap_data_index) {
+         submap_data_index != submap_data_[trajectory_id].size();
+         ++submap_data_index) {
       submap_data_[trajectory_id][submap_data_index].pose =
           ToPose(C_submaps[trajectory_id][submap_data_index]);
     }

cartographer/mapping_3d/global_trajectory_builder.h

@@ -20,7 +20,7 @@
 #include <memory>
 
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_3d/local_trajectory_builder.h"
+#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/sparse_pose_graph.h"
 
@@ -49,7 +49,7 @@ class GlobalTrajectoryBuilder
  private:
   const int trajectory_id_;
   mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
-  std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_;
+  std::unique_ptr<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
 };
 
 }  // namespace mapping_3d

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -22,7 +22,6 @@
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_3d/local_trajectory_builder_interface.h"
 #include "cartographer/mapping_3d/motion_filter.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
@@ -30,30 +29,39 @@
 #include "cartographer/mapping_3d/submaps.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/sensor/voxel_filter.h"
+#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping_3d {
 
 // Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
 // closure.
-class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
+class KalmanLocalTrajectoryBuilder {
  public:
+  using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
+
+  struct InsertionResult {
+    common::Time time;
+    sensor::RangeData range_data_in_tracking;
+    transform::Rigid3d pose_observation;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
+  };
+
   explicit KalmanLocalTrajectoryBuilder(
       const proto::LocalTrajectoryBuilderOptions& options);
-  ~KalmanLocalTrajectoryBuilder() override;
+  ~KalmanLocalTrajectoryBuilder();
 
   KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete;
   KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) =
       delete;
 
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
-                  const Eigen::Vector3d& angular_velocity) override;
+                  const Eigen::Vector3d& angular_velocity);
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges) override;
+      const sensor::PointCloud& ranges);
-  void AddOdometerData(common::Time time,
+  void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
-                       const transform::Rigid3d& pose) override;
+  const PoseEstimate& pose_estimate() const;
-  const PoseEstimate& pose_estimate() const override;
 
  private:
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -17,12 +17,11 @@
 #include "cartographer/mapping_3d/local_trajectory_builder.h"
 
 #include "cartographer/common/make_unique.h"
-#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
 
 namespace cartographer {
 namespace mapping_3d {
 
-std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
+std::unique_ptr<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions&
         local_trajectory_builder_options) {
   return common::make_unique<KalmanLocalTrajectoryBuilder>(

cartographer/mapping_3d/local_trajectory_builder.h

@@ -20,13 +20,13 @@
 #include <memory>
 #include <vector>
 
-#include "cartographer/mapping_3d/local_trajectory_builder_interface.h"
+#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 
 namespace cartographer {
 namespace mapping_3d {
 
-std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
+std::unique_ptr<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions&
         local_trajectory_builder_options);
 

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -1,68 +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_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_
-#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_
-
-#include <memory>
-#include <vector>
-
-#include "cartographer/common/time.h"
-#include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_3d/submaps.h"
-#include "cartographer/sensor/range_data.h"
-#include "cartographer/transform/rigid_transform.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-class LocalTrajectoryBuilderInterface {
- public:
-  using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
-
-  struct InsertionResult {
-    common::Time time;
-    sensor::RangeData range_data_in_tracking;
-    transform::Rigid3d pose_observation;
-    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-  };
-
-  virtual ~LocalTrajectoryBuilderInterface() {}
-
-  LocalTrajectoryBuilderInterface(const LocalTrajectoryBuilderInterface&) =
-      delete;
-  LocalTrajectoryBuilderInterface& operator=(
-      const LocalTrajectoryBuilderInterface&) = delete;
-
-  virtual void AddImuData(common::Time time,
-                          const Eigen::Vector3d& linear_acceleration,
-                          const Eigen::Vector3d& angular_velocity) = 0;
-  virtual std::unique_ptr<InsertionResult> AddRangefinderData(
-      common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges) = 0;
-  virtual void AddOdometerData(common::Time time,
-                               const transform::Rigid3d& pose) = 0;
-
-  virtual const PoseEstimate& pose_estimate() const = 0;
-
- protected:
-  LocalTrajectoryBuilderInterface() {}
-};
-
-}  // namespace mapping_3d
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_