From 094b5a4d93dc5808ed34188caded3ee6b5461a89 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 28 Aug 2017 15:43:26 +0200 Subject: [PATCH] Template dispatching of sensor data. (#484) This reduces the amount of code that needs to be written when a new type of sensor data is introduced by a tiny bit. Rangefinder data is not yet following this pattern. --- .../global_trajectory_builder_interface.h | 8 ++-- cartographer/mapping/trajectory_builder.h | 14 +++--- .../mapping_2d/global_trajectory_builder.cc | 13 +++-- .../mapping_2d/global_trajectory_builder.h | 8 ++-- .../mapping_2d/local_trajectory_builder.cc | 4 +- .../mapping_2d/local_trajectory_builder.h | 8 ++-- .../mapping_3d/global_trajectory_builder.cc | 18 +++---- .../mapping_3d/global_trajectory_builder.h | 15 +++--- .../mapping_3d/local_trajectory_builder.cc | 4 +- .../mapping_3d/local_trajectory_builder.h | 7 +-- cartographer/sensor/collator_test.cc | 12 ++--- cartographer/sensor/data.h | 48 +++++-------------- .../sensor/ordered_multi_queue_test.cc | 6 +-- 13 files changed, 68 insertions(+), 97 deletions(-) diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 63ccdd3..9cc1ff6 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -27,6 +27,7 @@ #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" @@ -53,10 +54,9 @@ class GlobalTrajectoryBuilderInterface { virtual void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) = 0; - virtual void AddImuData(const sensor::ImuData& imu_data) = 0; - virtual void AddOdometerData(common::Time time, - const transform::Rigid3d& pose) = 0; - virtual void AddFixedFramePoseData( + virtual void AddSensorData(const sensor::ImuData& imu_data) = 0; + virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0; + virtual void AddSensorData( const sensor::FixedFramePoseData& fixed_frame_pose) = 0; }; diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 0b7fb79..c5c5055 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -65,23 +65,21 @@ class TrajectoryBuilder { void AddImuData(const string& sensor_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { - AddSensorData(sensor_id, common::make_unique( - sensor::ImuData{time, linear_acceleration, - angular_velocity})); + AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{ + time, linear_acceleration, angular_velocity})); } void AddOdometerData(const string& sensor_id, common::Time time, const transform::Rigid3d& odometer_pose) { - AddSensorData(sensor_id, - common::make_unique( - time, odometer_pose)); + AddSensorData(sensor_id, sensor::MakeDispatchable( + sensor::OdometryData{time, odometer_pose})); } void AddFixedFramePoseData(const string& sensor_id, common::Time time, const transform::Rigid3d& fixed_frame_pose) { AddSensorData(sensor_id, - common::make_unique( - time, fixed_frame_pose)); + sensor::MakeDispatchable( + sensor::FixedFramePoseData{time, fixed_frame_pose})); } }; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 3d588cc..17bc9ed 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -45,19 +45,18 @@ void GlobalTrajectoryBuilder::AddRangefinderData( insertion_result->insertion_submaps); } -void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { +void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) { local_trajectory_builder_.AddImuData(imu_data); sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); } -void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, - const transform::Rigid3d& pose) { - local_trajectory_builder_.AddOdometerData(time, pose); - sparse_pose_graph_->AddOdometerData(trajectory_id_, - sensor::OdometryData{time, pose}); +void GlobalTrajectoryBuilder::AddSensorData( + const sensor::OdometryData& odometry_data) { + local_trajectory_builder_.AddOdometerData(odometry_data); + sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data); } -void GlobalTrajectoryBuilder::AddFixedFramePoseData( +void GlobalTrajectoryBuilder::AddSensorData( const sensor::FixedFramePoseData& fixed_frame_pose) { sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); } diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index 94ee44d..6005e75 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -20,6 +20,7 @@ #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_2d/local_trajectory_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_2d { @@ -41,10 +42,9 @@ class GlobalTrajectoryBuilder // parallel to the ground plane. void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; - void AddImuData(const sensor::ImuData& imu_data) override; - void AddOdometerData(common::Time time, - const transform::Rigid3d& pose) override; - void AddFixedFramePoseData( + void AddSensorData(const sensor::ImuData& imu_data) override; + void AddSensorData(const sensor::OdometryData& odometry_data) override; + void AddSensorData( const sensor::FixedFramePoseData& fixed_frame_pose) override; private: diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 94da92b..a0648b2 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -203,13 +203,13 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { } void LocalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& odometer_pose) { + const sensor::OdometryData& odometry_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator we cannot add odometry data. LOG(INFO) << "Extrapolator not yet initialized."; return; } - extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose}); + extrapolator_->AddOdometryData(odometry_data); } void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) { diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 50177f9..81cbc5a 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -20,13 +20,16 @@ #include #include "cartographer/common/time.h" -#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping/pose_estimate.h" #include "cartographer/mapping/pose_extrapolator.h" #include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_3d/motion_filter.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" @@ -57,8 +60,7 @@ class LocalTrajectoryBuilder { std::unique_ptr AddHorizontalRangeData( common::Time, const sensor::RangeData& range_data); void AddImuData(const sensor::ImuData& imu_data); - void AddOdometerData(common::Time time, - const transform::Rigid3d& odometer_pose); + void AddOdometerData(const sensor::OdometryData& odometry_data); private: std::unique_ptr AddAccumulatedRangeData( diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index ce9c30a..ae709a4 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -28,11 +28,6 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} -void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { - local_trajectory_builder_.AddImuData(imu_data); - sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); -} - void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { @@ -49,12 +44,17 @@ void GlobalTrajectoryBuilder::AddRangefinderData( insertion_result->insertion_submaps); } -void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, - const transform::Rigid3d& pose) { - local_trajectory_builder_.AddOdometerData(time, pose); +void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) { + local_trajectory_builder_.AddImuData(imu_data); + sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); } -void GlobalTrajectoryBuilder::AddFixedFramePoseData( +void GlobalTrajectoryBuilder::AddSensorData( + const sensor::OdometryData& odometry_data) { + local_trajectory_builder_.AddOdometerData(odometry_data); +} + +void GlobalTrajectoryBuilder::AddSensorData( const sensor::FixedFramePoseData& fixed_frame_pose) { sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index ce042b2..d28389d 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -30,25 +30,24 @@ class GlobalTrajectoryBuilder public: GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, int trajectory_id, - mapping_3d::SparsePoseGraph* sparse_pose_graph); + SparsePoseGraph* sparse_pose_graph); ~GlobalTrajectoryBuilder() override; GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; - void AddImuData(const sensor::ImuData& imu_data) override; + const mapping::PoseEstimate& pose_estimate() const override; + void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; - void AddOdometerData(common::Time time, - const transform::Rigid3d& pose) override; - void AddFixedFramePoseData( + void AddSensorData(const sensor::ImuData& imu_data) override; + void AddSensorData(const sensor::OdometryData& odometry_data) override; + void AddSensorData( const sensor::FixedFramePoseData& fixed_frame_pose) override; - const mapping::PoseEstimate& pose_estimate() const override; - private: const int trajectory_id_; - mapping_3d::SparsePoseGraph* const sparse_pose_graph_; + SparsePoseGraph* const sparse_pose_graph_; LocalTrajectoryBuilder local_trajectory_builder_; }; diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 89857f3..d49a544 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -168,13 +168,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( } void LocalTrajectoryBuilder::AddOdometerData( - const common::Time time, const transform::Rigid3d& odometer_pose) { + const sensor::OdometryData& odometry_data) { if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator we cannot add odometry data. LOG(INFO) << "Extrapolator not yet initialized."; return; } - extrapolator_->AddOdometryData(sensor::OdometryData{time, odometer_pose}); + extrapolator_->AddOdometryData(odometry_data); } const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 338400f..413f120 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -20,13 +20,15 @@ #include #include "cartographer/common/time.h" -#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping/pose_estimate.h" #include "cartographer/mapping/pose_extrapolator.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" #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" @@ -61,8 +63,7 @@ class LocalTrajectoryBuilder { std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges); - void AddOdometerData(common::Time time, - const transform::Rigid3d& odometer_pose); + void AddOdometerData(const sensor::OdometryData& odometry_data); const mapping::PoseEstimate& pose_estimate() const; private: diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index 2592205..7202522 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -38,13 +38,13 @@ TEST(Collator, Ordering) { Eigen::Vector3f::Zero(), {}); DispatchableRangefinderData second(common::FromUniversal(200), Eigen::Vector3f::Zero(), {}); - DispatchableImuData third(ImuData{common::FromUniversal(300)}); + ImuData third{common::FromUniversal(300)}; DispatchableRangefinderData fourth(common::FromUniversal(400), Eigen::Vector3f::Zero(), {}); DispatchableRangefinderData fifth(common::FromUniversal(500), Eigen::Vector3f::Zero(), {}); - DispatchableOdometerData sixth(common::FromUniversal(600), - transform::Rigid3d::Identity()); + OdometryData sixth{common::FromUniversal(600), + transform::Rigid3d::Identity()}; std::vector> received; Collator collator; @@ -73,8 +73,7 @@ TEST(Collator, Ordering) { collator.AddSensorData( kTrajectoryId, kSensorId[0], common::make_unique(first)); - collator.AddSensorData(kTrajectoryId, kSensorId[3], - common::make_unique(sixth)); + collator.AddSensorData(kTrajectoryId, kSensorId[3], MakeDispatchable(sixth)); collator.AddSensorData( kTrajectoryId, kSensorId[0], common::make_unique(fourth)); @@ -84,8 +83,7 @@ TEST(Collator, Ordering) { collator.AddSensorData( kTrajectoryId, kSensorId[1], common::make_unique(fifth)); - collator.AddSensorData(kTrajectoryId, kSensorId[2], - common::make_unique(third)); + collator.AddSensorData(kTrajectoryId, kSensorId[2], MakeDispatchable(third)); ASSERT_EQ(7, received.size()); EXPECT_EQ(100, common::ToUniversal(received[4].second)); diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 16f54d5..985d190 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_DATA_H_ #define CARTOGRAPHER_MAPPING_DATA_H_ +#include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/sensor/fixed_frame_pose_data.h" @@ -37,20 +38,6 @@ class Data { mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0; }; -class DispatchableImuData : public Data { - public: - DispatchableImuData(const ImuData& imu_data) : imu_data_(imu_data) {} - - common::Time GetTime() const override { return imu_data_.time; } - void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const - trajectory_builder) override { - trajectory_builder->AddImuData(imu_data_); - } - - private: - const ImuData imu_data_; -}; - class DispatchableRangefinderData : public Data { public: DispatchableRangefinderData(const common::Time time, @@ -70,38 +57,25 @@ class DispatchableRangefinderData : public Data { const PointCloud ranges_; }; -class DispatchableOdometerData : public Data { +template +class Dispatchable : public Data { public: - DispatchableOdometerData(const common::Time time, - const transform::Rigid3d& odometer_pose) - : time_(time), odometer_pose_(odometer_pose) {} + Dispatchable(const DataType& data) : data_(data) {} - common::Time GetTime() const override { return time_; } + common::Time GetTime() const override { return data_.time; } void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder) override { - trajectory_builder->AddOdometerData(time_, odometer_pose_); + trajectory_builder->AddSensorData(data_); } private: - const common::Time time_; - const transform::Rigid3d odometer_pose_; + const DataType data_; }; -class DispatchableFixedFramePoseData : public Data { - public: - DispatchableFixedFramePoseData(const common::Time time, - const transform::Rigid3d& fixed_frame_pose) - : fixed_frame_pose_data_{time, fixed_frame_pose} {} - - common::Time GetTime() const override { return fixed_frame_pose_data_.time; } - void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const - trajectory_builder) override { - trajectory_builder->AddFixedFramePoseData(fixed_frame_pose_data_); - } - - private: - const FixedFramePoseData fixed_frame_pose_data_; -}; +template +std::unique_ptr> MakeDispatchable(const DataType& data) { + return common::make_unique>(data); +} } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/ordered_multi_queue_test.cc b/cartographer/sensor/ordered_multi_queue_test.cc index d78efe7..efa53e3 100644 --- a/cartographer/sensor/ordered_multi_queue_test.cc +++ b/cartographer/sensor/ordered_multi_queue_test.cc @@ -44,9 +44,9 @@ class OrderedMultiQueueTest : public ::testing::Test { } std::unique_ptr MakeImu(const int ordinal) { - return common::make_unique( - sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero()}); + return MakeDispatchable(sensor::ImuData{common::FromUniversal(ordinal), + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero()}); } std::vector> values_;