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.

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 AddSensorData(const sensor::ImuData& imu_data) = 0;
-  virtual void AddOdometerData(common::Time time,
+  virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0;
-                               const transform::Rigid3d& pose) = 0;
+  virtual void AddSensorData(
-  virtual void AddFixedFramePoseData(
       const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
 };
 

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::DispatchableImuData>(
+    AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
-                                 sensor::ImuData{time, linear_acceleration,
+                                 time, linear_acceleration, angular_velocity}));
-                                                 angular_velocity}));
   }
 
   void AddOdometerData(const string& sensor_id, common::Time time,
                        const transform::Rigid3d& odometer_pose) {
-    AddSensorData(sensor_id,
+    AddSensorData(sensor_id, sensor::MakeDispatchable(
-                  common::make_unique<sensor::DispatchableOdometerData>(
+                                 sensor::OdometryData{time, odometer_pose}));
-                      time, odometer_pose));
   }
 
   void AddFixedFramePoseData(const string& sensor_id, common::Time time,
                              const transform::Rigid3d& fixed_frame_pose) {
     AddSensorData(sensor_id,
-                  common::make_unique<sensor::DispatchableFixedFramePoseData>(
+                  sensor::MakeDispatchable(
-                      time, fixed_frame_pose));
+                      sensor::FixedFramePoseData{time, fixed_frame_pose}));
   }
 };
 

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,
+void GlobalTrajectoryBuilder::AddSensorData(
-                                              const transform::Rigid3d& pose) {
+    const sensor::OdometryData& odometry_data) {
-  local_trajectory_builder_.AddOdometerData(time, pose);
+  local_trajectory_builder_.AddOdometerData(odometry_data);
-  sparse_pose_graph_->AddOdometerData(trajectory_id_,
+  sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
-                                      sensor::OdometryData{time, pose});
 }
 
-void GlobalTrajectoryBuilder::AddFixedFramePoseData(
+void GlobalTrajectoryBuilder::AddSensorData(
     const sensor::FixedFramePoseData& fixed_frame_pose) {
   sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
 }

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 AddSensorData(const sensor::ImuData& imu_data) override;
-  void AddOdometerData(common::Time time,
+  void AddSensorData(const sensor::OdometryData& odometry_data) override;
-                       const transform::Rigid3d& pose) override;
+  void AddSensorData(
-  void AddFixedFramePoseData(
       const sensor::FixedFramePoseData& fixed_frame_pose) override;
 
  private:

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) {

cartographer/mapping_2d/local_trajectory_builder.h

@@ -20,13 +20,16 @@
 #include <memory>
 
 #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<InsertionResult> AddHorizontalRangeData(
       common::Time, const sensor::RangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
-  void AddOdometerData(common::Time time,
+  void AddOdometerData(const sensor::OdometryData& odometry_data);
-                       const transform::Rigid3d& odometer_pose);
 
  private:
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(

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,
+void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
-                                              const transform::Rigid3d& pose) {
+  local_trajectory_builder_.AddImuData(imu_data);
-  local_trajectory_builder_.AddOdometerData(time, pose);
+  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);
 }

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,
+  void AddSensorData(const sensor::ImuData& imu_data) override;
-                       const transform::Rigid3d& pose) override;
+  void AddSensorData(const sensor::OdometryData& odometry_data) override;
-  void AddFixedFramePoseData(
+  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_;
 };
 

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 {

cartographer/mapping_3d/local_trajectory_builder.h

@@ -20,13 +20,15 @@
 #include <memory>
 
 #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<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
       const sensor::PointCloud& ranges);
-  void AddOdometerData(common::Time time,
+  void AddOdometerData(const sensor::OdometryData& odometry_data);
-                       const transform::Rigid3d& odometer_pose);
   const mapping::PoseEstimate& pose_estimate() const;
 
  private:

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),
+  OdometryData sixth{common::FromUniversal(600),
-                                 transform::Rigid3d::Identity());
+                     transform::Rigid3d::Identity()};
 
   std::vector<std::pair<string, common::Time>> received;
   Collator collator;
@@ -73,8 +73,7 @@ TEST(Collator, Ordering) {
   collator.AddSensorData(
       kTrajectoryId, kSensorId[0],
       common::make_unique<DispatchableRangefinderData>(first));
-  collator.AddSensorData(kTrajectoryId, kSensorId[3],
+  collator.AddSensorData(kTrajectoryId, kSensorId[3], MakeDispatchable(sixth));
-                         common::make_unique<DispatchableOdometerData>(sixth));
   collator.AddSensorData(
       kTrajectoryId, kSensorId[0],
       common::make_unique<DispatchableRangefinderData>(fourth));
@@ -84,8 +83,7 @@ TEST(Collator, Ordering) {
   collator.AddSensorData(
       kTrajectoryId, kSensorId[1],
       common::make_unique<DispatchableRangefinderData>(fifth));
-  collator.AddSensorData(kTrajectoryId, kSensorId[2],
+  collator.AddSensorData(kTrajectoryId, kSensorId[2], MakeDispatchable(third));
-                         common::make_unique<DispatchableImuData>(third));
 
   ASSERT_EQ(7, received.size());
   EXPECT_EQ(100, common::ToUniversal(received[4].second));

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 <typename DataType>
+class Dispatchable : public Data {
  public:
-  DispatchableOdometerData(const common::Time time,
+  Dispatchable(const DataType& data) : data_(data) {}
-                           const transform::Rigid3d& odometer_pose)
-      : time_(time), odometer_pose_(odometer_pose) {}
 
-  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 DataType data_;
-  const transform::Rigid3d odometer_pose_;
 };
 
-class DispatchableFixedFramePoseData : public Data {
+template <typename DataType>
- public:
+std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(const DataType& data) {
-  DispatchableFixedFramePoseData(const common::Time time,
+  return common::make_unique<Dispatchable<DataType>>(data);
-                                 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_;
-};
 
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/ordered_multi_queue_test.cc

@@ -44,9 +44,9 @@ class OrderedMultiQueueTest : public ::testing::Test {
   }
 
   std::unique_ptr<Data> MakeImu(const int ordinal) {
-    return common::make_unique<DispatchableImuData>(
+    return MakeDispatchable(sensor::ImuData{common::FromUniversal(ordinal),
-        sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
+                                            Eigen::Vector3d::Zero(),
-                        Eigen::Vector3d::Zero()});
+                                            Eigen::Vector3d::Zero()});
   }
 
   std::vector<std::unique_ptr<Data>> values_;