Remove LaserFan in favor of LaserFan3D. (#62)

LaserFan3D will be renamed in the next PR.

cartographer/mapping/trajectory_node.h

@@ -35,7 +35,7 @@ struct TrajectoryNode {
     common::Time time;
 
     // LaserFan in 'pose' frame. Only used in the 2D case.
-    sensor::LaserFan laser_fan;
+    sensor::LaserFan3D laser_fan_2d;
 
     // LaserFan in 'pose' frame. Only used in the 3D case.
     sensor::CompressedLaserFan3D laser_fan_3d;

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -47,7 +47,7 @@ void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
   if (insertion_result != nullptr) {
     sparse_pose_graph_->AddScan(
         insertion_result->time, insertion_result->tracking_to_tracking_2d,
-        sensor::ProjectLaserFan(insertion_result->laser_fan_in_tracking_2d),
+        insertion_result->laser_fan_in_tracking_2d,
         insertion_result->pose_estimate_2d,
         kalman_filter::Project2D(insertion_result->covariance_estimate),
         insertion_result->submaps, insertion_result->matching_submap,

cartographer/mapping_2d/laser_fan_inserter.cc

@@ -51,7 +51,7 @@ LaserFanInserter::LaserFanInserter(
       miss_table_(mapping::ComputeLookupTableToApplyOdds(
           mapping::Odds(options.miss_probability()))) {}
 
-void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
+void LaserFanInserter::Insert(const sensor::LaserFan3D& laser_fan,
                               ProbabilityGrid* const probability_grid) const {
   CHECK_NOTNULL(probability_grid)->StartUpdate();
 

cartographer/mapping_2d/laser_fan_inserter.h

@@ -42,7 +42,7 @@ class LaserFanInserter {
   LaserFanInserter& operator=(const LaserFanInserter&) = delete;
 
   // Inserts 'laser_fan' into 'probability_grid'.
-  void Insert(const sensor::LaserFan& laser_fan,
+  void Insert(const sensor::LaserFan3D& laser_fan,
               ProbabilityGrid* probability_grid) const;
 
   const std::vector<uint16>& hit_table() const { return hit_table_; }

cartographer/mapping_2d/laser_fan_inserter_test.cc

@@ -44,11 +44,11 @@ class LaserFanInserterTest : public ::testing::Test {
   }
 
   void InsertPointCloud() {
-    sensor::LaserFan laser_fan;
+    sensor::LaserFan3D laser_fan;
-    laser_fan.point_cloud.emplace_back(-3.5, 0.5);
+    laser_fan.returns.emplace_back(-3.5, 0.5, 0.f);
-    laser_fan.point_cloud.emplace_back(-2.5, 1.5);
+    laser_fan.returns.emplace_back(-2.5, 1.5, 0.f);
-    laser_fan.point_cloud.emplace_back(-1.5, 2.5);
+    laser_fan.returns.emplace_back(-1.5, 2.5, 0.f);
-    laser_fan.point_cloud.emplace_back(-0.5, 3.5);
+    laser_fan.returns.emplace_back(-0.5, 3.5, 0.f);
     laser_fan.origin.x() = -0.5;
     laser_fan.origin.y() = 0.5;
     probability_grid_.StartUpdate();

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -212,8 +212,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
   for (int insertion_index : submaps_.insertion_indices()) {
     insertion_submaps.push_back(submaps_.Get(insertion_index));
   }
-  submaps_.InsertLaserFan(sensor::ProjectLaserFan(TransformLaserFan3D(
+  submaps_.InsertLaserFan(TransformLaserFan3D(
-      laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>())));
+      laser_fan_in_tracking_2d, tracking_2d_to_map.cast<float>()));
 
   return common::make_unique<InsertionResult>(InsertionResult{
       time, &submaps_, matching_submap, insertion_submaps,

cartographer/mapping_2d/map_limits.h

@@ -102,21 +102,21 @@ class MapLimits {
     for (const auto& node : trajectory_nodes) {
       const auto& data = *node.constant_data;
       if (!data.laser_fan_3d.returns.empty()) {
-        const sensor::LaserFan3D laser_fan_3d = sensor::TransformLaserFan3D(
+        const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D(
             Decompress(data.laser_fan_3d), node.pose.cast<float>());
-        bounding_box.extend(laser_fan_3d.origin.head<2>());
+        bounding_box.extend(laser_fan.origin.head<2>());
-        for (const Eigen::Vector3f& hit : laser_fan_3d.returns) {
+        for (const Eigen::Vector3f& hit : laser_fan.returns) {
           bounding_box.extend(hit.head<2>());
         }
       } else {
-        const sensor::LaserFan laser_fan = sensor::TransformLaserFan(
+        const sensor::LaserFan3D laser_fan = sensor::TransformLaserFan3D(
-            data.laser_fan, transform::Project2D(node.pose).cast<float>());
+            data.laser_fan_2d, node.pose.cast<float>());
-        bounding_box.extend(laser_fan.origin);
+        bounding_box.extend(laser_fan.origin.head<2>());
-        for (const Eigen::Vector2f& hit : laser_fan.point_cloud) {
+        for (const Eigen::Vector3f& hit : laser_fan.returns) {
-          bounding_box.extend(hit);
+          bounding_box.extend(hit.head<2>());
         }
-        for (const Eigen::Vector2f& miss : laser_fan.missing_echo_point_cloud) {
+        for (const Eigen::Vector3f& miss : laser_fan.misses) {
-          bounding_box.extend(miss);
+          bounding_box.extend(miss.head<2>());
         }
       }
     }

cartographer/mapping_2d/map_limits_test.cc

@@ -39,9 +39,9 @@ TEST(MapLimitsTest, ConstructAndGet) {
 TEST(MapLimitsTest, ComputeMapLimits) {
   const mapping::TrajectoryNode::ConstantData constant_data{
       common::FromUniversal(52),
-      sensor::LaserFan{
+      sensor::LaserFan3D{
-          Eigen::Vector2f::Zero(),
+          Eigen::Vector3f::Zero(),
-          {Eigen::Vector2f(-30.f, 1.f), Eigen::Vector2f(50.f, -10.f)},
+          {Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)},
           {}},
       Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}),
       nullptr, transform::Rigid3d::Identity()};

cartographer/mapping_2d/ray_casting.cc

@@ -147,7 +147,7 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
 
 }  // namespace
 
-void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
+void CastRays(const sensor::LaserFan3D& laser_fan, const MapLimits& limits,
               const std::function<void(const Eigen::Array2i&)>& hit_visitor,
               const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
   const double superscaled_resolution = limits.resolution() / kSubpixelScale;
@@ -161,8 +161,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
 
   // Compute and add the end points.
   std::vector<Eigen::Array2i> ends;
-  ends.reserve(laser_fan.point_cloud.size());
+  ends.reserve(laser_fan.returns.size());
-  for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
+  for (const Eigen::Vector3f& laser_return : laser_fan.returns) {
     ends.push_back(superscaled_limits.GetXYIndexOfCellContainingPoint(
         laser_return.x(), laser_return.y()));
     hit_visitor(ends.back() / kSubpixelScale);
@@ -174,8 +174,7 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
   }
 
   // Finally, compute and add empty rays based on missing echos in the scan.
-  for (const Eigen::Vector2f& missing_echo :
+  for (const Eigen::Vector3f& missing_echo : laser_fan.misses) {
-       laser_fan.missing_echo_point_cloud) {
     CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint(
                        missing_echo.x(), missing_echo.y()),
             miss_visitor);

cartographer/mapping_2d/ray_casting.h

@@ -30,7 +30,7 @@ namespace mapping_2d {
 
 // For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the
 // appropriate cells. Hits are handled before misses.
-void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
+void CastRays(const sensor::LaserFan3D& laser_fan, const MapLimits& limits,
               const std::function<void(const Eigen::Array2i&)>& hit_visitor,
               const std::function<void(const Eigen::Array2i&)>& miss_visitor);
 

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -150,10 +150,11 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
         MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
     probability_grid.StartUpdate();
     laser_fan_inserter.Insert(
-        sensor::LaserFan{
+        sensor::LaserFan3D{
-            expected_pose.translation(),
+            Eigen::Vector3f(expected_pose.translation().x(),
-            sensor::TransformPointCloud2D(
+                            expected_pose.translation().y(), 0.f),
-                sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
+            sensor::TransformPointCloud(
+                point_cloud, transform::Embed3D(expected_pose.cast<float>())),
             {}},
         &probability_grid);
 
@@ -192,11 +193,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
         {10. * distribution(prng), 10. * distribution(prng)},
         1.6 * distribution(prng));
     const sensor::PointCloud point_cloud = sensor::TransformPointCloud(
-        unperturbed_point_cloud,
+        unperturbed_point_cloud, transform::Embed3D(perturbation));
-        transform::Rigid3f(Eigen::Vector3f(perturbation.translation().x(),
-                                           perturbation.translation().y(), 0.f),
-                           Eigen::AngleAxisf(perturbation.rotation().angle(),
-                                             Eigen::Vector3f::UnitZ())));
     const transform::Rigid2f expected_pose =
         transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)},
                            0.5 * distribution(prng)) *
@@ -206,10 +203,10 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
         MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
     probability_grid.StartUpdate();
     laser_fan_inserter.Insert(
-        sensor::LaserFan{
+        sensor::LaserFan3D{
-            (expected_pose * perturbation).translation(),
+            transform::Embed3D(expected_pose * perturbation).translation(),
-            sensor::TransformPointCloud2D(
+            sensor::TransformPointCloud(point_cloud,
-                sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
+                                        transform::Embed3D(expected_pose)),
             {}},
         &probability_grid);
 

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -58,9 +58,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
     point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
     probability_grid_.StartUpdate();
     laser_fan_inserter_->Insert(
-        sensor::LaserFan{Eigen::Vector2f::Zero(),
+        sensor::LaserFan3D{Eigen::Vector3f::Zero(), point_cloud_, {}},
-                         sensor::ProjectToPointCloud2D(point_cloud_),
-                         {}},
         &probability_grid_);
     {
       auto parameter_dictionary = common::MakeDictionary(

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -86,7 +86,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     common::Time time, const transform::Rigid3d& tracking_to_pose,
-    const sensor::LaserFan& laser_fan_in_pose, const transform::Rigid2d& pose,
+    const sensor::LaserFan3D& laser_fan_in_pose, const transform::Rigid2d& pose,
     const kalman_filter::Pose2DCovariance& covariance,
     const mapping::Submaps* submaps,
     const mapping::Submap* const matching_submap,
@@ -145,7 +145,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
           point_cloud_poses_[scan_index];
       constraint_builder_.MaybeAddConstraint(
           submap_index, submap, scan_index,
-          &trajectory_nodes_[scan_index].constant_data->laser_fan.point_cloud,
+          &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns,
           relative_pose);
     }
   }
@@ -202,8 +202,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
         constraint_builder_.MaybeAddGlobalConstraint(
             submap_index, submap_states_[submap_index].submap, scan_index,
             scan_trajectory, submap_trajectory, &trajectory_connectivity_,
-            &trajectory_nodes_[scan_index]
+            &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns);
-                 .constant_data->laser_fan.point_cloud);
       } else {
         const bool scan_and_submap_trajectories_connected =
             reverse_connected_components_.count(scan_trajectory) > 0 &&
@@ -215,7 +214,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           constraint_builder_.MaybeAddConstraint(
               submap_index, submap_states_[submap_index].submap, scan_index,
               &trajectory_nodes_[scan_index]
-                   .constant_data->laser_fan.point_cloud,
+                   .constant_data->laser_fan_2d.returns,
               relative_pose);
         }
       }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -70,7 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // by scan matching against the 'matching_submap' and the scan was inserted
   // into the 'insertion_submaps'.
   void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
-               const sensor::LaserFan& laser_fan_in_pose,
+               const sensor::LaserFan3D& laser_fan_in_pose,
                const transform::Rigid2d& pose,
                const kalman_filter::Pose2DCovariance& pose_covariance,
                const mapping::Submaps* submaps,

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -62,7 +62,7 @@ ConstraintBuilder::~ConstraintBuilder() {
 
 void ConstraintBuilder::MaybeAddConstraint(
     const int submap_index, const mapping::Submap* const submap,
-    const int scan_index, const sensor::PointCloud2D* const point_cloud,
+    const int scan_index, const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
       options_.max_constraint_distance()) {
@@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
     const int scan_index, const mapping::Submaps* scan_trajectory,
     const mapping::Submaps* submap_trajectory,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
-    const sensor::PointCloud2D* const point_cloud) {
+    const sensor::PointCloud* const point_cloud) {
   common::MutexLocker locker(&mutex_);
   CHECK_LE(scan_index, current_computation_);
   constraints_.emplace_back();
@@ -174,7 +174,7 @@ void ConstraintBuilder::ComputeConstraint(
     const int scan_index, const mapping::Submaps* scan_trajectory,
     const mapping::Submaps* submap_trajectory, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
-    const sensor::PointCloud2D* const point_cloud,
+    const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose,
     std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
   const transform::Rigid2d initial_pose =
@@ -182,7 +182,7 @@ void ConstraintBuilder::ComputeConstraint(
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_index);
   const sensor::PointCloud filtered_point_cloud =
-      adaptive_voxel_filter_.Filter(sensor::ToPointCloud(*point_cloud));
+      adaptive_voxel_filter_.Filter(*point_cloud);
 
   // The 'constraint_transform' (i <- j) is computed from:
   // - a 'filtered_point_cloud' in j,

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -78,8 +78,7 @@ class ConstraintBuilder {
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddConstraint(int submap_index, const mapping::Submap* submap,
-                          int scan_index,
+                          int scan_index, const sensor::PointCloud* point_cloud,
-                          const sensor::PointCloud2D* point_cloud,
                           const transform::Rigid2d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
@@ -97,7 +96,7 @@ class ConstraintBuilder {
       const mapping::Submaps* scan_trajectory,
       const mapping::Submaps* submap_trajectory,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
-      const sensor::PointCloud2D* point_cloud);
+      const sensor::PointCloud* point_cloud);
 
   // Must be called after all computations related to 'scan_index' are added.
   void NotifyEndOfScan(const int scan_index);
@@ -142,7 +141,7 @@ class ConstraintBuilder {
       const mapping::Submaps* scan_trajectory,
       const mapping::Submaps* submap_trajectory, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
-      const sensor::PointCloud2D* point_cloud,
+      const sensor::PointCloud* point_cloud,
       const transform::Rigid2d& initial_relative_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -44,7 +44,7 @@ class SparsePoseGraphTest : 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));
+      point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f);
     }
 
     {
@@ -146,8 +146,9 @@ class SparsePoseGraphTest : public ::testing::Test {
   void MoveRelativeWithNoise(const transform::Rigid2d& movement,
                              const transform::Rigid2d& noise) {
     current_pose_ = current_pose_ * movement;
-    const sensor::PointCloud2D new_point_cloud = sensor::TransformPointCloud2D(
+    const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
-        point_cloud_, current_pose_.inverse().cast<float>());
+        point_cloud_,
+        transform::Embed3D(current_pose_.inverse().cast<float>()));
     kalman_filter::Pose2DCovariance covariance =
         kalman_filter::Pose2DCovariance::Identity();
     const mapping::Submap* const matching_submap =
@@ -156,11 +157,11 @@ class SparsePoseGraphTest : public ::testing::Test {
     for (int insertion_index : submaps_->insertion_indices()) {
       insertion_submaps.push_back(submaps_->Get(insertion_index));
     }
-    const sensor::LaserFan laser_fan{
+    const sensor::LaserFan3D laser_fan{
-        Eigen::Vector2f::Zero(), new_point_cloud, {}};
+        Eigen::Vector3f::Zero(), new_point_cloud, {}};
     const transform::Rigid2d pose_estimate = noise * current_pose_;
-    submaps_->InsertLaserFan(
+    submaps_->InsertLaserFan(TransformLaserFan3D(
-        TransformLaserFan(laser_fan, pose_estimate.cast<float>()));
+        laser_fan, transform::Embed3D(pose_estimate.cast<float>())));
     sparse_pose_graph_->AddScan(common::FromUniversal(0),
                                 transform::Rigid3d::Identity(), laser_fan,
                                 pose_estimate, covariance, submaps_.get(),
@@ -171,7 +172,7 @@ class SparsePoseGraphTest : public ::testing::Test {
     MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
   }
 
-  sensor::PointCloud2D point_cloud_;
+  sensor::PointCloud point_cloud_;
   std::unique_ptr<Submaps> submaps_;
   std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
   common::ThreadPool thread_pool_;

cartographer/mapping_2d/submaps.cc

@@ -114,7 +114,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
   AddSubmap(Eigen::Vector2f::Zero());
 }
 
-void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) {
+void Submaps::InsertLaserFan(const sensor::LaserFan3D& laser_fan) {
   CHECK_LT(num_laser_fans_, std::numeric_limits<int>::max());
   ++num_laser_fans_;
   for (const int index : insertion_indices()) {
@@ -125,7 +125,7 @@ void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) {
   }
   ++num_laser_fans_in_last_submap_;
   if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) {
-    AddSubmap(laser_fan.origin);
+    AddSubmap(laser_fan.origin.head<2>());
   }
 }
 

cartographer/mapping_2d/submaps.h

@@ -64,7 +64,7 @@ class Submaps : public mapping::Submaps {
       mapping::proto::SubmapQuery::Response* response) override;
 
   // Inserts 'laser_fan' into the Submap collection.
-  void InsertLaserFan(const sensor::LaserFan& laser_fan);
+  void InsertLaserFan(const sensor::LaserFan3D& laser_fan);
 
  private:
   void FinishSubmap(int index);

cartographer/mapping_2d/submaps_test.cc

@@ -51,7 +51,7 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
            submaps.Get(i)->begin_laser_fan_index;
   };
   for (int i = 0; i != 1000; ++i) {
-    submaps.InsertLaserFan({Eigen::Vector2f::Zero(), {}, {}});
+    submaps.InsertLaserFan({Eigen::Vector3f::Zero(), {}, {}});
     const int matching = submaps.matching_index();
     // Except for the first, maps should only be returned after enough scans.
     if (matching != 0) {

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -96,7 +96,7 @@ int SparsePoseGraph::AddScan(
   CHECK_LT(j, std::numeric_limits<int>::max());
 
   constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
-      time, sensor::LaserFan{Eigen::Vector2f::Zero(), {}, {}},
+      time, sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}},
       sensor::Compress(laser_fan_in_tracking), submaps,
       transform::Rigid3d::Identity()});
   trajectory_nodes_.push_back(

cartographer/sensor/CMakeLists.txt

@@ -121,6 +121,7 @@ google_test(sensor_point_cloud_test
     point_cloud_test.cc
   DEPENDS
     sensor_point_cloud
+    transform_transform
 )
 
 google_test(sensor_voxel_filter_test

cartographer/sensor/laser.cc

@@ -37,14 +37,6 @@ std::vector<uint8> ReorderReflectivities(
 
 }  // namespace
 
-LaserFan TransformLaserFan(const LaserFan& laser_fan,
-                           const transform::Rigid2f& transform) {
-  return LaserFan{
-      transform * laser_fan.origin,
-      TransformPointCloud2D(laser_fan.point_cloud, transform),
-      TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)};
-}
-
 LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range,
                         const float max_range,
                         const float missing_echo_ray_length) {
@@ -119,12 +111,6 @@ LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, const float min_z,
                     Crop(laser_fan.misses, min_z, max_z)};
 }
 
-LaserFan ProjectLaserFan(const LaserFan3D& laser_fan) {
-  return LaserFan{laser_fan.origin.head<2>(),
-                  ProjectToPointCloud2D(laser_fan.returns),
-                  ProjectToPointCloud2D(laser_fan.misses)};
-}
-
 CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {
   std::vector<int> new_to_old;
   CompressedPointCloud compressed_returns =

cartographer/sensor/laser.h

@@ -25,16 +25,6 @@
 namespace cartographer {
 namespace sensor {
 
-struct LaserFan {
-  Eigen::Vector2f origin;
-  PointCloud2D point_cloud;
-  PointCloud2D missing_echo_point_cloud;
-};
-
-// Transforms 'laser_fan' according to 'transform'.
-LaserFan TransformLaserFan(const LaserFan& laser_fan,
-                           const transform::Rigid2f& transform);
-
 // A 3D variant of LaserFan. Rays begin at 'origin'. 'returns' are the points
 // where laser returns were detected. 'misses' are points in the direction of
 // rays for which no return was detected, and were inserted at a configured
@@ -73,9 +63,6 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
 // Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'.
 LaserFan3D CropLaserFan(const LaserFan3D& laser_fan, float min_z, float max_z);
 
-// Projects 'laser_fan' into 2D.
-LaserFan ProjectLaserFan(const LaserFan3D& laser_fan);
-
 // Like LaserFan3D but with compressed point clouds. The point order changes
 // when converting from LaserFan3D.
 struct CompressedLaserFan3D {

cartographer/sensor/point_cloud.cc

@@ -22,49 +22,16 @@
 namespace cartographer {
 namespace sensor {
 
-namespace {
+PointCloud TransformPointCloud(const PointCloud& point_cloud,
-
+                               const transform::Rigid3f& transform) {
-template <typename PointCloudType, typename TransformType>
+  PointCloud result;
-PointCloudType Transform(const PointCloudType& point_cloud,
-                         const TransformType& transform) {
-  PointCloudType result;
   result.reserve(point_cloud.size());
-  for (const auto& point : point_cloud) {
+  for (const Eigen::Vector3f& point : point_cloud) {
     result.emplace_back(transform * point);
   }
   return result;
 }
 
-}  // namespace
-
-PointCloud TransformPointCloud(const PointCloud& point_cloud,
-                               const transform::Rigid3f& transform) {
-  return Transform(point_cloud, transform);
-}
-
-PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d,
-                                   const transform::Rigid2f& transform) {
-  return Transform(point_cloud_2d, transform);
-}
-
-PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d) {
-  sensor::PointCloud point_cloud;
-  point_cloud.reserve(point_cloud_2d.size());
-  for (const auto& point : point_cloud_2d) {
-    point_cloud.emplace_back(point.x(), point.y(), 0.f);
-  }
-  return point_cloud;
-}
-
-PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud) {
-  sensor::PointCloud2D point_cloud_2d;
-  point_cloud_2d.reserve(point_cloud.size());
-  for (const auto& point : point_cloud) {
-    point_cloud_2d.emplace_back(point.x(), point.y());
-  }
-  return point_cloud_2d;
-}
-
 PointCloud Crop(const PointCloud& point_cloud, const float min_z,
                 const float max_z) {
   PointCloud cropped_point_cloud;

cartographer/sensor/point_cloud.h

@@ -29,22 +29,11 @@ namespace cartographer {
 namespace sensor {
 
 typedef std::vector<Eigen::Vector3f> PointCloud;
-typedef std::vector<Eigen::Vector2f> PointCloud2D;
 
 // Transforms 'point_cloud' according to 'transform'.
 PointCloud TransformPointCloud(const PointCloud& point_cloud,
                                const transform::Rigid3f& transform);
 
-// Transforms 'point_cloud_2d' according to 'transform'.
-PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d,
-                                   const transform::Rigid2f& transform);
-
-// Converts 'point_cloud_2d' to a 3D point cloud.
-PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d);
-
-// Converts 'point_cloud' to a 2D point cloud by removing the z component.
-PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud);
-
 // Returns a new point cloud without points that fall outside the region defined
 // by 'min_z' and 'max_z'.
 PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);

cartographer/sensor/point_cloud_test.cc

@@ -15,6 +15,7 @@
  */
 
 #include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/transform.h"
 
 #include <cmath>
 
@@ -24,12 +25,12 @@ namespace cartographer {
 namespace sensor {
 namespace {
 
-TEST(PointCloudTest, TransformPointCloud2D) {
+TEST(PointCloudTest, TransformPointCloud) {
-  PointCloud2D point_cloud;
+  PointCloud point_cloud;
-  point_cloud.emplace_back(0.5f, 0.5f);
+  point_cloud.emplace_back(0.5f, 0.5f, 1.f);
-  point_cloud.emplace_back(3.5f, 0.5f);
+  point_cloud.emplace_back(3.5f, 0.5f, 42.f);
-  const PointCloud2D transformed_point_cloud =
+  const PointCloud transformed_point_cloud = TransformPointCloud(
-      TransformPointCloud2D(point_cloud, transform::Rigid2f::Rotation(M_PI_2));
+      point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
   EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6);
   EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
   EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);