From 31f28b509746a1ea9dec417187acc26e7ca5f5ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 17 Nov 2017 16:47:06 +0100 Subject: [PATCH] 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). --- .../mapping/collated_trajectory_builder.cc | 4 -- .../mapping/collated_trajectory_builder.h | 2 - .../mapping/global_trajectory_builder.h | 4 -- .../global_trajectory_builder_interface.h | 2 - cartographer/mapping/pose_estimate.h | 43 ------------------- cartographer/mapping/trajectory_builder.h | 4 -- .../mapping_2d/local_trajectory_builder.cc | 5 --- .../mapping_2d/local_trajectory_builder.h | 4 -- .../mapping_3d/local_trajectory_builder.cc | 6 --- .../mapping_3d/local_trajectory_builder.h | 3 -- 10 files changed, 77 deletions(-) diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index beecb09..7065aba 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/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 data) { sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data)); diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index 5117781..ed20905 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/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 data) override; diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h index acc206d..b4ec905 100644 --- a/cartographer/mapping/global_trajectory_builder.h +++ b/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 { diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 04e85c1..e8fe26a 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/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; diff --git a/cartographer/mapping/pose_estimate.h b/cartographer/mapping/pose_estimate.h index 802c297..e69de29 100644 --- a/cartographer/mapping/pose_estimate.h +++ b/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_ diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 75d56e3..02ca784 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/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 data) = 0; diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index c42e923..6ca7d67 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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())); - last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns}; std::unique_ptr 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); diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index c2a10ca..66cdfab 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/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_; diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 461911e..984ac04 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/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()); - last_pose_estimate_ = {time, pose_estimate, - filtered_range_data_in_local.returns}; std::unique_ptr 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::InsertIntoSubmap( const common::Time time, diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 6ee3041..2a7b446 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -65,7 +65,6 @@ class LocalTrajectoryBuilder { std::unique_ptr 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 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 real_time_correlative_scan_matcher_;