Unify 2D/3D range data in trajectory nodes. (#396)

Since we only convert compressed range data to protos, we remove the
unused code to serialize (non-compressed) range data which should also
not be used in the future.

cartographer/mapping/map_builder.cc

@@ -175,13 +175,10 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
         range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
         range_data_proto->mutable_node_id()->set_node_index(node_index);
         const auto& data = *node_data[trajectory_id][node_index].constant_data;
-        if (!data.range_data_2d.returns.empty()) {
+        *range_data_proto->mutable_range_data() =
-          *range_data_proto->mutable_range_data_2d() =
+            sensor::ToProto(sensor::Compress(sensor::TransformRangeData(
-              sensor::ToProto(sensor::Compress(data.range_data_2d));
+                sensor::Decompress(data.range_data),
-        } else {
+                data.tracking_to_pose.inverse().cast<float>())));
-          *range_data_proto->mutable_range_data_3d() =
-              sensor::ToProto(data.range_data_3d);
-        }
         // TODO(whess): Only enable optionally? Resulting pbstream files will be
         // a lot larger now.
         writer->WriteProto(proto);

cartographer/mapping/proto/serialization.proto

@@ -28,8 +28,7 @@ message Submap {
 
 message RangeData {
   optional NodeId node_id = 1;
-  optional sensor.proto.CompressedRangeData range_data_2d = 2;
+  optional sensor.proto.CompressedRangeData range_data = 4;
-  optional sensor.proto.CompressedRangeData range_data_3d = 3;
 }
 
 message SerializedData {

cartographer/mapping/trajectory_node.h

@@ -32,11 +32,8 @@ struct TrajectoryNode {
   struct Data {
     common::Time time;
 
-    // Range data in 'pose' frame. Only used in the 2D case.
+    // Range data in 'pose' frame.
-    sensor::RangeData range_data_2d;
+    sensor::CompressedRangeData range_data;
-
-    // Range data in 'pose' frame. Only used in the 3D case.
-    sensor::CompressedRangeData range_data_3d;
 
     // Transform from the 3D 'tracking' frame to the 'pose' frame of the range
     // data, which contains roll, pitch and height for 2D. In 3D this is always

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -113,10 +113,8 @@ void SparsePoseGraph::AddScan(
       trajectory_id,
       mapping::TrajectoryNode{
           std::make_shared<const mapping::TrajectoryNode::Data>(
-              mapping::TrajectoryNode::Data{
+              mapping::TrajectoryNode::Data{time, Compress(range_data_in_pose),
-                  time, range_data_in_pose,
+                                            tracking_to_pose}),
-                  Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
-                  tracking_to_pose}),
           optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
@@ -178,7 +176,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
       global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-        &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
+        &trajectory_nodes_.at(node_id).constant_data->range_data.returns,
         &trajectory_connectivity_);
   } else {
     const bool scan_and_submap_trajectories_connected =
@@ -202,7 +200,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
               .point_cloud_pose;
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-          &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
+          &trajectory_nodes_.at(node_id).constant_data->range_data.returns,
           initial_relative_pose);
     }
   }

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -61,7 +61,8 @@ ConstraintBuilder::~ConstraintBuilder() {
 
 void ConstraintBuilder::MaybeAddConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
+    const mapping::NodeId& node_id,
+    const sensor::CompressedPointCloud* const compressed_point_cloud,
     const transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
       options_.max_constraint_distance()) {
@@ -75,10 +76,10 @@ void ConstraintBuilder::MaybeAddConstraint(
     const int current_computation = current_computation_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
-          ComputeConstraint(submap_id, submap, node_id,
+          ComputeConstraint(
-                            false,   /* match_full_submap */
+              submap_id, submap, node_id, false, /* match_full_submap */
-                            nullptr, /* trajectory_connectivity */
+              nullptr,                           /* trajectory_connectivity */
-                            point_cloud, initial_relative_pose, constraint);
+              compressed_point_cloud, initial_relative_pose, constraint);
           FinishComputation(current_computation);
         });
   }
@@ -86,7 +87,8 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
+    const mapping::NodeId& node_id,
+    const sensor::CompressedPointCloud* const compressed_point_cloud,
     mapping::TrajectoryConnectivity* const trajectory_connectivity) {
   common::MutexLocker locker(&mutex_);
   constraints_.emplace_back();
@@ -97,7 +99,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
       submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
         ComputeConstraint(submap_id, submap, node_id,
                           true, /* match_full_submap */
-                          trajectory_connectivity, point_cloud,
+                          trajectory_connectivity, compressed_point_cloud,
                           transform::Rigid2d::Identity(), constraint);
         FinishComputation(current_computation);
       });
@@ -162,7 +164,7 @@ void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
-    const sensor::PointCloud* const point_cloud,
+    const sensor::CompressedPointCloud* const compressed_point_cloud,
     const transform::Rigid2d& initial_relative_pose,
     std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
   const transform::Rigid2d initial_pose =
@@ -170,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint(
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);
   const sensor::PointCloud filtered_point_cloud =
-      adaptive_voxel_filter_.Filter(*point_cloud);
+      adaptive_voxel_filter_.Filter(compressed_point_cloud->Decompress());
 
   // The 'constraint_transform' (submap i <- scan j) is computed from:
   // - a 'filtered_point_cloud' in scan j,

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -72,27 +72,29 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is
+  // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
-  // relative to the 'submap'.
+  // 'initial_relative_pose' is relative to the 'submap'.
   //
-  // The pointees of 'submap' and 'point_cloud' must stay valid until all
+  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
-  // computations are finished.
+  // all computations are finished.
-  void MaybeAddConstraint(const mapping::SubmapId& submap_id,
+  void MaybeAddConstraint(
-                          const Submap* submap, const mapping::NodeId& node_id,
+      const mapping::SubmapId& submap_id, const Submap* submap,
-                          const sensor::PointCloud* point_cloud,
+      const mapping::NodeId& node_id,
-                          const transform::Rigid2d& initial_relative_pose);
+      const sensor::CompressedPointCloud* compressed_point_cloud,
+      const transform::Rigid2d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap
+  // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
-  // matching.
+  // This performs full-submap matching.
   //
   // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
   //
-  // The pointees of 'submap' and 'point_cloud' must stay valid until all
+  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
-  // computations are finished.
+  // all computations are finished.
   void MaybeAddGlobalConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
-      const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
+      const mapping::NodeId& node_id,
+      const sensor::CompressedPointCloud* compressed_point_cloud,
       mapping::TrajectoryConnectivity* trajectory_connectivity);
 
   // Must be called after all computations related to one node have been added.
@@ -131,16 +133,16 @@ class ConstraintBuilder {
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
 
   // Runs in a background thread and does computations for an additional
-  // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
+  // constraint, assuming 'submap' and 'compressed_point_cloud' do not change
-  // If 'match_full_submap' is true, and global localization succeeds, will
+  // anymore. If 'match_full_submap' is true, and global localization succeeds,
-  // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
+  // will connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
       const mapping::NodeId& node_id, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
-      const sensor::PointCloud* point_cloud,
+      const sensor::CompressedPointCloud* compressed_point_cloud,
       const transform::Rigid2d& initial_relative_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 

cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc

@@ -150,7 +150,7 @@ RotationalScanMatcher::RotationalScanMatcher(
   for (const mapping::TrajectoryNode& node : nodes) {
     AddValuesToHistogram(
         GetValuesForHistogram(sensor::TransformPointCloud(
-            node.constant_data->range_data_3d.returns.Decompress(),
+            node.constant_data->range_data.returns.Decompress(),
             node.pose.cast<float>())),
         0.f, &histogram_);
   }

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -100,14 +100,12 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * pose);
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
-      trajectory_id,
+      trajectory_id, mapping::TrajectoryNode{
-      mapping::TrajectoryNode{
+                         std::make_shared<const mapping::TrajectoryNode::Data>(
-          std::make_shared<const mapping::TrajectoryNode::Data>(
+                             mapping::TrajectoryNode::Data{
-              mapping::TrajectoryNode::Data{
+                                 time, sensor::Compress(range_data_in_tracking),
-                  time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
+                                 transform::Rigid3d::Identity()}),
-                  sensor::Compress(range_data_in_tracking),
+                         optimized_pose});
-                  transform::Rigid3d::Identity()}),
-          optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 
@@ -200,7 +198,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-        &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
+        &trajectory_nodes_.at(node_id).constant_data->range_data.returns,
         submap_nodes, initial_relative_pose.rotation(),
         &trajectory_connectivity_);
   } else {
@@ -213,7 +211,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-          &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
+          &trajectory_nodes_.at(node_id).constant_data->range_data.returns,
           submap_nodes, initial_relative_pose);
     }
   }

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -68,8 +68,8 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id', and the 'point_cloud' for 'node_id'. The 'intial_pose' is
+  // 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
-  // relative to the 'submap'.
+  // The 'initial_pose' is relative to the 'submap'.
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.

cartographer/sensor/point_cloud.cc

@@ -43,25 +43,5 @@ PointCloud Crop(const PointCloud& point_cloud, const float min_z,
   return cropped_point_cloud;
 }
 
-proto::PointCloud ToProto(const PointCloud& point_cloud) {
-  proto::PointCloud proto;
-  for (const auto& point : point_cloud) {
-    proto.add_x(point.x());
-    proto.add_y(point.y());
-    proto.add_z(point.z());
-  }
-  return proto;
-}
-
-PointCloud ToPointCloud(const proto::PointCloud& proto) {
-  PointCloud point_cloud;
-  const int size = std::min({proto.x_size(), proto.y_size(), proto.z_size()});
-  point_cloud.reserve(size);
-  for (int i = 0; i != size; ++i) {
-    point_cloud.emplace_back(proto.x(i), proto.y(i), proto.z(i));
-  }
-  return point_cloud;
-}
-
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/point_cloud.h

@@ -42,12 +42,6 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
 // by 'min_z' and 'max_z'.
 PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);
 
-// Converts 'point_cloud' to a proto::PointCloud.
-proto::PointCloud ToProto(const PointCloud& point_cloud);
-
-// Converts 'proto' to a PointCloud.
-PointCloud ToPointCloud(const proto::PointCloud& proto);
-
 }  // namespace sensor
 }  // namespace cartographer
 

cartographer/sensor/proto/sensor.proto

@@ -20,27 +20,12 @@ option java_outer_classname = "Sensor";
 
 import "cartographer/transform/proto/transform.proto";
 
-// Collection of 3D 'points'.
+// Compressed collection of 3D 'points'.
-message PointCloud {
-  // Points as repeated floats for efficiency. All fields have the same size.
-  repeated float x = 3 [packed = true];
-  repeated float y = 4 [packed = true];
-  repeated float z = 5 [packed = true];
-}
-
-// Compressed variant of PointCloud.
 message CompressedPointCloud {
   optional int32 num_points = 1;
   repeated int32 point_data = 3 [packed = true];
 }
 
-// Proto representation of ::cartographer::sensor::RangeData
-message RangeData {
-  optional transform.proto.Vector3f origin = 1;
-  optional PointCloud returns = 2;
-  optional PointCloud misses = 3;
-}
-
 // Proto representation of ::cartographer::sensor::ImuData
 message ImuData {
   optional int64 timestamp = 1;

cartographer/sensor/range_data.cc

@@ -22,21 +22,6 @@
 namespace cartographer {
 namespace sensor {
 
-proto::RangeData ToProto(const RangeData& range_data) {
-  proto::RangeData proto;
-  *proto.mutable_origin() = transform::ToProto(range_data.origin);
-  *proto.mutable_returns() = ToProto(range_data.returns);
-  *proto.mutable_misses() = ToProto(range_data.misses);
-  return proto;
-}
-
-RangeData FromProto(const proto::RangeData& proto) {
-  return RangeData{
-      transform::ToEigen(proto.origin()), ToPointCloud(proto.returns()),
-      ToPointCloud(proto.misses()),
-  };
-}
-
 RangeData TransformRangeData(const RangeData& range_data,
                              const transform::Rigid3f& transform) {
   return RangeData{

cartographer/sensor/range_data.h

@@ -35,12 +35,6 @@ struct RangeData {
   PointCloud misses;
 };
 
-// Converts 'range_data' to a proto::RangeData.
-proto::RangeData ToProto(const RangeData& range_data);
-
-// Converts 'proto' to a RangeData.
-RangeData FromProto(const proto::RangeData& proto);
-
 RangeData TransformRangeData(const RangeData& range_data,
                              const transform::Rigid3f& transform);
 

cartographer/sensor/range_data_test.cc

@@ -65,17 +65,6 @@ TEST_F(RangeDataTest, Compression) {
   }
 }
 
-TEST_F(RangeDataTest, RangeDataToAndFromProto) {
-  const auto expected = RangeData{origin_, returns_, misses_};
-  const auto actual = FromProto(ToProto(expected));
-
-  EXPECT_THAT(expected.origin, Near(actual.origin));
-  EXPECT_THAT(expected.returns,
-              testing::Pointwise(NearPointwise(), actual.returns));
-  EXPECT_THAT(expected.misses,
-              testing::Pointwise(NearPointwise(), actual.misses));
-}
-
 TEST_F(RangeDataTest, CompressedRangeDataToAndFromProto) {
   const auto expected = CompressedRangeData{
       origin_, CompressedPointCloud(returns_), CompressedPointCloud(misses_)};