From f7af7ae238295cfd92c194973c09b1a95830c574 Mon Sep 17 00:00:00 2001 From: "Brandon D. Northcutt" Date: Fri, 7 Jul 2017 03:26:36 -0700 Subject: [PATCH] Add serialization of compressed range data. (#334) --- cartographer/sensor/compressed_point_cloud.cc | 19 ++++- cartographer/sensor/compressed_point_cloud.h | 4 +- cartographer/sensor/proto/sensor.proto | 11 ++- cartographer/sensor/range_data.cc | 27 +++++-- cartographer/sensor/range_data.h | 3 + cartographer/sensor/range_data_test.cc | 75 +++++++++++++------ 6 files changed, 104 insertions(+), 35 deletions(-) diff --git a/cartographer/sensor/compressed_point_cloud.cc b/cartographer/sensor/compressed_point_cloud.cc index 933bc77..f98c619 100644 --- a/cartographer/sensor/compressed_point_cloud.cc +++ b/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& 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_); diff --git a/cartographer/sensor/compressed_point_cloud.h b/cartographer/sensor/compressed_point_cloud.h index 8729ffa..03b5c00 100644 --- a/cartographer/sensor/compressed_point_cloud.h +++ b/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& point_data, size_t num_points); - std::vector point_data_; size_t num_points_; }; diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index ee59637..354532c 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/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; +} diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index c9299ad..db23de0 100644 --- a/cartographer/sensor/range_data.cc +++ b/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), diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index bb73f23..91001f7 100644 --- a/cartographer/sensor/range_data.h +++ b/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); diff --git a/cartographer/sensor/range_data_test.cc b/cartographer/sensor/range_data_test.cc index 1a29048..99a3878 100644 --- a/cartographer/sensor/range_data_test.cc +++ b/cartographer/sensor/range_data_test.cc @@ -16,7 +16,7 @@ #include "cartographer/sensor/range_data.h" -#include +#include #include #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 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 returns_; + std::vector 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