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.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
-  points_batch->points.emplace_back(0.0f, 1.0f, 2.0f);
+  points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
-  points_batch->points.emplace_back(1.0f, 2.0f, 4.0f);
+  points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
-  points_batch->points.emplace_back(0.0f, 3.0f, 5.0f);
+  points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
-  points_batch->points.emplace_back(3.0f, 0.0f, 6.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) {
+  for (const sensor::RangefinderPoint& point : batch->points) {
-    WriteXyzPoint(point, file_writer_.get());
+    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) {
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
-    bounding_box.extend(hit.head<2>());
+    bounding_box.extend(hit.position.head<2>());
   }
-  for (const Eigen::Vector3f& miss : range_data.misses) {
+  for (const sensor::RangefinderPoint& miss : range_data.misses) {
-    bounding_box.extend(miss.head<2>());
+    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) {
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
-    ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
+    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.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}});
-    range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
+    range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}});
-    range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
+    range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}});
-    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, &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) {
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
-    const Eigen::Vector3f direction = (hit - range_data.origin).normalized();
+    const Eigen::Vector3f direction =
-    const Eigen::Vector3f end_position = hit + truncation_distance * 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) {
+  bool operator()(const sensor::RangefinderPoint& lhs,
-    const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized();
+                  const sensor::RangefinderPoint& rhs) {
-    const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized();
+    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.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
-  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
-  range_data.returns.emplace_back(10.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.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
-  range_data.returns.emplace_back(5.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.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
-  range_data.returns.emplace_back(5.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) {
+  for (const sensor::RangefinderPoint& hit : returns) {
-    const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
+    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) {
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
-    const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
+    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 = {
+    sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}},
-        {-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.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) {
+  for (const sensor::RangefinderPoint& hit : range_data.returns) {
-    if ((hit - range_data.origin).norm() <= max_range) {
+    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>();
+    sensor::RangefinderPoint hit_in_local =
-    const Eigen::Vector3f delta = hit_in_local - origin_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);
       }
     }
   }

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

@@ -32,7 +32,8 @@ float EstimateNormal(const sensor::PointCloud& returns,
                      const size_t sample_window_begin,
                      const size_t sample_window_end,
                      const Eigen::Vector3f& sensor_origin) {
-  const Eigen::Vector3f& estimation_point = returns[estimation_point_index];
+  const Eigen::Vector3f& estimation_point =
+      returns[estimation_point_index].position;
   if (sample_window_end - sample_window_begin < 2) {
     return NormalTo2DAngle(sensor_origin - estimation_point);
   }
@@ -42,7 +43,7 @@ float EstimateNormal(const sensor::PointCloud& returns,
   for (size_t sample_point_index = sample_window_begin;
        sample_point_index < sample_window_end; ++sample_point_index) {
     if (sample_point_index == estimation_point_index) continue;
-    const Eigen::Vector3f& sample_point = returns[sample_point_index];
+    const Eigen::Vector3f& sample_point = returns[sample_point_index].position;
     const Eigen::Vector3f& tangent = estimation_point - sample_point;
     Eigen::Vector3f sample_normal = {-tangent[1], tangent[0], 0.f};
     constexpr float kMinNormalLength = 1e-6f;
@@ -83,11 +84,11 @@ std::vector<float> EstimateNormals(
   const float sample_radius = normal_estimation_options.sample_radius();
   for (size_t current_point = 0; current_point < range_data.returns.size();
        ++current_point) {
-    const Eigen::Vector3f& hit = range_data.returns[current_point];
+    const Eigen::Vector3f& hit = range_data.returns[current_point].position;
     size_t sample_window_begin = current_point;
     for (; sample_window_begin > 0 &&
            current_point - sample_window_begin < max_num_samples / 2 &&
-           (hit - range_data.returns[sample_window_begin - 1]).norm() <
+           (hit - range_data.returns[sample_window_begin - 1].position).norm() <
                sample_radius;
          --sample_window_begin) {
     }
@@ -95,7 +96,8 @@ std::vector<float> EstimateNormals(
     for (;
          sample_window_end < range_data.returns.size() &&
          sample_window_end - current_point < ceil(max_num_samples / 2.0) + 1 &&
-         (hit - range_data.returns[sample_window_end]).norm() < sample_radius;
+         (hit - range_data.returns[sample_window_end].position).norm() <
+             sample_radius;
          ++sample_window_end) {
     }
     const float normal_estimate =

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

@@ -46,7 +46,8 @@ TEST(NormalEstimation2DTest, SinglePoint) {
                              static_cast<double>(num_angles) * 2. * M_PI -
                          M_PI;
     range_data.returns.clear();
-    range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f);
+    range_data.returns.push_back(
+        {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
     std::vector<float> normals;
     normals = EstimateNormals(range_data, options);
     EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI),
@@ -63,9 +64,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
   const proto::NormalEstimationOptions2D options =
       CreateNormalEstimationOptions2D(parameter_dictionary.get());
   sensor::RangeData range_data;
-  range_data.returns.emplace_back(-1.f, 1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
-  range_data.returns.emplace_back(0.f, 1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}});
-  range_data.returns.emplace_back(1.f, 1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
   range_data.origin.x() = 0.f;
   range_data.origin.y() = 0.f;
   std::vector<float> normals;
@@ -75,9 +76,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
   }
   normals.clear();
   range_data.returns.clear();
-  range_data.returns.emplace_back(1.f, 1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
-  range_data.returns.emplace_back(1.f, 0.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}});
-  range_data.returns.emplace_back(1.f, -1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
   normals = EstimateNormals(range_data, options);
   for (const float normal : normals) {
     EXPECT_NEAR(std::abs(normal), M_PI, 1e-4);
@@ -85,9 +86,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
 
   normals.clear();
   range_data.returns.clear();
-  range_data.returns.emplace_back(1.f, -1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
-  range_data.returns.emplace_back(0.f, -1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{0.f, -1.f, 0.f}});
-  range_data.returns.emplace_back(-1.f, -1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
   normals = EstimateNormals(range_data, options);
   for (const float normal : normals) {
     EXPECT_NEAR(normal, M_PI_2, 1e-4);
@@ -95,9 +96,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
 
   normals.clear();
   range_data.returns.clear();
-  range_data.returns.emplace_back(-1.f, -1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
-  range_data.returns.emplace_back(-1.f, 0.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}});
-  range_data.returns.emplace_back(-1.f, 1.f, 0.f);
+  range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
   normals = EstimateNormals(range_data, options);
   for (const float normal : normals) {
     EXPECT_NEAR(normal, 0, 1e-4);
@@ -122,7 +123,8 @@ TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) {
     const double angle = static_cast<double>(angle_idx) /
                              static_cast<double>(num_angles) * 2. * M_PI -
                          M_PI;
-    range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f);
+    range_data.returns.push_back(
+        {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
   }
   range_data.origin.x() = 0.f;
   range_data.origin.y() = 0.f;

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

@@ -44,7 +44,8 @@ class PoseGraph2DTest : public ::testing::Test {
     // kMinProbability) to unknown space (== kMinProbability).
     for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
       const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
-      point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f);
+      point_cloud_.push_back(
+          {Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}});
     }
 
     {

cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc

@@ -42,7 +42,7 @@ class CeresScanMatcherTest : public ::testing::Test {
         probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
         kMaxProbability);
 
-    point_cloud_.emplace_back(-3.f, 2.f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-3.f, 2.f, 0.f}});
 
     auto parameter_dictionary = common::MakeDictionary(R"text(
         return {

cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc

@@ -32,8 +32,8 @@ SearchParameters::SearchParameters(const double linear_search_window,
   // We set this value to something on the order of resolution to make sure that
   // the std::acos() below is defined.
   float max_scan_range = 3.f * resolution;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const sensor::RangefinderPoint& point : point_cloud) {
-    const float range = point.head<2>().norm();
+    const float range = point.position.head<2>().norm();
     max_scan_range = std::max(range, max_scan_range);
   }
   const double kSafetyMargin = 1. - 1e-3;
@@ -116,9 +116,9 @@ std::vector<DiscreteScan2D> DiscretizeScans(
   for (const sensor::PointCloud& scan : scans) {
     discrete_scans.emplace_back();
     discrete_scans.back().reserve(scan.size());
-    for (const Eigen::Vector3f& point : scan) {
+    for (const sensor::RangefinderPoint& point : scan) {
       const Eigen::Vector2f translated_point =
-          Eigen::Affine2f(initial_translation) * point.head<2>();
+          Eigen::Affine2f(initial_translation) * point.position.head<2>();
       discrete_scans.back().push_back(
           map_limits.GetCellIndex(translated_point));
     }

cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc

@@ -58,27 +58,27 @@ TEST(Candidate, Construction) {
 
 TEST(GenerateRotatedScans, GenerateRotatedScans) {
   sensor::PointCloud point_cloud;
-  point_cloud.emplace_back(-1.f, 1.f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
   const std::vector<sensor::PointCloud> scans =
       GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.));
   EXPECT_EQ(3, scans.size());
-  EXPECT_NEAR(1., scans[0][0].x(), 1e-6);
+  EXPECT_NEAR(1., scans[0][0].position.x(), 1e-6);
-  EXPECT_NEAR(1., scans[0][0].y(), 1e-6);
+  EXPECT_NEAR(1., scans[0][0].position.y(), 1e-6);
-  EXPECT_NEAR(-1., scans[1][0].x(), 1e-6);
+  EXPECT_NEAR(-1., scans[1][0].position.x(), 1e-6);
-  EXPECT_NEAR(1., scans[1][0].y(), 1e-6);
+  EXPECT_NEAR(1., scans[1][0].position.y(), 1e-6);
-  EXPECT_NEAR(-1., scans[2][0].x(), 1e-6);
+  EXPECT_NEAR(-1., scans[2][0].position.x(), 1e-6);
-  EXPECT_NEAR(-1., scans[2][0].y(), 1e-6);
+  EXPECT_NEAR(-1., scans[2][0].position.y(), 1e-6);
 }
 
 TEST(DiscretizeScans, DiscretizeScans) {
   sensor::PointCloud point_cloud;
-  point_cloud.emplace_back(0.025f, 0.175f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
-  point_cloud.emplace_back(-0.025f, 0.175f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
-  point_cloud.emplace_back(-0.075f, 0.175f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
-  point_cloud.emplace_back(-0.125f, 0.175f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
-  point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
-  point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
-  point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
   const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
                              CellLimits(6, 6));
   const std::vector<sensor::PointCloud> scans =

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc

@@ -150,12 +150,12 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
   const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
 
   sensor::PointCloud point_cloud;
-  point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
-  point_cloud.emplace_back(-2.f, 0.5f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}});
-  point_cloud.emplace_back(0.f, -0.5f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}});
-  point_cloud.emplace_back(0.5f, -1.6f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}});
-  point_cloud.emplace_back(2.5f, 0.5f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
-  point_cloud.emplace_back(2.5f, 1.7f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}});
 
   for (int i = 0; i != 50; ++i) {
     const transform::Rigid2f expected_pose(
@@ -200,12 +200,12 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
   const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
 
   sensor::PointCloud unperturbed_point_cloud;
-  unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
-  unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f);
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.25f, 0.5f, 0.f}});
-  unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f);
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}});
-  unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f);
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{0.25f, 1.6f, 0.f}});
-  unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f);
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
-  unperturbed_point_cloud.emplace_back(2.f, 1.8f, 0.f);
+  unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}});
 
   for (int i = 0; i != 20; ++i) {
     const transform::Rigid2f perturbation(

cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc

@@ -49,8 +49,9 @@ class OccupiedSpaceCostFunction2D {
 
     for (size_t i = 0; i < point_cloud_.size(); ++i) {
       // Note that this is a 2D point. The third component is a scaling factor.
-      const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
+      const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
-                                         (T(point_cloud_[i].y())), T(1.));
+                                         (T(point_cloud_[i].position.y())),
+                                         T(1.));
       const Eigen::Matrix<T, 3, 1> world = transform * point;
       interpolator.Evaluate(
           (limits.max().x() - world[0]) / limits.resolution() - 0.5 +

cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc

@@ -34,7 +34,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
   ValueConversionTables conversion_tables;
   ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
                        &conversion_tables);
-  sensor::PointCloud point_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
+  sensor::PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}};
   ceres::Problem problem;
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid));

cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc

@@ -52,13 +52,13 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
               CreateProbabilityGridRangeDataInserterOptions2D(
                   parameter_dictionary.get()));
     }
-    point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
-    point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
-    point_cloud_.emplace_back(-0.075f, 0.175f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
-    point_cloud_.emplace_back(-0.125f, 0.175f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
-    point_cloud_.emplace_back(-0.125f, 0.125f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
-    point_cloud_.emplace_back(-0.125f, 0.075f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
-    point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
     range_data_inserter_->Insert(
         sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
         &probability_grid_);

cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc

@@ -47,8 +47,9 @@ class TSDFMatchCostFunction2D {
     T summed_weight = T(0);
     for (size_t i = 0; i < point_cloud_.size(); ++i) {
       // Note that this is a 2D point. The third component is a scaling factor.
-      const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
+      const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
-                                         (T(point_cloud_[i].y())), T(1.));
+                                         (T(point_cloud_[i].position.y())),
+                                         T(1.));
       const Eigen::Matrix<T, 3, 1> world = transform * point;
       const T point_weight = interpolated_grid_.GetWeight(world[0], world[1]);
       summed_weight += point_weight;

cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc

@@ -58,7 +58,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test {
   void InsertPointcloud() {
     sensor::RangeData range_data;
     for (float x = -.5; x < 0.5f; x += 0.1) {
-      range_data.returns.emplace_back(x, 1.0f, 0.f);
+      range_data.returns.push_back({Eigen::Vector3f{x, 1.0f, 0.f}});
     }
     range_data.origin.x() = -0.5f;
     range_data.origin.y() = -0.5f;
@@ -73,7 +73,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test {
 };
 
 TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
-  const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
+  const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}};
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@@ -89,7 +89,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
 
 TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
   InsertPointcloud();
-  const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
+  const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@@ -109,7 +109,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
 
 TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
   InsertPointcloud();
-  sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
+  sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   std::array<double, 3> pose_estimate{{0., 0.1, 0.}};
@@ -140,7 +140,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
 
 TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) {
   InsertPointcloud();
-  sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
+  sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
   std::unique_ptr<ceres::CostFunction> cost_function(
       CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
   std::array<double, 3> pose_estimate{{0., 0.4, 0.}};

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -137,10 +137,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
   }
 
   CHECK(!synchronized_data.ranges.empty());
-  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 =
       current_sensor_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;
@@ -156,7 +156,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
 
   for (const auto& hit : hits) {
     common::Time time_point =
-        current_sensor_time + common::FromSeconds(hit.point_time[3]);
+        current_sensor_time + common::FromSeconds(hit.point_time.time);
     if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
       if (!warned) {
         LOG(ERROR)
@@ -176,11 +176,11 @@ LocalTrajectoryBuilder3D::AddRangeData(
   }
 
   for (size_t i = 0; i < hits.size(); ++i) {
-    const Eigen::Vector3f hit_in_local =
+    sensor::RangefinderPoint hit_in_local =
-        hits_poses[i] * hits[i].point_time.head<3>();
+        hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time);
     const Eigen::Vector3f origin_in_local =
         hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
-    const Eigen::Vector3f delta = hit_in_local - origin_in_local;
+    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()) {
@@ -189,8 +189,9 @@ LocalTrajectoryBuilder3D::AddRangeData(
         // 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
         // be updated.
-        accumulated_range_data_.misses.push_back(
+        hit_in_local.position =
-            origin_in_local + options_.max_range() / range * delta);
+            origin_in_local + options_.max_range() / range * delta;
+        accumulated_range_data_.misses.push_back(hit_in_local);
       }
     }
   }

cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc

@@ -179,38 +179,39 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
     sensor::TimedPointCloud directions_in_rangefinder_frame;
     for (int r = -8; r != 8; ++r) {
       for (int s = -250; s != 250; ++s) {
-        Eigen::Vector4f first_point;
+        const sensor::TimedRangefinderPoint first_point{
-        first_point << Eigen::AngleAxisf(M_PI * s / 250.,
+            Eigen::Vector3f{
-                                         Eigen::Vector3f::UnitZ()) *
+                Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
                 Eigen::AngleAxisf(M_PI / 12. * r / 8.,
                                   Eigen::Vector3f::UnitY()) *
-                           Eigen::Vector3f::UnitX(),
+                Eigen::Vector3f::UnitX()},
-            0.;
+            0.};
         directions_in_rangefinder_frame.push_back(first_point);
         // Second orthogonal rangefinder.
-        Eigen::Vector4f second_point;
+        const sensor::TimedRangefinderPoint second_point{
-        second_point << Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
+            Eigen::Vector3f{
-                            Eigen::AngleAxisf(M_PI * s / 250.,
+                Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
-                                              Eigen::Vector3f::UnitZ()) *
+                Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
                 Eigen::AngleAxisf(M_PI / 12. * r / 8.,
                                   Eigen::Vector3f::UnitY()) *
-                            Eigen::Vector3f::UnitX(),
+                Eigen::Vector3f::UnitX()},
-            0.;
+            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::TimedPointCloud returns_in_world_frame;
-    for (const Eigen::Vector4f& direction_in_world_frame :
+    for (const auto& direction_in_world_frame :
          sensor::TransformTimedPointCloud(directions_in_rangefinder_frame,
                                           pose.cast<float>())) {
       const Eigen::Vector3f origin =
           pose.cast<float>() * Eigen::Vector3f::Zero();
-      Eigen::Vector4f return_point;
+      const sensor::TimedRangefinderPoint return_point{
-      return_point << CollideWithBubbles(
+          CollideWithBubbles(
-          origin, CollideWithBox(origin, direction_in_world_frame.head<3>())),
+              origin,
-          0.;
+              CollideWithBox(origin, direction_in_world_frame.position)),
+          0.};
       returns_in_world_frame.push_back(return_point);
     }
     return {Eigen::Vector3f::Zero(),

cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc

@@ -42,7 +42,7 @@ class CeresScanMatcher3DTest : public ::testing::Test {
           Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
           Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
           Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
-      point_cloud_.push_back(point);
+      point_cloud_.push_back({point});
       hybrid_grid_.SetProbability(
           hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
     }

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc

@@ -167,9 +167,9 @@ FastCorrelativeScanMatcher3D::MatchFullSubmap(
     const Eigen::Quaterniond& global_submap_rotation,
     const TrajectoryNode::Data& constant_data, const float min_score) const {
   float max_point_distance = 0.f;
-  for (const Eigen::Vector3f& point :
+  for (const sensor::RangefinderPoint& point :
        constant_data.high_resolution_point_cloud) {
-    max_point_distance = std::max(max_point_distance, point.norm());
+    max_point_distance = std::max(max_point_distance, point.position.norm());
   }
   const int linear_window_size =
       (width_in_voxels_ + 1) / 2 +
@@ -223,9 +223,10 @@ DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan(
   const PrecomputationGrid3D& original_grid =
       precomputation_grid_stack_->Get(0);
   std::vector<Eigen::Array3i> full_resolution_cell_indices;
-  for (const Eigen::Vector3f& point :
+  for (const sensor::RangefinderPoint& point :
        sensor::TransformPointCloud(point_cloud, pose)) {
-    full_resolution_cell_indices.push_back(original_grid.GetCellIndex(point));
+    full_resolution_cell_indices.push_back(
+        original_grid.GetCellIndex(point.position));
   }
   const int full_resolution_depth = std::min(options_.full_resolution_depth(),
                                              options_.branch_and_bound_depth());
@@ -271,8 +272,8 @@ std::vector<DiscreteScan3D> FastCorrelativeScanMatcher3D::GenerateDiscreteScans(
   // We set this value to something on the order of resolution to make sure that
   // the std::acos() below is defined.
   float max_scan_range = 3.f * resolution_;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const sensor::RangefinderPoint& point : point_cloud) {
-    const float range = point.norm();
+    const float range = point.position.norm();
     max_scan_range = std::max(range, max_scan_range);
   }
   const float kSafetyMargin = 1.f - 1e-2f;

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc

@@ -41,12 +41,12 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
 
   void SetUp() override {
     point_cloud_ = {
-        Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f),
+        {Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 0.f, 0.f)},
-        Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f),
+        {Eigen::Vector3f(5.f, 0.f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)},
-        Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f),
+        {Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)},
-        Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f),
+        {Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)},
-        Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f),
+        {Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)},
-        Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)};
+        {Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}};
   }
 
   transform::Rigid3f GetRandomPose() {
@@ -157,7 +157,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
     const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
         low_resolution_result = fast_correlative_scan_matcher->Match(
             transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
-            CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
+            CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}),
+            kMinScore);
     EXPECT_THAT(low_resolution_result, testing::IsNull())
         << low_resolution_result->low_resolution_score;
   }
@@ -185,7 +186,7 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
   const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
       low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
           Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
-          CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
+          CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore);
   EXPECT_THAT(low_resolution_result, testing::IsNull())
       << low_resolution_result->low_resolution_score;
 }

cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc

@@ -24,11 +24,11 @@ std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
     const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
   return [=](const transform::Rigid3f& pose) {
     float score = 0.f;
-    for (const Eigen::Vector3f& point :
+    for (const sensor::RangefinderPoint& point :
          sensor::TransformPointCloud(*points, pose)) {
       // TODO(zhengj, whess): Interpolate the Grid to get better score.
       score += low_resolution_grid->GetProbability(
-          low_resolution_grid->GetCellIndex(point));
+          low_resolution_grid->GetCellIndex(point.position));
     }
     return score / points->size();
   };

cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h

@@ -71,7 +71,7 @@ class OccupiedSpaceCostFunction3D {
                 T* const residual) const {
     for (size_t i = 0; i < point_cloud_.size(); ++i) {
       const Eigen::Matrix<T, 3, 1> world =
-          transform * point_cloud_[i].cast<T>();
+          transform * point_cloud_[i].position.cast<T>();
       const T probability =
           interpolated_grid_.GetProbability(world[0], world[1], world[2]);
       residual[i] = scaling_factor_ * (1. - probability);

cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc

@@ -61,8 +61,8 @@ RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms(
   // We set this value to something on the order of resolution to make sure that
   // the std::acos() below is defined.
   float max_scan_range = 3.f * resolution;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const sensor::RangefinderPoint& point : point_cloud) {
-    const float range = point.norm();
+    const float range = point.position.norm();
     max_scan_range = std::max(range, max_scan_range);
   }
   const float kSafetyMargin = 1.f - 1e-3f;
@@ -99,8 +99,9 @@ float RealTimeCorrelativeScanMatcher3D::ScoreCandidate(
     const sensor::PointCloud& transformed_point_cloud,
     const transform::Rigid3f& transform) const {
   float score = 0.f;
-  for (const Eigen::Vector3f& point : transformed_point_cloud) {
+  for (const sensor::RangefinderPoint& point : transformed_point_cloud) {
-    score += hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point));
+    score +=
+        hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point.position));
   }
   score /= static_cast<float>(transformed_point_cloud.size());
   const float angle = transform::GetAngle(transform);

cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc

@@ -44,7 +44,7 @@ class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test {
           Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
           Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
           Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
-      point_cloud_.push_back(point);
+      point_cloud_.push_back({point});
       hybrid_grid_.SetProbability(
           hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
     }

cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc

@@ -52,8 +52,8 @@ void AddValueToHistogram(float angle, const float value,
 Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) {
   CHECK(!slice.empty());
   Eigen::Vector3f sum = Eigen::Vector3f::Zero();
-  for (const Eigen::Vector3f& point : slice) {
+  for (const sensor::RangefinderPoint& point : slice) {
-    sum += point;
+    sum += point.position;
   }
   return sum / static_cast<float>(slice.size());
 }
@@ -68,16 +68,17 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice,
   // will add the angle between points to the histogram with the maximum weight.
   // This is to reject, e.g., the angles observed on the ceiling and floor.
   const Eigen::Vector3f centroid = ComputeCentroid(slice);
-  Eigen::Vector3f last_point = slice.front();
+  Eigen::Vector3f last_point_position = slice.front().position;
-  for (const Eigen::Vector3f& point : slice) {
+  for (const sensor::RangefinderPoint& point : slice) {
-    const Eigen::Vector2f delta = (point - last_point).head<2>();
+    const Eigen::Vector2f delta =
-    const Eigen::Vector2f direction = (point - centroid).head<2>();
+        (point.position - last_point_position).head<2>();
+    const Eigen::Vector2f direction = (point.position - centroid).head<2>();
     const float distance = delta.norm();
     if (distance < kMinDistance || direction.norm() < kMinDistance) {
       continue;
     }
     if (distance > kMaxDistance) {
-      last_point = point;
+      last_point_position = point.position;
       continue;
     }
     const float angle = common::atan2(delta);
@@ -97,13 +98,13 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
     }
 
     float angle;
-    Eigen::Vector3f point;
+    sensor::RangefinderPoint point;
   };
   const Eigen::Vector3f centroid = ComputeCentroid(slice);
   std::vector<SortableAnglePointPair> by_angle;
   by_angle.reserve(slice.size());
-  for (const Eigen::Vector3f& point : slice) {
+  for (const sensor::RangefinderPoint& point : slice) {
-    const Eigen::Vector2f delta = (point - centroid).head<2>();
+    const Eigen::Vector2f delta = (point.position - centroid).head<2>();
     if (delta.norm() < kMinDistance) {
       continue;
     }
@@ -155,8 +156,9 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
     const sensor::PointCloud& point_cloud, const int histogram_size) {
   Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size);
   std::map<int, sensor::PointCloud> slices;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const sensor::RangefinderPoint& point : point_cloud) {
-    slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point);
+    slices[common::RoundToInt(point.position.z() / kSliceHeight)].push_back(
+        point);
   }
   for (const auto& slice : slices) {
     AddPointCloudSliceToHistogram(SortSlice(slice.second), &histogram);

cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc

@@ -70,7 +70,7 @@ TEST_F(ConstraintBuilder2DTest, CallsBack) {
 TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
   TrajectoryNode::Data node_data;
   node_data.filtered_gravity_aligned_point_cloud.push_back(
-      Eigen::Vector3f(0.1, 0.2, 0.3));
+      {Eigen::Vector3f(0.1, 0.2, 0.3)});
   node_data.gravity_alignment = Eigen::Quaterniond::Identity();
   node_data.local_pose = transform::Rigid3d::Identity();
   SubmapId submap_id{0, 1};

cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc

@@ -75,9 +75,9 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
   auto node_data = std::make_shared<TrajectoryNode::Data>();
   node_data->gravity_alignment = Eigen::Quaterniond::Identity();
   node_data->high_resolution_point_cloud.push_back(
-      Eigen::Vector3f(0.1, 0.2, 0.3));
+      {Eigen::Vector3f(0.1, 0.2, 0.3)});
   node_data->low_resolution_point_cloud.push_back(
-      Eigen::Vector3f(0.1, 0.2, 0.3));
+      {Eigen::Vector3f(0.1, 0.2, 0.3)});
   node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3);
   node_data->local_pose = transform::Rigid3d::Identity();
   node.constant_data = node_data;

cartographer/mapping/internal/range_data_collator.cc

@@ -63,13 +63,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
 
     auto overlap_begin = ranges.begin();
     while (overlap_begin < ranges.end() &&
-           data.time + common::FromSeconds((*overlap_begin)[3]) <
+           data.time + common::FromSeconds((*overlap_begin).time) <
                current_start_) {
       ++overlap_begin;
     }
     auto overlap_end = overlap_begin;
     while (overlap_end < ranges.end() &&
-           data.time + common::FromSeconds((*overlap_end)[3]) <= current_end_) {
+           data.time + common::FromSeconds((*overlap_end).time) <=
+               current_end_) {
       ++overlap_end;
     }
     if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
@@ -82,14 +83,15 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
     if (overlap_begin < overlap_end) {
       std::size_t origin_index = result.origins.size();
       result.origins.push_back(data.origin);
-      double time_correction = common::ToSeconds(data.time - current_end_);
+      const float time_correction =
+          static_cast<float>(common::ToSeconds(data.time - current_end_));
       for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
            ++overlap_it) {
         sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it,
                                                                   origin_index};
         // current_end_ + point_time[3]_after == in_timestamp +
         // point_time[3]_before
-        point.point_time[3] += time_correction;
+        point.point_time.time += time_correction;
         result.ranges.push_back(point);
       }
     }
@@ -110,7 +112,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
   std::sort(result.ranges.begin(), result.ranges.end(),
             [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
                const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
-              return a.point_time[3] < b.point_time[3];
+              return a.point_time.time < b.point_time.time;
             });
   return result;
 }

cartographer/mapping/internal/range_data_collator_test.cc

@@ -29,13 +29,13 @@ const int kNumSamples = 10;
 sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) {
   double duration = common::ToSeconds(common::FromUniversal(to) -
                                       common::FromUniversal(from));
-  sensor::TimedPointCloudData result{common::FromUniversal(to),
+  sensor::TimedPointCloudData result{
-                                     Eigen::Vector3f(0., 1., 2.),
+      common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}};
-                                     sensor::TimedPointCloud(kNumSamples)};
+  result.ranges.reserve(kNumSamples);
   for (int i = 0; i < kNumSamples; ++i) {
     double fraction = static_cast<double>(i) / (kNumSamples - 1);
-    double relative_time = (1.f - fraction) * -duration;
+    float relative_time = (1.f - fraction) * -duration;
-    result.ranges[i] = Eigen::Vector4f(1., 2., 3., relative_time);
+    result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time});
   }
   return result;
 }
@@ -44,7 +44,7 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) {
   std::vector<float> timestamps;
   timestamps.reserve(data.ranges.size());
   for (const auto& range : data.ranges) {
-    timestamps.push_back(range.point_time[3]);
+    timestamps.push_back(range.point_time.time);
   }
   return std::is_sorted(timestamps.begin(), timestamps.end());
 }
@@ -66,14 +66,14 @@ TEST(RangeDataCollatorTest, SingleSensor) {
   EXPECT_TRUE(ArePointTimestampsSorted(output_1));
   EXPECT_NEAR(common::ToUniversal(
                   output_1.time +
-                  common::FromSeconds(output_1.ranges[0].point_time[3])),
+                  common::FromSeconds(output_1.ranges[0].point_time.time)),
               300, 2);
   auto output_2 =
       collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510));
   EXPECT_EQ(common::ToUniversal(output_2.time), 510);
   EXPECT_EQ(output_2.origins.size(), 1);
   EXPECT_EQ(output_2.ranges.size(), 1);
-  EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f);
+  EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f);
   EXPECT_TRUE(ArePointTimestampsSorted(output_2));
 }
 
@@ -93,14 +93,14 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
   EXPECT_TRUE(ArePointTimestampsSorted(output_1));
   EXPECT_NEAR(common::ToUniversal(
                   output_1.time +
-                  common::FromSeconds(output_1.ranges[0].point_time[3])),
+                  common::FromSeconds(output_1.ranges[0].point_time.time)),
               300, 2);
   auto output_2 =
       collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510));
   EXPECT_EQ(common::ToUniversal(output_2.time), 510);
   EXPECT_EQ(output_2.origins.size(), 1);
   EXPECT_EQ(output_2.ranges.size(), 1);
-  EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f);
+  EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f);
   EXPECT_TRUE(ArePointTimestampsSorted(output_2));
 }
 
@@ -118,9 +118,9 @@ TEST(RangeDataCollatorTest, TwoSensors) {
   ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1);
   EXPECT_NEAR(common::ToUniversal(
                   output_1.time +
-                  common::FromSeconds(output_1.ranges[0].point_time[3])),
+                  common::FromSeconds(output_1.ranges[0].point_time.time)),
               -1000, 2);
-  EXPECT_EQ(output_1.ranges.back().point_time[3], 0.f);
+  EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f);
   EXPECT_TRUE(ArePointTimestampsSorted(output_1));
   auto output_2 =
       collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500));
@@ -129,9 +129,9 @@ TEST(RangeDataCollatorTest, TwoSensors) {
   ASSERT_EQ(output_2.ranges.size(), 2);
   EXPECT_NEAR(common::ToUniversal(
                   output_2.time +
-                  common::FromSeconds(output_2.ranges[0].point_time[3])),
+                  common::FromSeconds(output_2.ranges[0].point_time.time)),
               300, 2);
-  EXPECT_EQ(output_2.ranges.back().point_time[3], 0.f);
+  EXPECT_EQ(output_2.ranges.back().point_time.time, 0.f);
   EXPECT_TRUE(ArePointTimestampsSorted(output_2));
   // Sending the same sensor will flush everything before.
   auto output_3 =
@@ -140,7 +140,7 @@ TEST(RangeDataCollatorTest, TwoSensors) {
   EXPECT_EQ(
       output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(),
       3 * kNumSamples);
-  EXPECT_EQ(output_3.ranges.back().point_time[3], 0.f);
+  EXPECT_EQ(output_3.ranges.back().point_time.time, 0.f);
   EXPECT_TRUE(ArePointTimestampsSorted(output_3));
 }
 

cartographer/mapping/internal/testing/test_helpers.cc

@@ -56,8 +56,10 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
   for (double angle = 0.; angle < M_PI; angle += 0.01) {
     for (double height : {-0.4, -0.2, 0.0, 0.2, 0.4}) {
       constexpr double kRadius = 5;
-      point_cloud.emplace_back(kRadius * std::cos(angle),
+      point_cloud.push_back({Eigen::Vector3d{kRadius * std::cos(angle),
-                               kRadius * std::sin(angle), height, 0.);
+                                             kRadius * std::sin(angle), height}
+                                 .cast<float>(),
+                             0.});
     }
   }
   const Eigen::Vector3f kVelocity = translation / duration;

cartographer/mapping/trajectory_node_test.cc

@@ -32,10 +32,13 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
   const TrajectoryNode::Data expected{
       common::FromUniversal(42),
       Eigen::Quaterniond(1., 2., -3., -4.),
-      sensor::CompressedPointCloud({{1.f, 2.f, 0.f}, {0.f, 0.f, 1.f}})
+      sensor::CompressedPointCloud(
+          {{Eigen::Vector3f{1.f, 2.f, 0.f}}, {Eigen::Vector3f{0.f, 0.f, 1.f}}})
+          .Decompress(),
+      sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}})
+          .Decompress(),
+      sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}})
           .Decompress(),
-      sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(),
-      sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(),
       Eigen::VectorXf::Unit(20, 4),
       transform::Rigid3d({1., 2., 3.},
                          Eigen::Quaterniond(4., 5., -6., -7.).normalized())};

cartographer/sensor/compressed_point_cloud.cc

@@ -56,9 +56,9 @@ CompressedPointCloud::ConstIterator::EndIterator(
   return end_iterator;
 }
 
-Eigen::Vector3f CompressedPointCloud::ConstIterator::operator*() const {
+RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const {
   CHECK_GT(remaining_points_, 0);
-  return current_point_;
+  return {current_point_};
 }
 
 CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator::
@@ -109,14 +109,14 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
   CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());
   for (int point_index = 0; point_index < static_cast<int>(point_cloud.size());
        ++point_index) {
-    const Eigen::Vector3f& point = point_cloud[point_index];
+    const RangefinderPoint& point = point_cloud[point_index];
-    CHECK_LT(point.cwiseAbs().maxCoeff() / kPrecision,
+    CHECK_LT(point.position.cwiseAbs().maxCoeff() / kPrecision,
              1 << kMaxBitsPerDirection)
-        << "Point out of bounds: " << point;
+        << "Point out of bounds: " << point.position;
     Eigen::Array3i raster_point;
     Eigen::Array3i block_coordinate;
     for (int i = 0; i < 3; ++i) {
-      raster_point[i] = common::RoundToInt(point[i] / kPrecision);
+      raster_point[i] = common::RoundToInt(point.position[i] / kPrecision);
       block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate;
       raster_point[i] &= kCoordinateMask;
     }
@@ -170,14 +170,14 @@ CompressedPointCloud::ConstIterator CompressedPointCloud::end() const {
 
 PointCloud CompressedPointCloud::Decompress() const {
   PointCloud decompressed;
-  for (const Eigen::Vector3f& point : *this) {
+  for (const RangefinderPoint& point : *this) {
     decompressed.push_back(point);
   }
   return decompressed;
 }
 
-bool sensor::CompressedPointCloud::operator==(
+bool CompressedPointCloud::operator==(
-    const sensor::CompressedPointCloud& right_hand_container) const {
+    const CompressedPointCloud& right_hand_container) const {
   return point_data_ == right_hand_container.point_data_ &&
          num_points_ == right_hand_container.num_points_;
 }

cartographer/sensor/compressed_point_cloud.h

@@ -61,10 +61,10 @@ class CompressedPointCloud {
 class CompressedPointCloud::ConstIterator {
  public:
   using iterator_category = std::forward_iterator_tag;
-  using value_type = Eigen::Vector3f;
+  using value_type = RangefinderPoint;
   using difference_type = int64;
-  using pointer = const Eigen::Vector3f*;
+  using pointer = const RangefinderPoint*;
-  using reference = const Eigen::Vector3f&;
+  using reference = const RangefinderPoint&;
 
   // Creates begin iterator.
   explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud);
@@ -73,7 +73,7 @@ class CompressedPointCloud::ConstIterator {
   static ConstIterator EndIterator(
       const CompressedPointCloud* compressed_point_cloud);
 
-  Eigen::Vector3f operator*() const;
+  RangefinderPoint operator*() const;
   ConstIterator& operator++();
   bool operator!=(const ConstIterator& it) const;
 

cartographer/sensor/compressed_point_cloud_test.cc

@@ -42,13 +42,13 @@ constexpr float kPrecision = 0.001f;
 // Matcher for 3-d vectors w.r.t. to the target precision.
 MATCHER_P(ApproximatelyEquals, expected,
           std::string("is equal to ") + PrintToString(expected)) {
-  return (arg - expected).isZero(kPrecision);
+  return (arg.position - expected).isZero(kPrecision);
 }
 
 // Helper function to test the mapping of a single point. Includes test for
 // recompressing the same point again.
 void TestPoint(const Eigen::Vector3f& p) {
-  CompressedPointCloud compressed({p});
+  CompressedPointCloud compressed({{p}});
   EXPECT_EQ(1, compressed.size());
   EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p));
   CompressedPointCloud recompressed({*compressed.begin()});
@@ -70,9 +70,9 @@ TEST(CompressPointCloudTest, CompressesPointsCorrectly) {
 }
 
 TEST(CompressPointCloudTest, Compresses) {
-  const CompressedPointCloud compressed({Eigen::Vector3f(0.838f, 0, 0),
+  const CompressedPointCloud compressed({{Eigen::Vector3f(0.838f, 0, 0)},
-                                         Eigen::Vector3f(0.839f, 0, 0),
+                                         {Eigen::Vector3f(0.839f, 0, 0)},
-                                         Eigen::Vector3f(0.840f, 0, 0)});
+                                         {Eigen::Vector3f(0.840f, 0, 0)}});
   EXPECT_FALSE(compressed.empty());
   EXPECT_EQ(3, compressed.size());
   const PointCloud decompressed = compressed.Decompress();
@@ -98,7 +98,7 @@ TEST(CompressPointCloudTest, CompressesEmptyPointCloud) {
 TEST(CompressPointCloudTest, CompressesNoGaps) {
   PointCloud point_cloud;
   for (int i = 0; i < 3000; ++i) {
-    point_cloud.push_back(Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0));
+    point_cloud.push_back({Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)});
   }
   const CompressedPointCloud compressed(point_cloud);
   const PointCloud decompressed = compressed.Decompress();
@@ -106,8 +106,8 @@ TEST(CompressPointCloudTest, CompressesNoGaps) {
   EXPECT_EQ(decompressed.size(), recompressed.size());
 
   std::vector<float> x_coord;
-  for (const Eigen::Vector3f& p : compressed) {
+  for (const RangefinderPoint& p : compressed) {
-    x_coord.push_back(p[0]);
+    x_coord.push_back(p.position[0]);
   }
   std::sort(x_coord.begin(), x_coord.end());
   for (size_t i = 1; i < x_coord.size(); ++i) {

cartographer/sensor/internal/ordered_multi_queue_test.cc

@@ -45,8 +45,7 @@ class OrderedMultiQueueTest : public ::testing::Test {
 
   std::unique_ptr<Data> MakeImu(const int ordinal) {
     return MakeDispatchable(
-        "imu",
+        "imu", ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
-        sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
                        Eigen::Vector3d::Zero()});
   }
 

cartographer/sensor/internal/test_helpers.h

@@ -76,7 +76,7 @@ struct CollatorInput {
   }
 
   const int trajectory_id;
-  std::unique_ptr<sensor::Data> data;
+  std::unique_ptr<Data> data;
   const CollatorOutput expected_output;
 };
 

cartographer/sensor/internal/voxel_filter.cc

@@ -28,8 +28,8 @@ namespace {
 PointCloud FilterByMaxRange(const PointCloud& point_cloud,
                             const float max_range) {
   PointCloud result;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const RangefinderPoint& point : point_cloud) {
-    if (point.norm() <= max_range) {
+    if (point.position.norm() <= max_range) {
       result.push_back(point);
     }
   }
@@ -80,8 +80,9 @@ PointCloud AdaptivelyVoxelFiltered(
 
 PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
   PointCloud results;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const RangefinderPoint& point : point_cloud) {
-    auto it_inserted = voxel_set_.insert(IndexToKey(GetCellIndex(point)));
+    auto it_inserted =
+        voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
     if (it_inserted.second) {
       results.push_back(point);
     }
@@ -91,9 +92,9 @@ PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
 
 TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
   TimedPointCloud results;
-  for (const Eigen::Vector4f& point : timed_point_cloud) {
+  for (const TimedRangefinderPoint& point : timed_point_cloud) {
     auto it_inserted =
-        voxel_set_.insert(IndexToKey(GetCellIndex(point.head<3>())));
+        voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
     if (it_inserted.second) {
       results.push_back(point);
     }
@@ -101,14 +102,13 @@ TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
   return results;
 }
 
-std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>
+std::vector<TimedPointCloudOriginData::RangeMeasurement> VoxelFilter::Filter(
-VoxelFilter::Filter(
+    const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
-    const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
         range_measurements) {
-  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> results;
+  std::vector<TimedPointCloudOriginData::RangeMeasurement> results;
   for (const auto& range_measurement : range_measurements) {
     auto it_inserted = voxel_set_.insert(
-        IndexToKey(GetCellIndex(range_measurement.point_time.head<3>())));
+        IndexToKey(GetCellIndex(range_measurement.point_time.position)));
     if (it_inserted.second) {
       results.push_back(range_measurement);
     }

cartographer/sensor/internal/voxel_filter.h

@@ -46,8 +46,8 @@ class VoxelFilter {
   TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
 
   // Same for RangeMeasurement.
-  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> Filter(
+  std::vector<TimedPointCloudOriginData::RangeMeasurement> Filter(
-      const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
+      const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
           range_measurements);
 
  private:

cartographer/sensor/internal/voxel_filter_test.cc

@@ -27,19 +27,19 @@ namespace {
 using ::testing::ContainerEq;
 
 TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
-  PointCloud point_cloud = {{0.f, 0.f, 0.f},
+  PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}},
-                            {0.1f, -0.1f, 0.1f},
+                            {Eigen::Vector3f{0.1f, -0.1f, 0.1f}},
-                            {0.3f, -0.1f, 0.f},
+                            {Eigen::Vector3f{0.3f, -0.1f, 0.f}},
-                            {0.f, 0.f, 0.1f}};
+                            {Eigen::Vector3f{0.f, 0.f, 0.1f}}};
   EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud),
               ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
 }
 
 TEST(VoxelFilterTest, HandlesLargeCoordinates) {
-  PointCloud point_cloud = {{100000.f, 0.f, 0.f},
+  PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}},
-                            {100000.001f, -0.0001f, 0.0001f},
+                            {Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}},
-                            {100000.003f, -0.0001f, 0.f},
+                            {Eigen::Vector3f{100000.003f, -0.0001f, 0.f}},
-                            {-200000.f, 0.f, 0.f}};
+                            {Eigen::Vector3f{-200000.f, 0.f, 0.f}}};
   EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud),
               ContainerEq(PointCloud{point_cloud[0], point_cloud[3]}));
 }
@@ -47,7 +47,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) {
 TEST(VoxelFilterTest, IgnoresTime) {
   TimedPointCloud timed_point_cloud;
   for (int i = 0; i < 100; ++i) {
-    timed_point_cloud.emplace_back(-100.f, 0.3f, 0.4f, 1.f * i);
+    timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i});
   }
   EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud),
               ContainerEq(TimedPointCloud{timed_point_cloud[0]}));

cartographer/sensor/point_cloud.cc

@@ -26,7 +26,7 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
                                const transform::Rigid3f& transform) {
   PointCloud result;
   result.reserve(point_cloud.size());
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const RangefinderPoint& point : point_cloud) {
     result.emplace_back(transform * point);
   }
   return result;
@@ -36,11 +36,8 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
                                          const transform::Rigid3f& transform) {
   TimedPointCloud result;
   result.reserve(point_cloud.size());
-  for (const Eigen::Vector4f& point : point_cloud) {
+  for (const TimedRangefinderPoint& point : point_cloud) {
-    Eigen::Vector4f result_point;
+    result.push_back(transform * point);
-    result_point.head<3>() = transform * point.head<3>();
-    result_point[3] = point[3];
-    result.emplace_back(result_point);
   }
   return result;
 }
@@ -48,8 +45,8 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
 PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
                           const float max_z) {
   PointCloud cropped_point_cloud;
-  for (const Eigen::Vector3f& point : point_cloud) {
+  for (const RangefinderPoint& point : point_cloud) {
-    if (min_z <= point.z() && point.z() <= max_z) {
+    if (min_z <= point.position.z() && point.position.z() <= max_z) {
       cropped_point_cloud.push_back(point);
     }
   }
@@ -59,8 +56,8 @@ PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
 TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
                                     const float min_z, const float max_z) {
   TimedPointCloud cropped_point_cloud;
-  for (const Eigen::Vector4f& point : point_cloud) {
+  for (const TimedRangefinderPoint& point : point_cloud) {
-    if (min_z <= point.z() && point.z() <= max_z) {
+    if (min_z <= point.position.z() && point.position.z() <= max_z) {
       cropped_point_cloud.push_back(point);
     }
   }

cartographer/sensor/point_cloud.h

@@ -21,6 +21,7 @@
 
 #include "Eigen/Core"
 #include "cartographer/sensor/proto/sensor.pb.h"
+#include "cartographer/sensor/rangefinder_point.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "glog/logging.h"
 
@@ -29,14 +30,14 @@ namespace sensor {
 
 // Stores 3D positions of points.
 // For 2D points, the third entry is 0.f.
-typedef std::vector<Eigen::Vector3f> PointCloud;
+using PointCloud = std::vector<RangefinderPoint>;
 
 // Stores 3D positions of points with their relative measurement time in the
 // fourth entry. Time is in seconds, increasing and relative to the moment when
 // the last point was acquired. So, the fourth entry for the last point is 0.f.
 // 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;
+using TimedPointCloud = std::vector<TimedRangefinderPoint>;
 
 struct PointCloudWithIntensities {
   TimedPointCloud points;

cartographer/sensor/point_cloud_test.cc

@@ -27,26 +27,26 @@ namespace {
 
 TEST(PointCloudTest, TransformPointCloud) {
   PointCloud point_cloud;
-  point_cloud.emplace_back(0.5f, 0.5f, 1.f);
+  point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}});
-  point_cloud.emplace_back(3.5f, 0.5f, 42.f);
+  point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}});
   const PointCloud transformed_point_cloud = TransformPointCloud(
       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].position.x(), 1e-6);
-  EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
+  EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6);
-  EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
+  EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6);
-  EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
+  EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
 }
 
 TEST(PointCloudTest, TransformTimedPointCloud) {
   TimedPointCloud point_cloud;
-  point_cloud.emplace_back(0.5f, 0.5f, 1.f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}, 0.f});
-  point_cloud.emplace_back(3.5f, 0.5f, 42.f, 0.f);
+  point_cloud.push_back({Eigen::Vector3f{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].position.x(), 1e-6);
-  EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
+  EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6);
-  EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
+  EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6);
-  EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
+  EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
 }
 
 }  // namespace

cartographer/sensor/proto/sensor.proto

@@ -20,6 +20,15 @@ option java_outer_classname = "Sensor";
 
 import "cartographer/transform/proto/transform.proto";
 
+message RangefinderPoint {
+  transform.proto.Vector3f position = 1;
+}
+
+message TimedRangefinderPoint {
+  transform.proto.Vector3f position = 1;
+  float time = 2;
+}
+
 // Compressed collection of a 3D point cloud.
 message CompressedPointCloud {
   int32 num_points = 1;
@@ -30,14 +39,17 @@ message CompressedPointCloud {
 message TimedPointCloudData {
   int64 timestamp = 1;
   transform.proto.Vector3f origin = 2;
-  repeated transform.proto.Vector4f point_data = 3;
+  repeated transform.proto.Vector4f point_data_legacy = 3;
+  repeated TimedRangefinderPoint point_data = 4;
 }
 
 // Proto representation of ::cartographer::sensor::RangeData.
 message RangeData {
   transform.proto.Vector3f origin = 1;
-  repeated transform.proto.Vector3f returns = 2;
+  repeated transform.proto.Vector3f returns_legacy = 2;
-  repeated transform.proto.Vector3f misses = 3;
+  repeated transform.proto.Vector3f misses_legacy = 3;
+  repeated RangefinderPoint returns = 4;
+  repeated RangefinderPoint misses = 5;
 }
 
 // Proto representation of ::cartographer::sensor::ImuData.

cartographer/sensor/range_data.cc

@@ -58,30 +58,41 @@ proto::RangeData ToProto(const RangeData& range_data) {
   proto::RangeData proto;
   *proto.mutable_origin() = transform::ToProto(range_data.origin);
   proto.mutable_returns()->Reserve(range_data.returns.size());
-  for (const Eigen::Vector3f& point : range_data.returns) {
+  for (const RangefinderPoint& point : range_data.returns) {
-    *proto.add_returns() = transform::ToProto(point);
+    *proto.add_returns() = ToProto(point);
   }
   proto.mutable_misses()->Reserve(range_data.misses.size());
-  for (const Eigen::Vector3f& point : range_data.misses) {
+  for (const RangefinderPoint& point : range_data.misses) {
-    *proto.add_misses() = transform::ToProto(point);
+    *proto.add_misses() = ToProto(point);
   }
   return proto;
 }
 
 RangeData FromProto(const proto::RangeData& proto) {
   PointCloud returns;
+  if (proto.returns_size() > 0) {
     returns.reserve(proto.returns().size());
-  std::transform(
+    for (const auto& point_proto : proto.returns()) {
-      proto.returns().begin(), proto.returns().end(),
+      returns.push_back(FromProto(point_proto));
-      std::back_inserter(returns),
+    }
-      static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
+  } else {
-          transform::ToEigen));
+    returns.reserve(proto.returns_legacy().size());
+    for (const auto& point_proto : proto.returns_legacy()) {
+      returns.push_back({transform::ToEigen(point_proto)});
+    }
+  }
   PointCloud misses;
+  if (proto.misses_size() > 0) {
     misses.reserve(proto.misses().size());
-  std::transform(
+    for (const auto& point_proto : proto.misses()) {
-      proto.misses().begin(), proto.misses().end(), std::back_inserter(misses),
+      misses.push_back(FromProto(point_proto));
-      static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
+    }
-          transform::ToEigen));
+  } else {
+    misses.reserve(proto.misses_legacy().size());
+    for (const auto& point_proto : proto.misses_legacy()) {
+      misses.push_back({transform::ToEigen(point_proto)});
+    }
+  }
   return RangeData{transform::ToEigen(proto.origin()), returns, misses};
 }
 

cartographer/sensor/rangefinder_point.h

@@ -0,0 +1,108 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_
+#define CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_
+
+#include <vector>
+
+#include "Eigen/Core"
+#include "cartographer/sensor/proto/sensor.pb.h"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace sensor {
+
+// Stores 3D position of a point observed by a rangefinder sensor.
+struct RangefinderPoint {
+  Eigen::Vector3f position;
+};
+
+// Stores 3D position of a point with its relative measurement time.
+// See point_cloud.h for more details.
+struct TimedRangefinderPoint {
+  Eigen::Vector3f position;
+  float time;
+};
+
+template <class T>
+inline RangefinderPoint operator*(const transform::Rigid3<T>& lhs,
+                                  const RangefinderPoint& rhs) {
+  RangefinderPoint result = rhs;
+  result.position = lhs * rhs.position;
+  return result;
+}
+
+template <class T>
+inline TimedRangefinderPoint operator*(const transform::Rigid3<T>& lhs,
+                                       const TimedRangefinderPoint& rhs) {
+  TimedRangefinderPoint result = rhs;
+  result.position = lhs * rhs.position;
+  return result;
+}
+
+inline bool operator==(const RangefinderPoint& lhs,
+                       const RangefinderPoint& rhs) {
+  return lhs.position == rhs.position;
+}
+
+inline bool operator==(const TimedRangefinderPoint& lhs,
+                       const TimedRangefinderPoint& rhs) {
+  return lhs.position == rhs.position && lhs.time == rhs.time;
+}
+
+inline RangefinderPoint FromProto(
+    const proto::RangefinderPoint& rangefinder_point_proto) {
+  return {transform::ToEigen(rangefinder_point_proto.position())};
+}
+
+inline proto::RangefinderPoint ToProto(
+    const RangefinderPoint& rangefinder_point) {
+  proto::RangefinderPoint proto;
+  *proto.mutable_position() = transform::ToProto(rangefinder_point.position);
+  return proto;
+}
+
+inline TimedRangefinderPoint FromProto(
+    const proto::TimedRangefinderPoint& timed_rangefinder_point_proto) {
+  return {transform::ToEigen(timed_rangefinder_point_proto.position()),
+          timed_rangefinder_point_proto.time()};
+}
+
+inline proto::TimedRangefinderPoint ToProto(
+    const TimedRangefinderPoint& timed_rangefinder_point) {
+  proto::TimedRangefinderPoint proto;
+  *proto.mutable_position() =
+      transform::ToProto(timed_rangefinder_point.position);
+  proto.set_time(timed_rangefinder_point.time);
+  return proto;
+}
+
+inline RangefinderPoint ToRangefinderPoint(
+    const TimedRangefinderPoint& timed_rangefinder_point) {
+  return {timed_rangefinder_point.position};
+}
+
+inline TimedRangefinderPoint ToTimedRangefinderPoint(
+    const RangefinderPoint& rangefinder_point, const float time) {
+  return {rangefinder_point.position, time};
+}
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_

cartographer/sensor/timed_point_cloud_data.cc

@@ -28,20 +28,26 @@ proto::TimedPointCloudData ToProto(
   proto.set_timestamp(common::ToUniversal(timed_point_cloud_data.time));
   *proto.mutable_origin() = transform::ToProto(timed_point_cloud_data.origin);
   proto.mutable_point_data()->Reserve(timed_point_cloud_data.ranges.size());
-  for (const Eigen::Vector4f& range : timed_point_cloud_data.ranges) {
+  for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) {
-    *proto.add_point_data() = transform::ToProto(range);
+    *proto.add_point_data() = ToProto(range);
   }
   return proto;
 }
 
 TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
   TimedPointCloud timed_point_cloud;
+  if (proto.point_data().size() > 0) {
     timed_point_cloud.reserve(proto.point_data().size());
-  std::transform(
+    for (const auto& timed_point_proto : proto.point_data()) {
-      proto.point_data().begin(), proto.point_data().end(),
+      timed_point_cloud.push_back(FromProto(timed_point_proto));
-      std::back_inserter(timed_point_cloud),
+    }
-      static_cast<Eigen::Vector4f (*)(const transform::proto::Vector4f&)>(
+  } else {
-          transform::ToEigen));
+    timed_point_cloud.reserve(proto.point_data_legacy().size());
+    for (const auto& timed_point_proto : proto.point_data_legacy()) {
+      const Eigen::Vector4f timed_point = transform::ToEigen(timed_point_proto);
+      timed_point_cloud.push_back({timed_point.head<3>(), timed_point[3]});
+    }
+  }
   return TimedPointCloudData{common::FromUniversal(proto.timestamp()),
                              transform::ToEigen(proto.origin()),
                              timed_point_cloud};

cartographer/sensor/timed_point_cloud_data.h

@@ -27,12 +27,12 @@ namespace sensor {
 struct TimedPointCloudData {
   common::Time time;
   Eigen::Vector3f origin;
-  sensor::TimedPointCloud ranges;
+  TimedPointCloud ranges;
 };
 
 struct TimedPointCloudOriginData {
   struct RangeMeasurement {
-    Eigen::Vector4f point_time;
+    TimedRangefinderPoint point_time;
     size_t origin_index;
   };
   common::Time time;