Add serialization of compressed range data. (#334)

cartographer/sensor/compressed_point_cloud.cc

@@ -145,9 +145,16 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
   CHECK_EQ(num_blocks, 0);
 }
 
-CompressedPointCloud::CompressedPointCloud(const std::vector<int32>& point_data,
-                                           size_t num_points)
-    : point_data_(point_data), num_points_(num_points) {}
+CompressedPointCloud::CompressedPointCloud(
+    const proto::CompressedPointCloud& proto) {
+  num_points_ = proto.num_points();
+  const int data_size = proto.point_data_size();
+  point_data_.reserve(data_size);
+  // TODO(wohe): Verify that 'point_data_' does not contain malformed data.
+  for (int i = 0; i != data_size; ++i) {
+    point_data_.emplace_back(proto.point_data(i));
+  }
+}
 
 bool CompressedPointCloud::empty() const { return num_points_ == 0; }
 
@@ -169,6 +176,12 @@ PointCloud CompressedPointCloud::Decompress() const {
   return decompressed;
 }
 
+bool sensor::CompressedPointCloud::operator==(
+    const sensor::CompressedPointCloud& right_hand_container) const {
+  return point_data_ == right_hand_container.point_data_ &&
+         num_points_ == right_hand_container.num_points_;
+}
+
 proto::CompressedPointCloud CompressedPointCloud::ToProto() const {
   proto::CompressedPointCloud result;
   result.set_num_points(num_points_);

cartographer/sensor/compressed_point_cloud.h

@@ -39,6 +39,7 @@ class CompressedPointCloud {
 
   CompressedPointCloud() : num_points_(0) {}
   explicit CompressedPointCloud(const PointCloud& point_cloud);
+  explicit CompressedPointCloud(const proto::CompressedPointCloud& proto);
 
   // Returns decompressed point cloud.
   PointCloud Decompress() const;
@@ -48,11 +49,10 @@ class CompressedPointCloud {
   ConstIterator begin() const;
   ConstIterator end() const;
 
+  bool operator==(const CompressedPointCloud& right_hand_container) const;
   proto::CompressedPointCloud ToProto() const;
 
  private:
-  CompressedPointCloud(const std::vector<int32>& point_data, size_t num_points);
-
   std::vector<int32> point_data_;
   size_t num_points_;
 };

cartographer/sensor/proto/sensor.proto

@@ -37,8 +37,8 @@ message CompressedPointCloud {
 // Proto representation of ::cartographer::sensor::RangeData
 message RangeData {
   optional transform.proto.Vector3f origin = 1;
-  optional PointCloud point_cloud = 2;
-  optional PointCloud missing_echo_point_cloud = 3;
+  optional PointCloud returns = 2;
+  optional PointCloud misses = 3;
 }
 
 // Proto representation of ::cartographer::sensor::ImuData
@@ -47,3 +47,10 @@ message ImuData {
   optional transform.proto.Vector3d linear_acceleration = 2;
   optional transform.proto.Vector3d angular_velocity = 3;
 }
+
+// Proto representation of ::cartographer::sensor::CompressedRangeData
+message CompressedRangeData {
+  optional transform.proto.Vector3f origin = 1;
+  optional CompressedPointCloud returns = 2;
+  optional CompressedPointCloud misses = 3;
+}

cartographer/sensor/range_data.cc

@@ -25,17 +25,16 @@ namespace sensor {
 proto::RangeData ToProto(const RangeData& range_data) {
   proto::RangeData proto;
   *proto.mutable_origin() = transform::ToProto(range_data.origin);
-  *proto.mutable_point_cloud() = ToProto(range_data.returns);
-  *proto.mutable_missing_echo_point_cloud() = ToProto(range_data.misses);
+  *proto.mutable_returns() = ToProto(range_data.returns);
+  *proto.mutable_misses() = ToProto(range_data.misses);
   return proto;
 }
 
 RangeData FromProto(const proto::RangeData& proto) {
-  auto range_data = RangeData{
-      transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()),
-      ToPointCloud(proto.missing_echo_point_cloud()),
+  return RangeData{
+      transform::ToEigen(proto.origin()), ToPointCloud(proto.returns()),
+      ToPointCloud(proto.misses()),
   };
-  return range_data;
 }
 
 RangeData TransformRangeData(const RangeData& range_data,
@@ -53,6 +52,22 @@ RangeData CropRangeData(const RangeData& range_data, const float min_z,
                    Crop(range_data.misses, min_z, max_z)};
 }
 
+proto::CompressedRangeData ToProto(
+    const CompressedRangeData& compressed_range_data) {
+  proto::CompressedRangeData proto;
+  *proto.mutable_origin() = transform::ToProto(compressed_range_data.origin);
+  *proto.mutable_returns() = compressed_range_data.returns.ToProto();
+  *proto.mutable_misses() = compressed_range_data.misses.ToProto();
+  return proto;
+}
+
+CompressedRangeData FromProto(const proto::CompressedRangeData& proto) {
+  return CompressedRangeData{
+      transform::ToEigen(proto.origin()), CompressedPointCloud(proto.returns()),
+      CompressedPointCloud(proto.misses()),
+  };
+}
+
 CompressedRangeData Compress(const RangeData& range_data) {
   return CompressedRangeData{
       range_data.origin, CompressedPointCloud(range_data.returns),

cartographer/sensor/range_data.h

@@ -55,6 +55,9 @@ struct CompressedRangeData {
   CompressedPointCloud misses;
 };
 
+proto::CompressedRangeData ToProto(
+    const CompressedRangeData& compressed_range_data);
+CompressedRangeData FromProto(const proto::CompressedRangeData& proto);
 CompressedRangeData Compress(const RangeData& range_data);
 
 RangeData Decompress(const CompressedRangeData& compressed_range_Data);

cartographer/sensor/range_data_test.cc

@@ -16,7 +16,7 @@
 
 #include "cartographer/sensor/range_data.h"
 
-#include <utility>
+#include <tuple>
 #include <vector>
 
 #include "gmock/gmock.h"
@@ -28,30 +28,61 @@ namespace {
 using ::testing::Contains;
 using ::testing::PrintToString;
 
-// Custom matcher for Eigen::Vector3f entries.
-MATCHER_P(ApproximatelyEquals, expected,
-          string("is equal to ") + PrintToString(expected)) {
-  return (arg - expected).isZero(0.001f);
+MATCHER(NearPointwise, std::string(negation ? "Doesn't" : "Does") + " match.") {
+  return std::get<0>(arg).isApprox(std::get<1>(arg), 0.001f);
 }
 
-TEST(RangeDataTest, Compression) {
-  const std::vector<Eigen::Vector3f> returns = {Eigen::Vector3f(0, 1, 2),
-                                                Eigen::Vector3f(4, 5, 6),
-                                                Eigen::Vector3f(0, 1, 2)};
-  const RangeData range_data = {
-      Eigen::Vector3f(1, 1, 1), returns, {Eigen::Vector3f(7, 8, 9)}};
-  const RangeData actual = Decompress(Compress(range_data));
-  EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
-  EXPECT_EQ(3, actual.returns.size());
-  EXPECT_EQ(1, actual.misses.size());
-  EXPECT_TRUE(actual.misses[0].isApprox(Eigen::Vector3f(7, 8, 9), 0.001f));
+MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") {
+  return arg.isApprox(point, 0.001f);
+}
 
-  // Returns will be reordered, so we compare in an unordered manner.
-  EXPECT_EQ(3, actual.returns.size());
-  EXPECT_THAT(actual.returns,
-              Contains(ApproximatelyEquals(Eigen::Vector3f(0, 1, 2))));
-  EXPECT_THAT(actual.returns,
-              Contains(ApproximatelyEquals(Eigen::Vector3f(4, 5, 6))));
+class RangeDataTest : public ::testing::Test {
+ protected:
+  RangeDataTest() : origin_(Eigen::Vector3f(1, 1, 1)) {
+    returns_.emplace_back(0, 1, 2);
+    returns_.emplace_back(4, 5, 6);
+    returns_.emplace_back(0, 1, 2);
+    misses_.emplace_back(7, 8, 9);
+  }
+  Eigen::Vector3f origin_;
+  std::vector<Eigen::Vector3f> returns_;
+  std::vector<Eigen::Vector3f> misses_;
+};
+
+TEST_F(RangeDataTest, Compression) {
+  const RangeData expected_data = {origin_, returns_, misses_};
+  const RangeData actual_data = Decompress(Compress(expected_data));
+  EXPECT_THAT(expected_data.origin, Near(actual_data.origin));
+  EXPECT_EQ(3, actual_data.returns.size());
+  EXPECT_EQ(1, actual_data.misses.size());
+
+  // Returns may be reordered, so we compare in an unordered manner.
+  for (const auto& expected : expected_data.returns) {
+    EXPECT_THAT(actual_data.returns, Contains(Near(expected)));
+  }
+  for (const auto& expected : expected_data.misses) {
+    EXPECT_THAT(actual_data.misses, Contains(Near(expected)));
+  }
+}
+
+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_)};
+  const auto actual = FromProto(ToProto(expected));
+  EXPECT_THAT(expected.origin, Near(actual.origin));
+  EXPECT_EQ(expected.returns, actual.returns);
+  EXPECT_EQ(expected.misses, actual.misses);
 }
 
 }  // namespace