Introduce [Timed]RangefinderPoint. (#1357)

cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc

@@ -41,7 +41,10 @@ const std::string kMessage = R"(
       x: 3.f y: 4.f z: 5.f
     }
     point_data {
-      x: 6.f y: 7.f z: 8.f t: 9.f
+      position {
+          x: 6.f y: 7.f z: 8.f
+      }
+      time: 9.f
     }
   })";
 

cartographer/io/min_max_range_filtering_points_processor.cc

@@ -40,7 +40,7 @@ void MinMaxRangeFiteringPointsProcessor::Process(
     std::unique_ptr<PointsBatch> batch) {
   std::unordered_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
-    const float range = (batch->points[i] - batch->origin).norm();
+    const float range = (batch->points[i].position - batch->origin).norm();
     if (!(min_range_ <= range && range <= max_range_)) {
       to_remove.insert(i);
     }

cartographer/io/outlier_removing_points_processor.cc

@@ -81,7 +81,8 @@ PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() {
 void OutlierRemovingPointsProcessor::ProcessInPhaseOne(
     const PointsBatch& batch) {
   for (size_t i = 0; i < batch.points.size(); ++i) {
-    ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits;
+    ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i].position))
+          ->hits;
   }
 }
 
@@ -91,7 +92,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
   // by better ray casting, and also by marking the hits of the current range
   // data to be excluded.
   for (size_t i = 0; i < batch.points.size(); ++i) {
-    const Eigen::Vector3f delta = batch.points[i] - batch.origin;
+    const Eigen::Vector3f delta = batch.points[i].position - batch.origin;
     const float length = delta.norm();
     for (float x = 0; x < length; x += voxel_size_) {
       const Eigen::Array3i index =
@@ -109,7 +110,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
   std::unordered_set<int> to_remove;
   for (size_t i = 0; i < batch->points.size(); ++i) {
     const VoxelData voxel =
-        voxels_.value(voxels_.GetCellIndex(batch->points[i]));
+        voxels_.value(voxels_.GetCellIndex(batch->points[i].position));
     if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
       to_remove.insert(i);
     }

cartographer/io/pcd_writing_points_processor.cc

@@ -119,7 +119,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
     WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
   }
   for (size_t i = 0; i < batch->points.size(); ++i) {
-    WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
+    WriteBinaryPcdPointCoordinate(batch->points[i].position,
+                                  file_writer_.get());
     if (!batch->colors.empty()) {
       WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
                                file_writer_.get());

cartographer/io/ply_writing_points_processor.cc

@@ -136,7 +136,7 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   }
 
   for (size_t i = 0; i < batch->points.size(); ++i) {
-    WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
+    WriteBinaryPlyPointCoordinate(batch->points[i].position, file_.get());
     if (has_colors_) {
       WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
     }

cartographer/io/points_batch.cc

@@ -21,7 +21,7 @@ namespace io {
 
 void RemovePoints(std::unordered_set<int> to_remove, PointsBatch* batch) {
   const int new_num_points = batch->points.size() - to_remove.size();
-  std::vector<Eigen::Vector3f> points;
+  sensor::PointCloud points;
   points.reserve(new_num_points);
   std::vector<float> intensities;
   if (!batch->intensities.empty()) {

cartographer/io/points_batch.h

@@ -17,6 +17,7 @@
 #ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_
 #define CARTOGRAPHER_IO_POINTS_BATCH_H_
 
+#include <cartographer/sensor/point_cloud.h>
 #include <array>
 #include <cstdint>
 #include <unordered_set>
@@ -52,7 +53,7 @@ struct PointsBatch {
   int trajectory_id;
 
   // Geometry of the points in the map frame.
-  std::vector<Eigen::Vector3f> points;
+  sensor::PointCloud points;
 
   // Intensities are optional and may be unspecified. The meaning of these
   // intensity values varies by device. For example, the VLP16 provides values

cartographer/io/probability_grid_points_processor_test.cc

@@ -34,11 +34,11 @@ namespace {
 std::unique_ptr<PointsBatch> CreatePointsBatch() {
   auto points_batch = ::absl::make_unique<PointsBatch>();
   points_batch->origin = Eigen::Vector3f(0, 0, 0);
-  points_batch->points.emplace_back(0.0f, 0.0f, 0.0f);
-  points_batch->points.emplace_back(0.0f, 1.0f, 2.0f);
-  points_batch->points.emplace_back(1.0f, 2.0f, 4.0f);
-  points_batch->points.emplace_back(0.0f, 3.0f, 5.0f);
-  points_batch->points.emplace_back(3.0f, 0.0f, 6.0f);
+  points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
+  points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
+  points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
+  points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
+  points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}});
   return points_batch;
 }
 

cartographer/io/xray_points_processor.cc

@@ -196,9 +196,9 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch,
                                  Aggregation* const aggregation) {
   constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}};
   for (size_t i = 0; i < batch.points.size(); ++i) {
-    const Eigen::Vector3f camera_point = transform_ * batch.points[i];
+    const sensor::RangefinderPoint camera_point = transform_ * batch.points[i];
     const Eigen::Array3i cell_index =
-        aggregation->voxels.GetCellIndex(camera_point);
+        aggregation->voxels.GetCellIndex(camera_point.position);
     *aggregation->voxels.mutable_value(cell_index) = true;
     bounding_box_.extend(cell_index.matrix());
     ColumnData& column_data =

cartographer/io/xyz_writing_points_processor.cc

@@ -48,8 +48,8 @@ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
 }
 
 void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
-  for (const Eigen::Vector3f& point : batch->points) {
-    WriteXyzPoint(point, file_writer_.get());
+  for (const sensor::RangefinderPoint& point : batch->points) {
+    WriteXyzPoint(point.position, file_writer_.get());
   }
   next_->Process(std::move(batch));
 }

cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

@@ -37,11 +37,11 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
   Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
   // Padding around bounding box to avoid numerical issues at cell boundaries.
   constexpr float kPadding = 1e-6f;
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    bounding_box.extend(hit.head<2>());
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
+    bounding_box.extend(hit.position.head<2>());
   }
-  for (const Eigen::Vector3f& miss : range_data.misses) {
-    bounding_box.extend(miss.head<2>());
+  for (const sensor::RangefinderPoint& miss : range_data.misses) {
+    bounding_box.extend(miss.position.head<2>());
   }
   probability_grid->GrowLimits(bounding_box.min() -
                                kPadding * Eigen::Vector2f::Ones());
@@ -66,8 +66,8 @@ void CastRays(const sensor::RangeData& range_data,
   // Compute and add the end points.
   std::vector<Eigen::Array2i> ends;
   ends.reserve(range_data.returns.size());
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
+    ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
     probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
   }
 
@@ -85,9 +85,9 @@ void CastRays(const sensor::RangeData& range_data,
   }
 
   // Finally, compute and add empty rays based on misses in the range data.
-  for (const Eigen::Vector3f& missing_echo : range_data.misses) {
+  for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
     std::vector<Eigen::Array2i> ray = RayToPixelMask(
-        begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
+        begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
         kSubpixelScale);
     for (const Eigen::Array2i& cell_index : ray) {
       probability_grid->ApplyLookupTable(cell_index, miss_table);

cartographer/mapping/2d/range_data_inserter_2d_test.cc

@@ -49,10 +49,10 @@ class RangeDataInserterTest2D : public ::testing::Test {
 
   void InsertPointCloud() {
     sensor::RangeData range_data;
-    range_data.returns.emplace_back(-3.5f, 0.5f, 0.f);
-    range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
-    range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
-    range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
+    range_data.returns.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}});
+    range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}});
+    range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}});
+    range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
     range_data.origin.x() = -0.5f;
     range_data.origin.y() = 0.5f;
     range_data_inserter_->Insert(range_data, &probability_grid_);

cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc

@@ -33,9 +33,11 @@ const float kSqrtTwoPi = std::sqrt(2.0 * M_PI);
 void GrowAsNeeded(const sensor::RangeData& range_data,
                   const float truncation_distance, TSDF2D* const tsdf) {
   Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    const Eigen::Vector3f direction = (hit - range_data.origin).normalized();
-    const Eigen::Vector3f end_position = hit + truncation_distance * direction;
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
+    const Eigen::Vector3f direction =
+        (hit.position - range_data.origin).normalized();
+    const Eigen::Vector3f end_position =
+        hit.position + truncation_distance * direction;
     bounding_box.extend(end_position.head<2>());
   }
   // Padding around bounding box to avoid numerical issues at cell boundaries.
@@ -66,9 +68,12 @@ std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
 
 struct RangeDataSorter {
   RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); }
-  bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) {
-    const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized();
-    const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized();
+  bool operator()(const sensor::RangefinderPoint& lhs,
+                  const sensor::RangefinderPoint& rhs) {
+    const Eigen::Vector2f delta_lhs =
+        (lhs.position.head<2>() - origin_).normalized();
+    const Eigen::Vector2f delta_rhs =
+        (lhs.position.head<2>() - origin_).normalized();
     if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) {
       return delta_lhs[1] < 0.f;
     } else if (delta_lhs[1] < 0.f) {
@@ -147,7 +152,8 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
   const Eigen::Vector2f origin = sorted_range_data.origin.head<2>();
   for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size();
        ++hit_index) {
-    const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].head<2>();
+    const Eigen::Vector2f hit =
+        sorted_range_data.returns[hit_index].position.head<2>();
     const float normal = normals.empty()
                              ? std::numeric_limits<float>::quiet_NaN()
                              : normals[hit_index];

cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc

@@ -49,7 +49,7 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test {
 
   void InsertPoint() {
     sensor::RangeData range_data;
-    range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
+    range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
     range_data.origin.x() = -0.5f;
     range_data.origin.y() = -0.5f;
     range_data_inserter_->Insert(range_data, &tsdf_);
@@ -237,9 +237,9 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) {
 TEST_F(RangeDataInserterTest2DTSDF,
        InsertSmallAnglePointWithoutNormalProjection) {
   sensor::RangeData range_data;
-  range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
-  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
-  range_data.returns.emplace_back(10.5f, 3.5f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
+  range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
+  range_data.returns.push_back({Eigen::Vector3f{10.5f, 3.5f, 0.f}});
   range_data.origin.x() = -0.5f;
   range_data.origin.y() = -0.5f;
   range_data_inserter_->Insert(range_data, &tsdf_);
@@ -264,8 +264,8 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) {
   options_.set_project_sdf_distance_to_scan_normal(true);
   range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
   sensor::RangeData range_data;
-  range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
-  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
+  range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
   range_data.origin.x() = -0.5f;
   range_data.origin.y() = -0.5f;
   range_data_inserter_->Insert(range_data, &tsdf_);
@@ -293,8 +293,8 @@ TEST_F(RangeDataInserterTest2DTSDF,
   options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith);
   range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
   sensor::RangeData range_data;
-  range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
-  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
+  range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
   range_data.origin.x() = -0.5f;
   range_data.origin.y() = -0.5f;
   range_data_inserter_->Insert(range_data, &tsdf_);

cartographer/mapping/3d/range_data_inserter_3d.cc

@@ -30,8 +30,8 @@ void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
                           HybridGrid* hybrid_grid,
                           const int num_free_space_voxels) {
   const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin);
-  for (const Eigen::Vector3f& hit : returns) {
-    const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
+  for (const sensor::RangefinderPoint& hit : returns) {
+    const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
 
     const Eigen::Array3i delta = hit_cell - origin_cell;
     const int num_samples = delta.cwiseAbs().maxCoeff();
@@ -79,8 +79,8 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
                                  HybridGrid* hybrid_grid) const {
   CHECK_NOTNULL(hybrid_grid);
 
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
+    const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
     hybrid_grid->ApplyLookupTable(hit_cell, hit_table_);
   }
 

cartographer/mapping/3d/range_data_inserter_3d_test.cc

@@ -41,8 +41,10 @@ class RangeDataInserter3DTest : public ::testing::Test {
 
   void InsertPointCloud() {
     const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
-    sensor::PointCloud returns = {
-        {-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}};
+    sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}},
+                                  {Eigen::Vector3f{-2.f, 0.f, 4.f}},
+                                  {Eigen::Vector3f{-1.f, 1.f, 4.f}},
+                                  {Eigen::Vector3f{0.f, 2.f, 4.f}}};
     range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
                                  &hybrid_grid_);
   }

cartographer/mapping/3d/submap_3d.cc

@@ -41,8 +41,8 @@ struct PixelData {
 sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
                                             const float max_range) {
   sensor::RangeData result{range_data.origin, {}, {}};
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    if ((hit - range_data.origin).norm() <= max_range) {
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
+    if ((hit.position - range_data.origin).norm() <= max_range) {
       result.returns.push_back(hit);
     }
   }

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -130,10 +130,10 @@ LocalTrajectoryBuilder2D::AddRangeData(
 
   CHECK(!synchronized_data.ranges.empty());
   // TODO(gaschler): Check if this can strictly be 0.
-  CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
+  CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
   const common::Time time_first_point =
       time +
-      common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
+      common::FromSeconds(synchronized_data.ranges.front().point_time.time);
   if (time_first_point < extrapolator_->GetLastPoseTime()) {
     LOG(INFO) << "Extrapolator is still initializing.";
     return nullptr;
@@ -143,7 +143,7 @@ LocalTrajectoryBuilder2D::AddRangeData(
   range_data_poses.reserve(synchronized_data.ranges.size());
   bool warned = false;
   for (const auto& range : synchronized_data.ranges) {
-    common::Time time_point = time + common::FromSeconds(range.point_time[3]);
+    common::Time time_point = time + common::FromSeconds(range.point_time.time);
     if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
       if (!warned) {
         LOG(ERROR)
@@ -166,20 +166,23 @@ LocalTrajectoryBuilder2D::AddRangeData(
   // Drop any returns below the minimum range and convert returns beyond the
   // maximum range into misses.
   for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
-    const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;
+    const sensor::TimedRangefinderPoint& hit =
+        synchronized_data.ranges[i].point_time;
     const Eigen::Vector3f origin_in_local =
         range_data_poses[i] *
         synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
-    const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
-    const Eigen::Vector3f delta = hit_in_local - origin_in_local;
+    sensor::RangefinderPoint hit_in_local =
+        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
+    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
     const float range = delta.norm();
     if (range >= options_.min_range()) {
       if (range <= options_.max_range()) {
         accumulated_range_data_.returns.push_back(hit_in_local);
       } else {
-        accumulated_range_data_.misses.push_back(
+        hit_in_local.position =
             origin_in_local +
-            options_.missing_data_ray_length() / range * delta);
+            options_.missing_data_ray_length() / range * delta;
+        accumulated_range_data_.misses.push_back(hit_in_local);
       }
     }
   }