Remove PoseEstimate. (#670)

Replaces #620.

Depends on switching `cartographer_ros` to use the new callback API for handling the last scan and pose estimate (googlecartographer/cartographer_ros#594).

cartographer/mapping/collated_trajectory_builder.cc

@@ -46,10 +46,6 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
 
 CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
 
-const PoseEstimate& CollatedTrajectoryBuilder::pose_estimate() const {
-  return wrapped_trajectory_builder_->pose_estimate();
-}
-
 void CollatedTrajectoryBuilder::AddSensorData(
     const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
   sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));

cartographer/mapping/collated_trajectory_builder.h

@@ -49,8 +49,6 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
   CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
       delete;
 
-  const PoseEstimate& pose_estimate() const override;
-
   void AddSensorData(const std::string& sensor_id,
                      std::unique_ptr<sensor::Data> data) override;
 

cartographer/mapping/global_trajectory_builder.h

@@ -42,10 +42,6 @@ class GlobalTrajectoryBuilder
   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::TimedPointCloud& ranges) override {

cartographer/mapping/global_trajectory_builder_interface.h

@@ -49,8 +49,6 @@ class GlobalTrajectoryBuilderInterface {
   GlobalTrajectoryBuilderInterface& operator=(
       const GlobalTrajectoryBuilderInterface&) = delete;
 
-  virtual const PoseEstimate& pose_estimate() const = 0;
-
   virtual void AddRangefinderData(common::Time time,
                                   const Eigen::Vector3f& origin,
                                   const sensor::TimedPointCloud& ranges) = 0;

cartographer/mapping/pose_estimate.h

@@ -1,43 +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_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

@@ -40,16 +40,12 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
 // This interface is used for both 2D and 3D SLAM.
 class TrajectoryBuilder {
  public:
-  using PoseEstimate = mapping::PoseEstimate;
-
   TrajectoryBuilder() {}
   virtual ~TrajectoryBuilder() {}
 
   TrajectoryBuilder(const TrajectoryBuilder&) = delete;
   TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
 
-  virtual const PoseEstimate& pose_estimate() const = 0;
-
   virtual void AddSensorData(const std::string& sensor_id,
                              std::unique_ptr<sensor::Data> data) = 0;
 

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -169,7 +169,6 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   sensor::RangeData range_data_in_local =
       TransformRangeData(gravity_aligned_range_data,
                          transform::Embed3D(pose_estimate_2d->cast<float>()));
-  last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
   std::unique_ptr<InsertionResult> insertion_result =
       InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
                        pose_estimate, gravity_alignment.rotation());
@@ -214,10 +213,6 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
       std::move(insertion_submaps)});
 }
 
-const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
-  return last_pose_estimate_;
-}
-
 void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
   CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
   InitializeExtrapolator(imu_data.time);

cartographer/mapping_2d/local_trajectory_builder.h

@@ -60,8 +60,6 @@ class LocalTrajectoryBuilder {
   LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
   LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
 
-  const mapping::PoseEstimate& pose_estimate() const;
-
   // Returns 'MatchingResult' when range data accumulation completed,
   // otherwise 'nullptr'. Range data must be approximately horizontal
   // for 2D SLAM.
@@ -96,8 +94,6 @@ class LocalTrajectoryBuilder {
   const proto::LocalTrajectoryBuilderOptions options_;
   ActiveSubmaps active_submaps_;
 
-  mapping::PoseEstimate last_pose_estimate_;
-
   mapping_3d::MotionFilter motion_filter_;
   scan_matching::RealTimeCorrelativeScanMatcher
       real_time_correlative_scan_matcher_;

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -173,8 +173,6 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
 
   sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
       filtered_range_data_in_tracking, pose_estimate.cast<float>());
-  last_pose_estimate_ = {time, pose_estimate,
-                         filtered_range_data_in_local.returns};
   std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
       time, filtered_range_data_in_local, filtered_range_data_in_tracking,
       high_resolution_point_cloud_in_tracking,
@@ -194,10 +192,6 @@ void LocalTrajectoryBuilder::AddOdometryData(
   extrapolator_->AddOdometryData(odometry_data);
 }
 
-const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
-  return last_pose_estimate_;
-}
-
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::InsertIntoSubmap(
     const common::Time time,

cartographer/mapping_3d/local_trajectory_builder.h

@@ -65,7 +65,6 @@ class LocalTrajectoryBuilder {
   std::unique_ptr<MatchingResult> AddRangeData(
       common::Time time, const sensor::TimedRangeData& range_data);
   void AddOdometryData(const sensor::OdometryData& odometry_data);
-  const mapping::PoseEstimate& pose_estimate() const;
 
  private:
   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
@@ -83,8 +82,6 @@ class LocalTrajectoryBuilder {
   const proto::LocalTrajectoryBuilderOptions options_;
   ActiveSubmaps active_submaps_;
 
-  mapping::PoseEstimate last_pose_estimate_;
-
   MotionFilter motion_filter_;
   std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>
       real_time_correlative_scan_matcher_;