Add serialization of compressed range data. (#334)
parent
75d5b7a453
commit
f7af7ae238
|
@ -145,9 +145,16 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
|
||||||
CHECK_EQ(num_blocks, 0);
|
CHECK_EQ(num_blocks, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
CompressedPointCloud::CompressedPointCloud(const std::vector<int32>& point_data,
|
CompressedPointCloud::CompressedPointCloud(
|
||||||
size_t num_points)
|
const proto::CompressedPointCloud& proto) {
|
||||||
: point_data_(point_data), num_points_(num_points) {}
|
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; }
|
bool CompressedPointCloud::empty() const { return num_points_ == 0; }
|
||||||
|
|
||||||
|
@ -169,6 +176,12 @@ PointCloud CompressedPointCloud::Decompress() const {
|
||||||
return decompressed;
|
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 CompressedPointCloud::ToProto() const {
|
||||||
proto::CompressedPointCloud result;
|
proto::CompressedPointCloud result;
|
||||||
result.set_num_points(num_points_);
|
result.set_num_points(num_points_);
|
||||||
|
|
|
@ -39,6 +39,7 @@ class CompressedPointCloud {
|
||||||
|
|
||||||
CompressedPointCloud() : num_points_(0) {}
|
CompressedPointCloud() : num_points_(0) {}
|
||||||
explicit CompressedPointCloud(const PointCloud& point_cloud);
|
explicit CompressedPointCloud(const PointCloud& point_cloud);
|
||||||
|
explicit CompressedPointCloud(const proto::CompressedPointCloud& proto);
|
||||||
|
|
||||||
// Returns decompressed point cloud.
|
// Returns decompressed point cloud.
|
||||||
PointCloud Decompress() const;
|
PointCloud Decompress() const;
|
||||||
|
@ -48,11 +49,10 @@ class CompressedPointCloud {
|
||||||
ConstIterator begin() const;
|
ConstIterator begin() const;
|
||||||
ConstIterator end() const;
|
ConstIterator end() const;
|
||||||
|
|
||||||
|
bool operator==(const CompressedPointCloud& right_hand_container) const;
|
||||||
proto::CompressedPointCloud ToProto() const;
|
proto::CompressedPointCloud ToProto() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
CompressedPointCloud(const std::vector<int32>& point_data, size_t num_points);
|
|
||||||
|
|
||||||
std::vector<int32> point_data_;
|
std::vector<int32> point_data_;
|
||||||
size_t num_points_;
|
size_t num_points_;
|
||||||
};
|
};
|
||||||
|
|
|
@ -37,8 +37,8 @@ message CompressedPointCloud {
|
||||||
// Proto representation of ::cartographer::sensor::RangeData
|
// Proto representation of ::cartographer::sensor::RangeData
|
||||||
message RangeData {
|
message RangeData {
|
||||||
optional transform.proto.Vector3f origin = 1;
|
optional transform.proto.Vector3f origin = 1;
|
||||||
optional PointCloud point_cloud = 2;
|
optional PointCloud returns = 2;
|
||||||
optional PointCloud missing_echo_point_cloud = 3;
|
optional PointCloud misses = 3;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Proto representation of ::cartographer::sensor::ImuData
|
// Proto representation of ::cartographer::sensor::ImuData
|
||||||
|
@ -47,3 +47,10 @@ message ImuData {
|
||||||
optional transform.proto.Vector3d linear_acceleration = 2;
|
optional transform.proto.Vector3d linear_acceleration = 2;
|
||||||
optional transform.proto.Vector3d angular_velocity = 3;
|
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;
|
||||||
|
}
|
||||||
|
|
|
@ -25,17 +25,16 @@ namespace sensor {
|
||||||
proto::RangeData ToProto(const RangeData& range_data) {
|
proto::RangeData ToProto(const RangeData& range_data) {
|
||||||
proto::RangeData proto;
|
proto::RangeData proto;
|
||||||
*proto.mutable_origin() = transform::ToProto(range_data.origin);
|
*proto.mutable_origin() = transform::ToProto(range_data.origin);
|
||||||
*proto.mutable_point_cloud() = ToProto(range_data.returns);
|
*proto.mutable_returns() = ToProto(range_data.returns);
|
||||||
*proto.mutable_missing_echo_point_cloud() = ToProto(range_data.misses);
|
*proto.mutable_misses() = ToProto(range_data.misses);
|
||||||
return proto;
|
return proto;
|
||||||
}
|
}
|
||||||
|
|
||||||
RangeData FromProto(const proto::RangeData& proto) {
|
RangeData FromProto(const proto::RangeData& proto) {
|
||||||
auto range_data = RangeData{
|
return RangeData{
|
||||||
transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()),
|
transform::ToEigen(proto.origin()), ToPointCloud(proto.returns()),
|
||||||
ToPointCloud(proto.missing_echo_point_cloud()),
|
ToPointCloud(proto.misses()),
|
||||||
};
|
};
|
||||||
return range_data;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
RangeData TransformRangeData(const RangeData& 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)};
|
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) {
|
CompressedRangeData Compress(const RangeData& range_data) {
|
||||||
return CompressedRangeData{
|
return CompressedRangeData{
|
||||||
range_data.origin, CompressedPointCloud(range_data.returns),
|
range_data.origin, CompressedPointCloud(range_data.returns),
|
||||||
|
|
|
@ -55,6 +55,9 @@ struct CompressedRangeData {
|
||||||
CompressedPointCloud misses;
|
CompressedPointCloud misses;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
proto::CompressedRangeData ToProto(
|
||||||
|
const CompressedRangeData& compressed_range_data);
|
||||||
|
CompressedRangeData FromProto(const proto::CompressedRangeData& proto);
|
||||||
CompressedRangeData Compress(const RangeData& range_data);
|
CompressedRangeData Compress(const RangeData& range_data);
|
||||||
|
|
||||||
RangeData Decompress(const CompressedRangeData& compressed_range_Data);
|
RangeData Decompress(const CompressedRangeData& compressed_range_Data);
|
||||||
|
|
|
@ -16,7 +16,7 @@
|
||||||
|
|
||||||
#include "cartographer/sensor/range_data.h"
|
#include "cartographer/sensor/range_data.h"
|
||||||
|
|
||||||
#include <utility>
|
#include <tuple>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "gmock/gmock.h"
|
#include "gmock/gmock.h"
|
||||||
|
@ -28,30 +28,61 @@ namespace {
|
||||||
using ::testing::Contains;
|
using ::testing::Contains;
|
||||||
using ::testing::PrintToString;
|
using ::testing::PrintToString;
|
||||||
|
|
||||||
// Custom matcher for Eigen::Vector3f entries.
|
MATCHER(NearPointwise, std::string(negation ? "Doesn't" : "Does") + " match.") {
|
||||||
MATCHER_P(ApproximatelyEquals, expected,
|
return std::get<0>(arg).isApprox(std::get<1>(arg), 0.001f);
|
||||||
string("is equal to ") + PrintToString(expected)) {
|
|
||||||
return (arg - expected).isZero(0.001f);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(RangeDataTest, Compression) {
|
MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") {
|
||||||
const std::vector<Eigen::Vector3f> returns = {Eigen::Vector3f(0, 1, 2),
|
return arg.isApprox(point, 0.001f);
|
||||||
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));
|
|
||||||
|
|
||||||
// Returns will be reordered, so we compare in an unordered manner.
|
class RangeDataTest : public ::testing::Test {
|
||||||
EXPECT_EQ(3, actual.returns.size());
|
protected:
|
||||||
EXPECT_THAT(actual.returns,
|
RangeDataTest() : origin_(Eigen::Vector3f(1, 1, 1)) {
|
||||||
Contains(ApproximatelyEquals(Eigen::Vector3f(0, 1, 2))));
|
returns_.emplace_back(0, 1, 2);
|
||||||
EXPECT_THAT(actual.returns,
|
returns_.emplace_back(4, 5, 6);
|
||||||
Contains(ApproximatelyEquals(Eigen::Vector3f(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
|
} // namespace
|
||||||
|
|
Loading…
Reference in New Issue