Removes LaserFan from public API. (#136)

cartographer/mapping/collated_trajectory_builder.cc

@@ -88,8 +88,9 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
                                               data->imu.angular_velocity);
       return;
 
-    case sensor::Data::Type::kLaserFan:
+    case sensor::Data::Type::kRangefinder:
-      wrapped_trajectory_builder_->AddLaserFan(data->time, data->laser_fan);
+      wrapped_trajectory_builder_->AddRangefinderData(
+          data->time, data->rangefinder.origin, data->rangefinder.ranges);
       return;
 
     case sensor::Data::Type::kOdometer:

cartographer/mapping/global_trajectory_builder_interface.h

@@ -50,8 +50,9 @@ class GlobalTrajectoryBuilderInterface {
   virtual const Submaps* submaps() const = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
-  virtual void AddLaserFan(common::Time time,
+  virtual void AddRangefinderData(common::Time time,
-                           const sensor::LaserFan& laser_fan) = 0;
+                                  const Eigen::Vector3f& origin,
+                                  const sensor::PointCloud& ranges) = 0;
   virtual void AddImuData(common::Time time,
                           const Eigen::Vector3d& linear_acceleration,
                           const Eigen::Vector3d& angular_velocity) = 0;

cartographer/mapping/trajectory_builder.h

@@ -83,10 +83,12 @@ class TrajectoryBuilder {
   virtual void AddSensorData(const string& sensor_id,
                              std::unique_ptr<sensor::Data> data) = 0;
 
-  void AddLaserFan(const string& sensor_id, common::Time time,
+  void AddRangefinderData(const string& sensor_id, common::Time time,
-                   const sensor::LaserFan& laser_fan) {
+                          const Eigen::Vector3f& origin,
+                          const sensor::PointCloud& ranges) {
     AddSensorData(sensor_id,
-                  common::make_unique<sensor::Data>(time, laser_fan));
+                  common::make_unique<sensor::Data>(
+                      time, sensor::Data::Rangefinder{origin, ranges}));
   }
 
   void AddImuData(const string& sensor_id, common::Time time,

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -32,10 +32,12 @@ const Submaps* GlobalTrajectoryBuilder::submaps() const {
   return local_trajectory_builder_.submaps();
 }
 
-void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
+void GlobalTrajectoryBuilder::AddRangefinderData(
-                                          const sensor::LaserFan& laser_fan) {
+    const common::Time time, const Eigen::Vector3f& origin,
+    const sensor::PointCloud& ranges) {
   std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
-      local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
+      local_trajectory_builder_.AddHorizontalLaserFan(
+          time, sensor::LaserFan{origin, ranges, {}, {}});
   if (insertion_result != nullptr) {
     sparse_pose_graph_->AddScan(
         insertion_result->time, insertion_result->tracking_to_tracking_2d,

cartographer/mapping_2d/global_trajectory_builder.h

@@ -38,10 +38,10 @@ class GlobalTrajectoryBuilder
   const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
       const override;
 
-  // Projects 'laser_fan' to 2D, and therefore should be approximately
+  // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately
-  // horizontal.
+  // parallel to the ground plane.
-  void AddLaserFan(common::Time time,
+  void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
-                   const sensor::LaserFan& laser_fan) override;
+                          const sensor::PointCloud& ranges) override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
   void AddOdometerData(

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -41,10 +41,11 @@ void GlobalTrajectoryBuilder::AddImuData(
   sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
 }
 
-void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
+void GlobalTrajectoryBuilder::AddRangefinderData(
-                                          const sensor::LaserFan& laser_fan) {
+    const common::Time time, const Eigen::Vector3f& origin,
+    const sensor::PointCloud& ranges) {
   auto insertion_result =
-      local_trajectory_builder_->AddLaserFan(time, laser_fan);
+      local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
 
   if (insertion_result == nullptr) {
     return;

cartographer/mapping_3d/global_trajectory_builder.h

@@ -40,8 +40,8 @@ class GlobalTrajectoryBuilder
   const mapping_3d::Submaps* submaps() const override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
-  void AddLaserFan(common::Time time,
+  void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
-                   const sensor::LaserFan& laser_fan) override;
+                          const sensor::PointCloud& ranges) override;
   void AddOdometerData(
       common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) override;

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -70,8 +70,9 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
 }
 
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
-KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time,
+KalmanLocalTrajectoryBuilder::AddRangefinderData(
-                                          const sensor::LaserFan& laser_fan) {
+    const common::Time time, const Eigen::Vector3f& origin,
+    const sensor::PointCloud& ranges) {
   if (!pose_tracker_) {
     LOG(INFO) << "PoseTracker not yet initialized.";
     return nullptr;
@@ -89,7 +90,8 @@ KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time,
   const transform::Rigid3f tracking_delta =
       first_pose_prediction_.inverse() * pose_prediction.cast<float>();
   const sensor::LaserFan laser_fan_in_first_tracking =
-      sensor::TransformLaserFan(laser_fan, tracking_delta);
+      sensor::TransformLaserFan(sensor::LaserFan{origin, ranges, {}, {}},
+                                tracking_delta);
   for (const Eigen::Vector3f& laser_return :
        laser_fan_in_first_tracking.returns) {
     const Eigen::Vector3f delta =

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -48,8 +48,9 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
 
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
-  std::unique_ptr<InsertionResult> AddLaserFan(
+  std::unique_ptr<InsertionResult> AddRangefinderData(
-      common::Time time, const sensor::LaserFan& laser_fan) override;
+      common::Time time, const Eigen::Vector3f& origin,
+      const sensor::PointCloud& ranges) override;
   void AddOdometerData(
       common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) override;

cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc

@@ -256,8 +256,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
     int num_poses = 0;
     for (const TrajectoryNode& node : expected_trajectory) {
       AddLinearOnlyImuObservation(node.time, node.pose);
-      if (local_trajectory_builder_->AddLaserFan(
+      const auto laser_fan = GenerateLaserFan(node.pose);
-              node.time, GenerateLaserFan(node.pose)) != nullptr) {
+      if (local_trajectory_builder_->AddRangefinderData(
+              node.time, laser_fan.origin, laser_fan.returns) != nullptr) {
         const auto pose_estimate = local_trajectory_builder_->pose_estimate();
         EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
         ++num_poses;

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -53,8 +53,9 @@ class LocalTrajectoryBuilderInterface {
   virtual void AddImuData(common::Time time,
                           const Eigen::Vector3d& linear_acceleration,
                           const Eigen::Vector3d& angular_velocity) = 0;
-  virtual std::unique_ptr<InsertionResult> AddLaserFan(
+  virtual std::unique_ptr<InsertionResult> AddRangefinderData(
-      common::Time time, const sensor::LaserFan& laser_fan) = 0;
+      common::Time time, const Eigen::Vector3f& origin,
+      const sensor::PointCloud& ranges) = 0;
   virtual void AddOdometerData(
       common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) = 0;

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -133,15 +133,16 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData(
 }
 
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
-OptimizingLocalTrajectoryBuilder::AddLaserFan(
+OptimizingLocalTrajectoryBuilder::AddRangefinderData(
-    const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
+    const common::Time time, const Eigen::Vector3f& origin,
-  CHECK_GT(laser_fan_in_tracking.returns.size(), 0);
+    const sensor::PointCloud& ranges) {
+  CHECK_GT(ranges.size(), 0);
 
   // TODO(hrapp): Handle misses.
   // TODO(hrapp): Where are NaNs in laser_fan_in_tracking coming from?
   sensor::PointCloud point_cloud;
-  for (const Eigen::Vector3f& laser_return : laser_fan_in_tracking.returns) {
+  for (const Eigen::Vector3f& laser_return : ranges) {
-    const Eigen::Vector3f delta = laser_return - laser_fan_in_tracking.origin;
+    const Eigen::Vector3f delta = laser_return - origin;
     const float range = delta.norm();
     if (range >= options_.laser_min_range()) {
       if (range <= options_.laser_max_range()) {

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -52,10 +52,9 @@ class OptimizingLocalTrajectoryBuilder
 
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
-  std::unique_ptr<InsertionResult> AddLaserFan(
+  std::unique_ptr<InsertionResult> AddRangefinderData(
-      common::Time time,
+      common::Time time, const Eigen::Vector3f& origin,
-      const sensor::LaserFan& laser_fan_in_tracking) override;
+      const sensor::PointCloud& ranges) override;
-
   void AddOdometerData(
       const common::Time time, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) override;

cartographer/sensor/collator_test.cc

@@ -31,12 +31,12 @@ namespace {
 TEST(Collator, Ordering) {
   const std::array<string, 4> kSensorId = {
       {"horizontal_laser", "vertical_laser", "imu", "odometry"}};
-  Data zero(common::FromUniversal(0), sensor::LaserFan{});
+  Data zero(common::FromUniversal(0), sensor::Data::Rangefinder{});
-  Data first(common::FromUniversal(100), sensor::LaserFan{});
+  Data first(common::FromUniversal(100), sensor::Data::Rangefinder{});
-  Data second(common::FromUniversal(200), sensor::LaserFan{});
+  Data second(common::FromUniversal(200), sensor::Data::Rangefinder{});
   Data third(common::FromUniversal(300), Data::Imu{});
-  Data fourth(common::FromUniversal(400), sensor::LaserFan{});
+  Data fourth(common::FromUniversal(400), sensor::Data::Rangefinder{});
-  Data fifth(common::FromUniversal(500), sensor::LaserFan{});
+  Data fifth(common::FromUniversal(500), sensor::Data::Rangefinder{});
   Data sixth(common::FromUniversal(600), Data::Odometer{});
 
   std::vector<std::pair<string, Data>> received;

cartographer/sensor/data.h

@@ -20,6 +20,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/sensor/laser.h"
+#include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
@@ -29,7 +30,17 @@ namespace sensor {
 // filled in. It is only used for time ordering sensor data before passing it
 // on.
 struct Data {
-  enum class Type { kImu, kLaserFan, kOdometer };
+  enum class Type { kImu, kRangefinder, kOdometer };
+
+  struct Imu {
+    Eigen::Vector3d linear_acceleration;
+    Eigen::Vector3d angular_velocity;
+  };
+
+  struct Rangefinder {
+    Eigen::Vector3f origin;
+    PointCloud ranges;
+  };
 
   struct Odometer {
     transform::Rigid3d pose;
@@ -38,17 +49,11 @@ struct Data {
     kalman_filter::PoseCovariance covariance;
   };
 
-  struct Imu {
-    Eigen::Vector3d linear_acceleration;
-    Eigen::Vector3d angular_velocity;
-  };
-
   Data(const common::Time time, const Imu& imu)
       : type(Type::kImu), time(time), imu(imu) {}
 
-  Data(const common::Time time,
+  Data(const common::Time time, const Rangefinder& rangefinder)
-       const ::cartographer::sensor::LaserFan& laser_fan)
+      : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {}
-      : type(Type::kLaserFan), time(time), laser_fan(laser_fan) {}
 
   Data(const common::Time time, const Odometer& odometer)
       : type(Type::kOdometer), time(time), odometer(odometer) {}
@@ -56,7 +61,7 @@ struct Data {
   Type type;
   common::Time time;
   Imu imu;
-  sensor::LaserFan laser_fan;
+  Rangefinder rangefinder;
   Odometer odometer;
 };