MRPT  2.0.4
CPointsMap_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
15 #include <mrpt/poses/CPoint2D.h>
16 #include <sstream>
17 
18 using namespace mrpt;
19 using namespace mrpt::maps;
20 using namespace mrpt::obs;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 using namespace std;
24 
25 const size_t demo9_N = 9;
26 const float demo9_xs[demo9_N] = {0, 0, 0, 1, 1, 1, 2, 2, 2};
27 const float demo9_ys[demo9_N] = {0, 1, 2, 0, 1, 2, 0, 1, 2};
28 const float demo9_zs[demo9_N] = {0, 1, 2, 0, 1, 2, 0, 1, 2};
29 
30 template <class MAP>
31 void load_demo_9pts_map(MAP& pts)
32 {
33  pts.clear();
34  for (size_t i = 0; i < demo9_N; i++)
35  pts.insertPoint(demo9_xs[i], demo9_ys[i], demo9_zs[i]);
36 }
37 
38 template <class MAP>
40 {
41  // test 1: Insert and check expected values:
42  {
43  MAP pts;
44  load_demo_9pts_map(pts);
45 
46  EXPECT_EQ(pts.size(), demo9_N);
47 
48  for (size_t i = 0; i < demo9_N; i++)
49  {
50  float x, y, z;
51  pts.getPoint(i, x, y, z);
52  EXPECT_EQ(x, demo9_xs[i]);
53  EXPECT_EQ(y, demo9_ys[i]);
54  EXPECT_EQ(z, demo9_zs[i]);
55  }
56  }
57 
58  // test 2: Copy between maps
59  {
60  MAP pts1;
61  load_demo_9pts_map(pts1);
62 
63  MAP pts2 = pts1;
64  MAP pts3 = pts1;
65 
66  EXPECT_EQ(pts2.size(), pts3.size());
67  for (size_t i = 0; i < demo9_N; i++)
68  {
69  float x2, y2, z2;
70  float x3, y3, z3;
71  pts2.getPoint(i, x2, y2, z2);
72  pts3.getPoint(i, x3, y3, z3);
73  EXPECT_EQ(x2, x3);
74  EXPECT_EQ(y2, y3);
75  EXPECT_EQ(z2, z3);
76  }
77  }
78 }
79 
80 template <class MAP>
82 {
83  MAP pts0;
84  load_demo_9pts_map(pts0);
85 
86  // Clip: z=[-10,-1] -> 0 pts
87  {
88  MAP pts = pts0;
89  pts.clipOutOfRangeInZ(-10, -1);
90  EXPECT_EQ(pts.size(), 0u);
91  }
92 
93  // Clip: z=[-10,10] -> 9 pts
94  {
95  MAP pts = pts0;
96  pts.clipOutOfRangeInZ(-10, 10);
97  EXPECT_EQ(pts.size(), 9u);
98  }
99 
100  // Clip: z=[0.5,1.5] -> 3 pts
101  {
102  MAP pts = pts0;
103  pts.clipOutOfRangeInZ(0.5, 1.5);
104  EXPECT_EQ(pts.size(), 3u);
105  }
106 }
107 
108 template <class MAP>
110 {
111  MAP pts0;
112  load_demo_9pts_map(pts0);
113 
114  // Clip:
115  {
116  TPoint2D pivot(0, 0);
117  const float radius = 0.5;
118 
119  MAP pts = pts0;
120  pts.clipOutOfRange(pivot, radius);
121  EXPECT_EQ(pts.size(), 1u);
122  }
123 
124  // Clip:
125  {
126  TPoint2D pivot(-10, -10);
127  const float radius = 1;
128 
129  MAP pts = pts0;
130  pts.clipOutOfRange(pivot, radius);
131  EXPECT_EQ(pts.size(), 0u);
132  }
133 
134  // Clip:
135  {
136  TPoint2D pivot(0, 0);
137  const float radius = 1.1f;
138 
139  MAP pts = pts0;
140  pts.clipOutOfRange(pivot, radius);
141  EXPECT_EQ(pts.size(), 3u);
142  }
143 }
144 
145 template <class MAP>
147 {
148  MAP pts0;
149  load_demo_9pts_map(pts0);
150 
151  EXPECT_EQ(pts0.size(), 9u);
152 
153  auto lmb1 = [&]() -> std::string {
154  std::stringstream ss;
155  bool ret = pts0.save3D_to_text_stream(ss);
156  EXPECT_TRUE(ret);
157  return ss.str();
158  };
159 
160  // Correct format:
161  {
162  MAP pts1;
163  std::stringstream ss;
164  ss.str(lmb1());
165  bool ret = pts1.load3D_from_text_stream(ss);
166  EXPECT_TRUE(ret);
167  EXPECT_EQ(pts0.size(), pts1.size());
168  }
169  {
170  MAP pts1;
171  std::stringstream ss;
172  ss.str("0 1\n1 2\n 3 4\n");
173  bool ret = pts1.load2D_from_text_stream(ss);
174  EXPECT_TRUE(ret);
175  EXPECT_EQ(pts1.size(), 3u);
176  }
177  // Incorrect format:
178  {
179  MAP pts1;
180  std::stringstream ss;
181  ss.str("0 1\n1 2\n 3 4\n");
182  std::string errMsg;
183  bool ret = pts1.load3D_from_text_stream(ss, errMsg);
184  EXPECT_FALSE(ret);
185  EXPECT_EQ(pts1.size(), 0u);
186  }
187  {
188  MAP pts1;
189  std::stringstream ss;
190  ss.str("0 1 3\n1 2 3 4\n3 4\n");
191  std::string errMsg;
192  bool ret = pts1.load3D_from_text_stream(ss, errMsg);
193  EXPECT_FALSE(ret);
194  }
195  {
196  MAP pts1;
197  std::stringstream ss;
198  ss.str("0 1\n1 2 3 4\n3 4\n");
199  std::string errMsg;
200  bool ret = pts1.load3D_from_text_stream(ss, errMsg);
201  EXPECT_FALSE(ret);
202  }
203 }
204 
205 TEST(CSimplePointsMapTests, insertPoints)
206 {
207  do_test_insertPoints<CSimplePointsMap>();
208 }
209 
210 TEST(CWeightedPointsMapTests, insertPoints)
211 {
212  do_test_insertPoints<CWeightedPointsMap>();
213 }
214 
215 TEST(CColouredPointsMapTests, insertPoints)
216 {
217  do_test_insertPoints<CColouredPointsMap>();
218 }
219 
220 TEST(CPointsMapXYZI, insertPoints) { do_test_insertPoints<CPointsMapXYZI>(); }
221 
222 TEST(CSimplePointsMapTests, clipOutOfRangeInZ)
223 {
224  do_test_clipOutOfRangeInZ<CSimplePointsMap>();
225 }
226 
227 TEST(CWeightedPointsMapTests, clipOutOfRangeInZ)
228 {
229  do_test_clipOutOfRangeInZ<CWeightedPointsMap>();
230 }
231 
232 TEST(CColouredPointsMapTests, clipOutOfRangeInZ)
233 {
234  do_test_clipOutOfRangeInZ<CColouredPointsMap>();
235 }
236 
237 TEST(CSimplePointsMapTests, clipOutOfRange)
238 {
239  do_test_clipOutOfRange<CSimplePointsMap>();
240 }
241 
242 TEST(CWeightedPointsMapTests, clipOutOfRange)
243 {
244  do_test_clipOutOfRange<CWeightedPointsMap>();
245 }
246 
247 TEST(CColouredPointsMapTests, clipOutOfRange)
248 {
249  do_test_clipOutOfRange<CColouredPointsMap>();
250 }
251 
252 TEST(CSimplePointsMapTests, loadSaveStreams)
253 {
254  do_tests_loadSaveStreams<CSimplePointsMap>();
255 }
256 
257 TEST(CWeightedPointsMapTests, loadSaveStreams)
258 {
259  do_tests_loadSaveStreams<CWeightedPointsMap>();
260 }
261 
262 TEST(CColouredPointsMapTests, loadSaveStreams)
263 {
264  do_tests_loadSaveStreams<CColouredPointsMap>();
265 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
const float demo9_zs[demo9_N]
void do_test_clipOutOfRange()
void do_test_clipOutOfRangeInZ()
const float demo9_xs[demo9_N]
STL namespace.
This base provides a set of functions for maths stuff.
This namespace contains representation of robot actions and observations.
const float demo9_ys[demo9_N]
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void load_demo_9pts_map(MAP &pts)
const size_t demo9_N
void do_test_insertPoints()
A map of 3D points with reflectance/intensity (float).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void do_tests_loadSaveStreams()
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
TEST(CSimplePointsMapTests, insertPoints)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020