Extract PoseEstimate into its own file. (#474)

This is to remove the dependency of GlobalTrajectoryBuilderInterface
on the TrajectoryBuilder.

cartographer/mapping/collated_trajectory_builder.cc

@@ -46,8 +46,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
 
 CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
 
-const TrajectoryBuilder::PoseEstimate&
+const PoseEstimate& CollatedTrajectoryBuilder::pose_estimate() const {
-CollatedTrajectoryBuilder::pose_estimate() const {
   return wrapped_trajectory_builder_->pose_estimate();
 }
 

cartographer/mapping/global_trajectory_builder_interface.h

@@ -23,8 +23,8 @@
 #include <vector>
 
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/pose_estimate.h"
 #include "cartographer/mapping/submaps.h"
-#include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
@@ -39,8 +39,6 @@ namespace mapping {
 // optimized pose estimates.
 class GlobalTrajectoryBuilderInterface {
  public:
-  using PoseEstimate = TrajectoryBuilder::PoseEstimate;
-
   GlobalTrajectoryBuilderInterface() {}
   virtual ~GlobalTrajectoryBuilderInterface() {}
 

cartographer/mapping/pose_estimate.h

@@ -0,0 +1,43 @@
+/*
+ * 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_POSE_ESTIMATE_H_
+#define CARTOGRAPHER_MAPPING_POSE_ESTIMATE_H_
+
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Represents a newly computed pose. 'pose' is the end-user visualization of
+// orientation and 'point_cloud' is the point cloud, in the local map frame.
+struct PoseEstimate {
+  PoseEstimate() = default;
+  PoseEstimate(common::Time time, const transform::Rigid3d& pose,
+               const sensor::PointCloud& point_cloud)
+      : time(time), pose(pose), point_cloud(point_cloud) {}
+
+  common::Time time = common::Time::min();
+  transform::Rigid3d pose = transform::Rigid3d::Identity();
+  sensor::PointCloud point_cloud;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_POSE_ESTIMATE_H_

cartographer/mapping/trajectory_builder.h

@@ -25,6 +25,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/pose_estimate.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/sensor/data.h"
@@ -40,18 +41,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
 // This interface is used for both 2D and 3D SLAM.
 class TrajectoryBuilder {
  public:
-  // Represents a newly computed pose. 'pose' is the end-user visualization of
+  using PoseEstimate = mapping::PoseEstimate;
-  // orientation and 'point_cloud' is the point cloud, in the local map frame.
-  struct PoseEstimate {
-    PoseEstimate() = default;
-    PoseEstimate(common::Time time, const transform::Rigid3d& pose,
-                 const sensor::PointCloud& point_cloud)
-        : time(time), pose(pose), point_cloud(point_cloud) {}
-
-    common::Time time = common::Time::min();
-    transform::Rigid3d pose = transform::Rigid3d::Identity();
-    sensor::PointCloud point_cloud;
-  };
 
   TrajectoryBuilder() {}
   virtual ~TrajectoryBuilder() {}

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -56,8 +56,7 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
                                       sensor::OdometryData{time, pose});
 }
 
-const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
+const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
-GlobalTrajectoryBuilder::pose_estimate() const {
   return local_trajectory_builder_.pose_estimate();
 }
 

cartographer/mapping_2d/global_trajectory_builder.h

@@ -35,8 +35,7 @@ class GlobalTrajectoryBuilder
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
   GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
 
-  const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
+  const mapping::PoseEstimate& pose_estimate() const override;
-      const override;
 
   // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately
   // parallel to the ground plane.

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -186,8 +186,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       range_data_in_tracking_2d, pose_estimate_2d});
 }
 
-const LocalTrajectoryBuilder::PoseEstimate&
+const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
-LocalTrajectoryBuilder::pose_estimate() const {
   return last_pose_estimate_;
 }
 

cartographer/mapping_2d/local_trajectory_builder.h

@@ -33,12 +33,10 @@
 namespace cartographer {
 namespace mapping_2d {
 
-// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
+// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
-// closure.
+// without loop closure.
 class LocalTrajectoryBuilder {
  public:
-  using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
-
   struct InsertionResult {
     common::Time time;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
@@ -54,7 +52,7 @@ class LocalTrajectoryBuilder {
   LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
   LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
 
-  const PoseEstimate& pose_estimate() const;
+  const mapping::PoseEstimate& pose_estimate() const;
   std::unique_ptr<InsertionResult> AddHorizontalRangeData(
       common::Time, const sensor::RangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
@@ -81,7 +79,7 @@ class LocalTrajectoryBuilder {
   const proto::LocalTrajectoryBuilderOptions options_;
   ActiveSubmaps active_submaps_;
 
-  PoseEstimate last_pose_estimate_;
+  mapping::PoseEstimate last_pose_estimate_;
 
   mapping_3d::MotionFilter motion_filter_;
   scan_matching::RealTimeCorrelativeScanMatcher

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -52,8 +52,7 @@ void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
   local_trajectory_builder_.AddOdometerData(time, pose);
 }
 
-const GlobalTrajectoryBuilder::PoseEstimate&
+const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
-GlobalTrajectoryBuilder::pose_estimate() const {
   return local_trajectory_builder_.pose_estimate();
 }
 

cartographer/mapping_3d/global_trajectory_builder.h

@@ -41,7 +41,7 @@ class GlobalTrajectoryBuilder
                           const sensor::PointCloud& ranges) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
-  const PoseEstimate& pose_estimate() const override;
+  const mapping::PoseEstimate& pose_estimate() const override;
 
  private:
   const int trajectory_id_;

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -175,8 +175,7 @@ void LocalTrajectoryBuilder::AddOdometerData(
   extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose});
 }
 
-const LocalTrajectoryBuilder::PoseEstimate&
+const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
-LocalTrajectoryBuilder::pose_estimate() const {
   return last_pose_estimate_;
 }
 

cartographer/mapping_3d/local_trajectory_builder.h

@@ -34,12 +34,10 @@
 namespace cartographer {
 namespace mapping_3d {
 
-// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
+// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
-// closure.
+// without loop closure.
 class LocalTrajectoryBuilder {
  public:
-  using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
-
   struct InsertionResult {
     common::Time time;
     sensor::RangeData range_data_in_tracking;
@@ -60,7 +58,7 @@ class LocalTrajectoryBuilder {
       const sensor::PointCloud& ranges);
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& odometer_pose);
-  const PoseEstimate& pose_estimate() const;
+  const mapping::PoseEstimate& pose_estimate() const;
 
  private:
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
@@ -73,7 +71,7 @@ class LocalTrajectoryBuilder {
   const proto::LocalTrajectoryBuilderOptions options_;
   ActiveSubmaps active_submaps_;
 
-  PoseEstimate last_pose_estimate_;
+  mapping::PoseEstimate last_pose_estimate_;
 
   MotionFilter motion_filter_;
   std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>