compressed_point_cloud_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include "gmock/gmock.h"
20 
21 namespace Eigen {
22 
23 // Prints Vector3f in a readable format in matcher ApproximatelyEquals when
24 // failing a test. Without this function, the output is formated as hexadecimal
25 // 8 bit numbers.
26 void PrintTo(const Vector3f& x, std::ostream* os) {
27  *os << "(" << x[0] << ", " << x[1] << ", " << x[2] << ")";
28 }
29 
30 } // namespace Eigen
31 
32 namespace cartographer {
33 namespace sensor {
34 namespace {
35 
36 using ::testing::Contains;
37 using ::testing::FloatNear;
38 using ::testing::PrintToString;
39 
40 constexpr float kPrecision = 0.001f;
41 
42 // Matcher for 3-d vectors w.r.t. to the target precision.
43 MATCHER_P(ApproximatelyEquals, expected,
44  std::string("is equal to ") + PrintToString(expected)) {
45  return (arg - expected).isZero(kPrecision);
46 }
47 
48 // Helper function to test the mapping of a single point. Includes test for
49 // recompressing the same point again.
50 void TestPoint(const Eigen::Vector3f& p) {
51  CompressedPointCloud compressed({p});
52  EXPECT_EQ(1, compressed.size());
53  EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p));
54  CompressedPointCloud recompressed({*compressed.begin()});
55  EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p));
56 }
57 
58 TEST(CompressPointCloudTest, CompressesPointsCorrectly) {
59  TestPoint(Eigen::Vector3f(8000.f, 7500.f, 5000.f));
60  TestPoint(Eigen::Vector3f(1000.f, 2000.f, 3000.f));
61  TestPoint(Eigen::Vector3f(100.f, 200.f, 300.f));
62  TestPoint(Eigen::Vector3f(10.f, 20.f, 30.f));
63  TestPoint(Eigen::Vector3f(-0.00049f, -0.0005f, -0.0015f));
64  TestPoint(Eigen::Vector3f(0.05119f, 0.0512f, 0.05121));
65  TestPoint(Eigen::Vector3f(-0.05119f, -0.0512f, -0.05121));
66  TestPoint(Eigen::Vector3f(0.8405f, 0.84f, 0.8396f));
67  TestPoint(Eigen::Vector3f(0.8395f, 0.8394f, 0.8393f));
68  TestPoint(Eigen::Vector3f(0.839f, 0.8391f, 0.8392f));
69  TestPoint(Eigen::Vector3f(0.8389f, 0.8388f, 0.83985f));
70 }
71 
72 TEST(CompressPointCloudTest, Compresses) {
73  const CompressedPointCloud compressed({Eigen::Vector3f(0.838f, 0, 0),
74  Eigen::Vector3f(0.839f, 0, 0),
75  Eigen::Vector3f(0.840f, 0, 0)});
76  EXPECT_FALSE(compressed.empty());
77  EXPECT_EQ(3, compressed.size());
78  const PointCloud decompressed = compressed.Decompress();
79  EXPECT_EQ(3, decompressed.size());
80  EXPECT_THAT(decompressed,
81  Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0))));
82  EXPECT_THAT(decompressed,
83  Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0))));
84  EXPECT_THAT(decompressed,
85  Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0))));
86 }
87 
88 TEST(CompressPointCloudTest, CompressesEmptyPointCloud) {
89  CompressedPointCloud compressed;
90  EXPECT_TRUE(compressed.empty());
91  EXPECT_EQ(0, compressed.size());
92 }
93 
94 // Test for gaps.
95 // Produces a series of points densly packed along the x axis, compresses these
96 // points (twice), and tests, whether there are gaps between two consecutive
97 // points.
98 TEST(CompressPointCloudTest, CompressesNoGaps) {
99  PointCloud point_cloud;
100  for (int i = 0; i < 3000; ++i) {
101  point_cloud.push_back(Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0));
102  }
103  const CompressedPointCloud compressed(point_cloud);
104  const PointCloud decompressed = compressed.Decompress();
105  const CompressedPointCloud recompressed(decompressed);
106  EXPECT_EQ(decompressed.size(), recompressed.size());
107 
108  std::vector<float> x_coord;
109  for (const Eigen::Vector3f& p : compressed) {
110  x_coord.push_back(p[0]);
111  }
112  std::sort(x_coord.begin(), x_coord.end());
113  for (size_t i = 1; i < x_coord.size(); ++i) {
114  EXPECT_THAT(std::abs(x_coord[i] - x_coord[i - 1]),
115  FloatNear(kPrecision, 1e-7f));
116  }
117 }
118 
119 } // namespace
120 } // namespace sensor
121 } // namespace cartographer
void PrintTo(const Vector3f &x, std::ostream *os)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:32
MATCHER_P(Near, point, std::string(negation ? "Doesn't" :"Does")+" match.")
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58