Add TimedPointCloud and TimedRangeData. (#601)

Adds a type TimedPointCloud that holds 4-dimensional vectors where measurement time of individual points are stored in the fourth entry.
Uses TimedRangeData to pass TimedPointCloud of incoming measurements to LocalTrajectoryBuilder.

Fixes #573.

cartographer/mapping/global_trajectory_builder.h

@@ -46,10 +46,10 @@ class GlobalTrajectoryBuilder
 
   void AddRangefinderData(const common::Time time,
                           const Eigen::Vector3f& origin,
-                          const sensor::PointCloud& ranges) override {
+                          const sensor::TimedPointCloud& ranges) override {
     std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
         insertion_result = local_trajectory_builder_.AddRangeData(
-            time, sensor::RangeData{origin, ranges, {}});
+            time, sensor::TimedRangeData{origin, ranges, {}});
     if (insertion_result == nullptr) {
       return;
     }

cartographer/mapping/global_trajectory_builder_interface.h

@@ -53,7 +53,7 @@ class GlobalTrajectoryBuilderInterface {
 
   virtual void AddRangefinderData(common::Time time,
                                   const Eigen::Vector3f& origin,
-                                  const sensor::PointCloud& ranges) = 0;
+                                  const sensor::TimedPointCloud& ranges) = 0;
   virtual void AddSensorData(const sensor::ImuData& imu_data) = 0;
   virtual void AddSensorData(const sensor::OdometryData& odometry_data) = 0;
   virtual void AddSensorData(

cartographer/mapping/trajectory_builder.h

@@ -56,7 +56,7 @@ class TrajectoryBuilder {
 
   void AddRangefinderData(const string& sensor_id, common::Time time,
                           const Eigen::Vector3f& origin,
-                          const sensor::PointCloud& ranges) {
+                          const sensor::TimedPointCloud& ranges) {
     AddSensorData(sensor_id,
                   common::make_unique<sensor::DispatchableRangefinderData>(
                       time, origin, ranges));

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -75,7 +75,7 @@ void LocalTrajectoryBuilder::ScanMatch(
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddRangeData(const common::Time time,
-                                     const sensor::RangeData& range_data) {
+                                     const sensor::TimedRangeData& range_data) {
   // Initialize extrapolator now if we do not ever use an IMU.
   if (!options_.use_imu_data()) {
     InitializeExtrapolator(time);
@@ -93,19 +93,22 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
         sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
   }
 
+  // TODO(gaschler): Take time delta of individual points into account.
   const transform::Rigid3f tracking_delta =
       first_pose_estimate_.inverse() *
       extrapolator_->ExtrapolatePose(time).cast<float>();
-  const sensor::RangeData range_data_in_first_tracking =
+  // TODO(gaschler): Try to move this common behavior with 3D into helper.
-      sensor::TransformRangeData(range_data, tracking_delta);
+  const sensor::TimedRangeData range_data_in_first_tracking =
+      sensor::TransformTimedRangeData(range_data, tracking_delta);
   // Drop any returns below the minimum range and convert returns beyond the
   // maximum range into misses.
-  for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
+  for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) {
-    const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
+    const Eigen::Vector3f delta =
+        hit.head<3>() - range_data_in_first_tracking.origin;
     const float range = delta.norm();
     if (range >= options_.min_range()) {
       if (range <= options_.max_range()) {
-        accumulated_range_data_.returns.push_back(hit);
+        accumulated_range_data_.returns.push_back(hit.head<3>());
       } else {
         accumulated_range_data_.misses.push_back(
             range_data_in_first_tracking.origin +

cartographer/mapping_2d/local_trajectory_builder.h

@@ -38,6 +38,7 @@ namespace mapping_2d {
 
 // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
 // without loop closure.
+// TODO(gaschler): Add test for this class similar to the 3D test.
 class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
@@ -56,7 +57,7 @@ class LocalTrajectoryBuilder {
 
   // Range data must be approximately horizontal for 2D SLAM.
   std::unique_ptr<InsertionResult> AddRangeData(
-      common::Time, const sensor::RangeData& range_data);
+      common::Time, const sensor::TimedRangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
   void AddOdometerData(const sensor::OdometryData& odometry_data);
 

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -60,7 +60,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddRangeData(const common::Time time,
-                                     const sensor::RangeData& range_data) {
+                                     const sensor::TimedRangeData& range_data) {
   if (extrapolator_ == nullptr) {
     // Until we've initialized the extrapolator with our first IMU message, we
     // cannot compute the orientation of the rangefinder.
@@ -73,17 +73,19 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
         sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
   }
 
+  // TODO(gaschler): Take time delta of individual points into account.
   const transform::Rigid3f tracking_delta =
       first_pose_estimate_.inverse() *
       extrapolator_->ExtrapolatePose(time).cast<float>();
-  const sensor::RangeData range_data_in_first_tracking =
+  const sensor::TimedRangeData range_data_in_first_tracking =
-      sensor::TransformRangeData(range_data, tracking_delta);
+      sensor::TransformTimedRangeData(range_data, tracking_delta);
-  for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
+  for (const Eigen::Vector4f& hit : range_data_in_first_tracking.returns) {
-    const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
+    const Eigen::Vector3f delta =
+        hit.head<3>() - range_data_in_first_tracking.origin;
     const float range = delta.norm();
     if (range >= options_.min_range()) {
       if (range <= options_.max_range()) {
-        accumulated_range_data_.returns.push_back(hit);
+        accumulated_range_data_.returns.push_back(hit.head<3>());
       } else {
         // We insert a ray cropped to 'max_range' as a miss for hits beyond the
         // maximum range. This way the free space up to the maximum range will

cartographer/mapping_3d/local_trajectory_builder.h

@@ -54,7 +54,7 @@ class LocalTrajectoryBuilder {
 
   void AddImuData(const sensor::ImuData& imu_data);
   std::unique_ptr<InsertionResult> AddRangeData(
-      common::Time time, const sensor::RangeData& range_data);
+      common::Time time, const sensor::TimedRangeData& range_data);
   void AddOdometerData(const sensor::OdometryData& odometry_data);
   const mapping::PoseEstimate& pose_estimate() const;
 

cartographer/mapping_3d/local_trajectory_builder_test.cc

@@ -170,36 +170,47 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
     return first * (to - from) + from;
   }
 
-  sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) {
+  sensor::TimedRangeData GenerateRangeData(const transform::Rigid3d& pose) {
     // 360 degree rays at 16 angles.
-    sensor::PointCloud directions_in_rangefinder_frame;
+    sensor::TimedPointCloud directions_in_rangefinder_frame;
     for (int r = -8; r != 8; ++r) {
       for (int s = -250; s != 250; ++s) {
-        directions_in_rangefinder_frame.push_back(
+        Eigen::Vector4f first_point;
-            Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
+        first_point << Eigen::AngleAxisf(M_PI * s / 250.,
-            Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
+                                         Eigen::Vector3f::UnitZ()) *
-            Eigen::Vector3f::UnitX());
+                           Eigen::AngleAxisf(M_PI / 12. * r / 8.,
+                                             Eigen::Vector3f::UnitY()) *
+                           Eigen::Vector3f::UnitX(),
+            0.;
+        directions_in_rangefinder_frame.push_back(first_point);
         // Second orthogonal rangefinder.
-        directions_in_rangefinder_frame.push_back(
+        Eigen::Vector4f second_point;
-            Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
+        second_point << Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
-            Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
+                            Eigen::AngleAxisf(M_PI * s / 250.,
-            Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) *
+                                              Eigen::Vector3f::UnitZ()) *
-            Eigen::Vector3f::UnitX());
+                            Eigen::AngleAxisf(M_PI / 12. * r / 8.,
+                                              Eigen::Vector3f::UnitY()) *
+                            Eigen::Vector3f::UnitX(),
+            0.;
+        directions_in_rangefinder_frame.push_back(second_point);
       }
     }
     // We simulate a 30 m edge length box around the origin, also containing
     // 50 cm radius spheres.
-    sensor::PointCloud returns_in_world_frame;
+    sensor::TimedPointCloud returns_in_world_frame;
-    for (const Eigen::Vector3f& direction_in_world_frame :
+    for (const Eigen::Vector4f& direction_in_world_frame :
-         sensor::TransformPointCloud(directions_in_rangefinder_frame,
+         sensor::TransformTimedPointCloud(directions_in_rangefinder_frame,
                                           pose.cast<float>())) {
       const Eigen::Vector3f origin =
           pose.cast<float>() * Eigen::Vector3f::Zero();
-      returns_in_world_frame.push_back(CollideWithBubbles(
+      Eigen::Vector4f return_point;
-          origin, CollideWithBox(origin, direction_in_world_frame)));
+      return_point << CollideWithBubbles(
+          origin, CollideWithBox(origin, direction_in_world_frame.head<3>())),
+          0.;
+      returns_in_world_frame.push_back(return_point);
     }
     return {Eigen::Vector3f::Zero(),
-            sensor::TransformPointCloud(returns_in_world_frame,
+            sensor::TransformTimedPointCloud(returns_in_world_frame,
                                              pose.inverse().cast<float>()),
             {}};
   }
@@ -242,8 +253,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
       const auto range_data = GenerateRangeData(node.pose);
       if (local_trajectory_builder_->AddRangeData(
               node.time,
-              sensor::RangeData{range_data.origin, range_data.returns, {}}) !=
+              sensor::TimedRangeData{
-          nullptr) {
+                  range_data.origin, range_data.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/sensor/compressed_point_cloud.h

@@ -29,7 +29,7 @@ namespace cartographer {
 namespace sensor {
 
 // A compressed representation of a point cloud consisting of a collection of
-// points (Vector3f).
+// points (Vector3f) without time information.
 // Internally, points are grouped by blocks. Each block encodes a bit of meta
 // data (number of points in block, coordinates of the block) and encodes each
 // point with a fixed bit rate in relation to the block.

cartographer/sensor/data.h

@@ -42,7 +42,7 @@ class DispatchableRangefinderData : public Data {
  public:
   DispatchableRangefinderData(const common::Time time,
                               const Eigen::Vector3f& origin,
-                              const PointCloud& ranges)
+                              const TimedPointCloud& ranges)
       : time_(time), origin_(origin), ranges_(ranges) {}
 
   common::Time GetTime() const override { return time_; }
@@ -54,7 +54,7 @@ class DispatchableRangefinderData : public Data {
  private:
   const common::Time time_;
   const Eigen::Vector3f origin_;
-  const PointCloud ranges_;
+  const TimedPointCloud ranges_;
 };
 
 template <typename DataType>

cartographer/sensor/point_cloud.cc

@@ -32,7 +32,20 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
   return result;
 }
 
-PointCloud Crop(const PointCloud& point_cloud, const float min_z,
+TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
+                                         const transform::Rigid3f& transform) {
+  TimedPointCloud result;
+  result.reserve(point_cloud.size());
+  for (const Eigen::Vector4f& point : point_cloud) {
+    Eigen::Vector4f result_point;
+    result_point.head<3>() = transform * point.head<3>();
+    result_point[3] = point[3];
+    result.emplace_back(result_point);
+  }
+  return result;
+}
+
+PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
                           const float max_z) {
   PointCloud cropped_point_cloud;
   for (const auto& point : point_cloud) {
@@ -43,5 +56,16 @@ PointCloud Crop(const PointCloud& point_cloud, const float min_z,
   return cropped_point_cloud;
 }
 
+TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
+                                    const float min_z, const float max_z) {
+  TimedPointCloud cropped_point_cloud;
+  for (const auto& point : point_cloud) {
+    if (min_z <= point.z() && point.z() <= max_z) {
+      cropped_point_cloud.push_back(point);
+    }
+  }
+  return cropped_point_cloud;
+}
+
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/point_cloud.h

@@ -27,25 +27,38 @@
 namespace cartographer {
 namespace sensor {
 
+// Stores 3D positions of points.
+// For 2D points, the third entry is 0.f.
 typedef std::vector<Eigen::Vector3f> PointCloud;
 
-struct PointCloudWithIntensities {
+// Stores 3D positions of points with their measurement time in the fourth
-  PointCloud points;
+// entry. Time is in seconds, increasing and relative to the moment when
-  std::vector<float> intensities;
+// 'points[0]' was acquired. If timing is not available, all fourth entries
+// are 0.f. For 2D points, the third entry is 0.f and the fourth entry is time.
+typedef std::vector<Eigen::Vector4f> TimedPointCloud;
 
-  // For each item in 'points', contains the time delta of when it was acquired
+struct PointCloudWithIntensities {
-  // after points[0], i.e. the first entry is always 0.f. If timing
+  TimedPointCloud points;
-  // information is not available all entries will be 0.f.
+  std::vector<float> intensities;
-  std::vector<float> offset_seconds;
 };
 
 // Transforms 'point_cloud' according to 'transform'.
 PointCloud TransformPointCloud(const PointCloud& point_cloud,
                                const transform::Rigid3f& transform);
 
+// Transforms 'point_cloud' according to 'transform'.
+TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
+                                         const transform::Rigid3f& transform);
+
 // Returns a new point cloud without points that fall outside the region defined
 // by 'min_z' and 'max_z'.
-PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);
+PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z,
+                          float max_z);
+
+// Returns a new point cloud without points that fall outside the region defined
+// by 'min_z' and 'max_z'.
+TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
+                                    float min_z, float max_z);
 
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/point_cloud_test.cc

@@ -37,6 +37,18 @@ TEST(PointCloudTest, TransformPointCloud) {
   EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
 }
 
+TEST(PointCloudTest, TransformTimedPointCloud) {
+  TimedPointCloud point_cloud;
+  point_cloud.emplace_back(0.5f, 0.5f, 1.f, 0.f);
+  point_cloud.emplace_back(3.5f, 0.5f, 42.f, 0.f);
+  const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud(
+      point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
+  EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6);
+  EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
+  EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
+  EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
+}
+
 }  // namespace
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/range_data.cc

@@ -31,10 +31,27 @@ RangeData TransformRangeData(const RangeData& range_data,
   };
 }
 
+TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data,
+                                       const transform::Rigid3f& transform) {
+  return TimedRangeData{
+      transform * range_data.origin,
+      TransformTimedPointCloud(range_data.returns, transform),
+      TransformTimedPointCloud(range_data.misses, transform),
+  };
+}
+
 RangeData CropRangeData(const RangeData& range_data, const float min_z,
                         const float max_z) {
-  return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z),
+  return RangeData{range_data.origin,
-                   Crop(range_data.misses, min_z, max_z)};
+                   CropPointCloud(range_data.returns, min_z, max_z),
+                   CropPointCloud(range_data.misses, min_z, max_z)};
+}
+
+TimedRangeData CropTimedRangeData(const TimedRangeData& range_data,
+                                  const float min_z, const float max_z) {
+  return TimedRangeData{range_data.origin,
+                        CropTimedPointCloud(range_data.returns, min_z, max_z),
+                        CropTimedPointCloud(range_data.misses, min_z, max_z)};
 }
 
 }  // namespace sensor

cartographer/sensor/range_data.h

@@ -35,12 +35,26 @@ struct RangeData {
   PointCloud misses;
 };
 
+// Like 'RangeData', but with 'TimedPointClouds'.
+struct TimedRangeData {
+  Eigen::Vector3f origin;
+  TimedPointCloud returns;
+  TimedPointCloud misses;
+};
+
 RangeData TransformRangeData(const RangeData& range_data,
                              const transform::Rigid3f& transform);
 
+TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data,
+                                       const transform::Rigid3f& transform);
+
 // Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
 RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z);
 
+// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
+TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z,
+                                  float max_z);
+
 }  // namespace sensor
 }  // namespace cartographer