From e78e2cb5ade11efbe6eb3443c013e882e2c1d890 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 24 Aug 2017 12:04:39 +0200 Subject: [PATCH] Extract PoseEstimate into its own file. (#474) This is to remove the dependency of GlobalTrajectoryBuilderInterface on the TrajectoryBuilder. --- .../mapping/collated_trajectory_builder.cc | 3 +- .../global_trajectory_builder_interface.h | 4 +- cartographer/mapping/pose_estimate.h | 43 +++++++++++++++++++ cartographer/mapping/trajectory_builder.h | 14 +----- .../mapping_2d/global_trajectory_builder.cc | 3 +- .../mapping_2d/global_trajectory_builder.h | 3 +- .../mapping_2d/local_trajectory_builder.cc | 3 +- .../mapping_2d/local_trajectory_builder.h | 10 ++--- .../mapping_3d/global_trajectory_builder.cc | 3 +- .../mapping_3d/global_trajectory_builder.h | 2 +- .../mapping_3d/local_trajectory_builder.cc | 3 +- .../mapping_3d/local_trajectory_builder.h | 10 ++--- 12 files changed, 61 insertions(+), 40 deletions(-) create mode 100644 cartographer/mapping/pose_estimate.h diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index 11c5cbf..2fe3ef2 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/cartographer/mapping/collated_trajectory_builder.cc @@ -46,8 +46,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {} -const TrajectoryBuilder::PoseEstimate& -CollatedTrajectoryBuilder::pose_estimate() const { +const PoseEstimate& CollatedTrajectoryBuilder::pose_estimate() const { return wrapped_trajectory_builder_->pose_estimate(); } diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 0d44971..cd90504 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -23,8 +23,8 @@ #include #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() {} diff --git a/cartographer/mapping/pose_estimate.h b/cartographer/mapping/pose_estimate.h new file mode 100644 index 0000000..802c297 --- /dev/null +++ b/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_ diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 258fc72..0fb6a65 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/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 - // 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; - }; + using PoseEstimate = mapping::PoseEstimate; TrajectoryBuilder() {} virtual ~TrajectoryBuilder() {} diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index a9d8dad..05af4e2 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/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& -GlobalTrajectoryBuilder::pose_estimate() const { +const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_.pose_estimate(); } diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index eee14cc..82ba132 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/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 override; + const mapping::PoseEstimate& pose_estimate() const override; // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately // parallel to the ground plane. diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index d25209b..d4980a9 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -186,8 +186,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( range_data_in_tracking_2d, pose_estimate_2d}); } -const LocalTrajectoryBuilder::PoseEstimate& -LocalTrajectoryBuilder::pose_estimate() const { +const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 15e6251..78048a3 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/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 -// closure. +// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) +// without loop closure. class LocalTrajectoryBuilder { public: - using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; - struct InsertionResult { common::Time time; std::vector> 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 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 diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 0c2d815..dc11c19 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/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& -GlobalTrajectoryBuilder::pose_estimate() const { +const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_.pose_estimate(); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 98c43eb..ce5ab8e 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/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_; diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 55de4de..9718636 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -175,8 +175,7 @@ void LocalTrajectoryBuilder::AddOdometerData( extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose}); } -const LocalTrajectoryBuilder::PoseEstimate& -LocalTrajectoryBuilder::pose_estimate() const { +const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index a9c8a36..e20c0c0 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/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 -// closure. +// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.) +// 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 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