GridMapMathTest.cpp
Go to the documentation of this file.
1 /*
2  * GridMapMathTest.cpp
3  *
4  * Created on: Feb 10, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #include "grid_map_core/GridMapMath.hpp"
10 
11 // Eigen
12 #include <Eigen/Core>
13 
14 // gtest
15 #include <gtest/gtest.h>
16 
17 // Limits
18 #include <cfloat>
19 
20 // Vector
21 #include <vector>
22 
23 using namespace std;
24 using namespace grid_map;
25 
26 TEST(PositionFromIndex, Simple)
27 {
28  Length mapLength(3.0, 2.0);
29  Position mapPosition(-1.0, 2.0);
30  double resolution = 1.0;
31  Size bufferSize(3, 2);
32  Position position;
33 
34  EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize));
35  EXPECT_DOUBLE_EQ(1.0 + mapPosition.x(), position.x());
36  EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());
37 
38  EXPECT_TRUE(getPositionFromIndex(position, Index(1, 0), mapLength, mapPosition, resolution, bufferSize));
39  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
40  EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());
41 
42  EXPECT_TRUE(getPositionFromIndex(position, Index(1, 1), mapLength, mapPosition, resolution, bufferSize));
43  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
44  EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());
45 
46  EXPECT_TRUE(getPositionFromIndex(position, Index(2, 1), mapLength, mapPosition, resolution, bufferSize));
47  EXPECT_DOUBLE_EQ(-1.0 + mapPosition.x(), position.x());
48  EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());
49 
50  EXPECT_FALSE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize));
51 }
52 
53 TEST(PositionFromIndex, CircularBuffer)
54 {
55  Length mapLength(0.5, 0.4);
56  Position mapPosition(-0.1, 13.4);
57  double resolution = 0.1;
58  Size bufferSize(5, 4);
59  Index bufferStartIndex(3, 1);
60  Position position;
61 
62  EXPECT_TRUE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
63  EXPECT_DOUBLE_EQ(0.2 + mapPosition.x(), position.x());
64  EXPECT_DOUBLE_EQ(0.15 + mapPosition.y(), position.y());
65 
66  EXPECT_TRUE(getPositionFromIndex(position, Index(4, 2), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
67  EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
68  EXPECT_DOUBLE_EQ(0.05 + mapPosition.y(), position.y());
69 
70  EXPECT_TRUE(getPositionFromIndex(position, Index(2, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
71  EXPECT_DOUBLE_EQ(-0.2 + mapPosition.x(), position.x());
72  EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());
73 
74  EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
75  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
76  EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());
77 
78  EXPECT_TRUE(getPositionFromIndex(position, Index(4, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
79  EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
80  EXPECT_DOUBLE_EQ(-0.05 + mapPosition.y(), position.y());
81 
82  EXPECT_FALSE(getPositionFromIndex(position, Index(5, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
83 }
84 
85 TEST(IndexFromPosition, Simple)
86 {
87  Length mapLength(3.0, 2.0);
88  Position mapPosition(-12.4, -7.1);
89  double resolution = 1.0;
90  Index bufferSize(3, 2);
91  Index index;
92 
93  EXPECT_TRUE(getIndexFromPosition(index, Position(1.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
94  EXPECT_EQ(0, index(0));
95  EXPECT_EQ(0, index(1));
96 
97  EXPECT_TRUE(getIndexFromPosition(index, Position(-1.0, -0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
98  EXPECT_EQ(2, index(0));
99  EXPECT_EQ(1, index(1));
100 
101  EXPECT_TRUE(getIndexFromPosition(index, Position(0.6, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
102  EXPECT_EQ(0, index(0));
103  EXPECT_EQ(0, index(1));
104 
105  EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, -0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
106  EXPECT_EQ(1, index(0));
107  EXPECT_EQ(1, index(1));
108 
109  EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
110  EXPECT_EQ(1, index(0));
111  EXPECT_EQ(0, index(1));
112 
113  EXPECT_FALSE(getIndexFromPosition(index, Position(4.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
114 }
115 
116 TEST(IndexFromPosition, EdgeCases)
117 {
118  Length mapLength(3.0, 2.0);
119  Position mapPosition(0.0, 0.0);
120  double resolution = 1.0;
121  Size bufferSize(3, 2);
122  Index index;
123 
124  EXPECT_TRUE(getIndexFromPosition(index, Position(0.0, DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
125  EXPECT_EQ(1, index(0));
126  EXPECT_EQ(0, index(1));
127 
128  EXPECT_TRUE(getIndexFromPosition(index, Position(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
129  EXPECT_EQ(1, index(0));
130  EXPECT_EQ(1, index(1));
131 
132  EXPECT_TRUE(getIndexFromPosition(index, Position(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
133  EXPECT_EQ(2, index(0));
134  EXPECT_EQ(1, index(1));
135 
136  EXPECT_FALSE(getIndexFromPosition(index, Position(-1.5, 1.0), mapLength, mapPosition, resolution, bufferSize));
137 }
138 
139 TEST(IndexFromPosition, CircularBuffer)
140 {
141  Length mapLength(0.5, 0.4);
142  Position mapPosition(0.4, -0.9);
143  double resolution = 0.1;
144  Size bufferSize(5, 4);
145  Index bufferStartIndex(3, 1);
146  Index index;
147 
148  EXPECT_TRUE(getIndexFromPosition(index, Position(0.2, 0.15) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
149  EXPECT_EQ(3, index(0));
150  EXPECT_EQ(1, index(1));
151 
152  EXPECT_TRUE(getIndexFromPosition(index, Position(0.03, -0.17) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
153  EXPECT_EQ(0, index(0));
154  EXPECT_EQ(0, index(1));
155 }
156 
157 TEST(checkIfPositionWithinMap, Inside)
158 {
159  Length mapLength(50.0, 25.0);
160  Position mapPosition(11.4, 0.0);
161 
162  EXPECT_TRUE(checkIfPositionWithinMap(Position(0.0, 0.0) + mapPosition, mapLength, mapPosition));
163  EXPECT_TRUE(checkIfPositionWithinMap(Position(5.0, 5.0) + mapPosition, mapLength, mapPosition));
164  EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, 10.0) + mapPosition, mapLength, mapPosition));
165  EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, -10.0) + mapPosition, mapLength, mapPosition));
166  EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, 10.0) + mapPosition, mapLength, mapPosition));
167  EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, -10.0) + mapPosition, mapLength, mapPosition));
168 }
169 
170 TEST(checkIfPositionWithinMap, Outside)
171 {
172  Length mapLength(10.0, 5.0);
173  Position mapPosition(-3.0, 145.2);
174 
175  EXPECT_FALSE(checkIfPositionWithinMap(Position(5.5, 0.0) + mapPosition, mapLength, mapPosition));
176  EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 0.0) + mapPosition, mapLength, mapPosition));
177  EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 3.0) + mapPosition, mapLength, mapPosition));
178  EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, -3.0) + mapPosition, mapLength, mapPosition));
179  EXPECT_FALSE(checkIfPositionWithinMap(Position(3.0, 3.0) + mapPosition, mapLength, mapPosition));
180 }
181 
182 TEST(checkIfPositionWithinMap, EdgeCases)
183 {
184  Length mapLength(2.0, 3.0);
185  Position mapPosition(0.0, 0.0);
186 
187  /*
188  *
189  * A (is inside) B (is not inside)
190  * +-----------------------+
191  * | |
192  * | |
193  * | X |
194  * | ^ |
195  * | | |
196  * | | |
197  * | <-----+ |
198  * | Y |
199  * | |
200  * | |
201  * | |
202  * +-----------------------+
203  * C (is not inside) D (is not inside)
204  *
205  * Resulting coordinates are:
206  * A: (1.0, 1.5)
207  * B: (1.0, -1.5)
208  * C: (-1.0, 1.5)
209  * D: (-1.0, -1.5)
210  *
211  */
212 
213  // Noise around A.
214  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5), mapLength, mapPosition));
215  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition));
216  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition));
217  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition));
218  EXPECT_TRUE(checkIfPositionWithinMap(Position(1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition));
219 
220  // Noise around B.
221  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition));
222  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, - 1.5), mapLength, mapPosition));
223  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 - DBL_EPSILON, - 1.5), mapLength, mapPosition));
224  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 + DBL_EPSILON), mapLength, mapPosition));
225  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, - 1.5 - DBL_EPSILON), mapLength, mapPosition));
226 
227  // Noise around C.
228  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
229  EXPECT_TRUE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, 1.5), mapLength, mapPosition));
230  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, 1.5), mapLength, mapPosition));
231  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 + DBL_EPSILON), mapLength, mapPosition));
232  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5 - DBL_EPSILON), mapLength, mapPosition));
233 
234  // Noise around D.
235  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5), mapLength, mapPosition));
236  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 + DBL_EPSILON, -1.5), mapLength, mapPosition));
237  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0 - DBL_EPSILON, -1.5), mapLength, mapPosition));
238  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 + DBL_EPSILON), mapLength, mapPosition));
239  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, -1.5 - DBL_EPSILON), mapLength, mapPosition));
240 
241  // Extra tests.
242  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
243  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition));
244  EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition));
245  EXPECT_FALSE(checkIfPositionWithinMap(Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition));
246  EXPECT_TRUE(checkIfPositionWithinMap(Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition));
247 }
248 
249 TEST(getIndexShiftFromPositionShift, All)
250 {
251  double resolution = 1.0;
252  Index indexShift;
253 
254  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.0, 0.0), resolution));
255  EXPECT_EQ(0, indexShift(0));
256  EXPECT_EQ(0, indexShift(1));
257 
258  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.35, -0.45), resolution));
259  EXPECT_EQ(0, indexShift(0));
260  EXPECT_EQ(0, indexShift(1));
261 
262  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.55, -0.45), resolution));
263  EXPECT_EQ(-1, indexShift(0));
264  EXPECT_EQ(0, indexShift(1));
265 
266  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-1.3, -2.65), resolution));
267  EXPECT_EQ(1, indexShift(0));
268  EXPECT_EQ(3, indexShift(1));
269 
270  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-0.4, 0.09), 0.2));
271  EXPECT_EQ(2, indexShift(0));
272  EXPECT_EQ(0, indexShift(1));
273 }
274 
275 TEST(getPositionShiftFromIndexShift, All)
276 {
277  double resolution = 0.3;
278  Vector positionShift;
279 
280  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(0, 0), resolution));
281  EXPECT_DOUBLE_EQ(0.0, positionShift.x());
282  EXPECT_DOUBLE_EQ(0.0, positionShift.y());
283 
284  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(1, -1), resolution));
285  EXPECT_DOUBLE_EQ(-0.3, positionShift.x());
286  EXPECT_DOUBLE_EQ(0.3, positionShift.y());
287 
288  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(2, 1), resolution));
289  EXPECT_DOUBLE_EQ(-0.6, positionShift.x());
290  EXPECT_DOUBLE_EQ(-0.3, positionShift.y());
291 }
292 
293 TEST(checkIfIndexInRange, All)
294 {
295  Size bufferSize(10, 15);
296  EXPECT_TRUE(checkIfIndexInRange(Index(0, 0), bufferSize));
297  EXPECT_TRUE(checkIfIndexInRange(Index(9, 14), bufferSize));
298  EXPECT_FALSE(checkIfIndexInRange(Index(10, 5), bufferSize));
299  EXPECT_FALSE(checkIfIndexInRange(Index(5, 300), bufferSize));
300  EXPECT_FALSE(checkIfIndexInRange(Index(-1, 0), bufferSize));
301  EXPECT_FALSE(checkIfIndexInRange(Index(0, -300), bufferSize));
302 }
303 
304 TEST(boundIndexToRange, All)
305 {
306  int index;
307  int bufferSize = 10;
308 
309  index = 0;
310  boundIndexToRange(index, bufferSize);
311  EXPECT_EQ(0, index);
312 
313  index = 1;
314  boundIndexToRange(index, bufferSize);
315  EXPECT_EQ(1, index);
316 
317  index = -1;
318  boundIndexToRange(index, bufferSize);
319  EXPECT_EQ(0, index);
320 
321  index = 9;
322  boundIndexToRange(index, bufferSize);
323  EXPECT_EQ(9, index);
324 
325  index = 10;
326  boundIndexToRange(index, bufferSize);
327  EXPECT_EQ(9, index);
328 
329  index = 35;
330  boundIndexToRange(index, bufferSize);
331  EXPECT_EQ(9, index);
332 
333  index = -19;
334  boundIndexToRange(index, bufferSize);
335  EXPECT_EQ(0, index);
336 }
337 
338 TEST(wrapIndexToRange, All)
339 {
340  int index;
341  int bufferSize = 10;
342 
343  index = 0;
344  wrapIndexToRange(index, bufferSize);
345  EXPECT_EQ(0, index);
346 
347  index = 1;
348  wrapIndexToRange(index, bufferSize);
349  EXPECT_EQ(1, index);
350 
351  index = -1;
352  wrapIndexToRange(index, bufferSize);
353  EXPECT_EQ(9, index);
354 
355  index = 9;
356  wrapIndexToRange(index, bufferSize);
357  EXPECT_EQ(9, index);
358 
359  index = 10;
360  wrapIndexToRange(index, bufferSize);
361  EXPECT_EQ(0, index);
362 
363  index = 11;
364  wrapIndexToRange(index, bufferSize);
365  EXPECT_EQ(1, index);
366 
367  index = 35;
368  wrapIndexToRange(index, bufferSize);
369  EXPECT_EQ(5, index);
370 
371  index = -9;
372  wrapIndexToRange(index, bufferSize);
373  EXPECT_EQ(1, index);
374 
375  index = -19;
376  wrapIndexToRange(index, bufferSize);
377  EXPECT_EQ(1, index);
378 }
379 
380 TEST(boundPositionToRange, Simple)
381 {
382  double epsilon = 11.0 * numeric_limits<double>::epsilon();
383 
384  Length mapLength(30.0, 10.0);
385  Position mapPosition(0.0, 0.0);
386  Position position;
387 
388  position << 0.0, 0.0;
389  boundPositionToRange(position, mapLength, mapPosition);
390  EXPECT_DOUBLE_EQ(0.0, position.x());
391  EXPECT_DOUBLE_EQ(0.0, position.y());
392 
393  position << 15.0, 5.0;
394  boundPositionToRange(position, mapLength, mapPosition);
395  EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon);
396  EXPECT_GE(15.0, position.x());
397  EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon);
398  EXPECT_GE(5.0, position.y());
399 
400  position << -15.0, -5.0;
401  boundPositionToRange(position, mapLength, mapPosition);
402  EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon);
403  EXPECT_LE(-15.0, position.x());
404  EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon);
405  EXPECT_LE(-5.0, position.y());
406 
407  position << 16.0, 6.0;
408  boundPositionToRange(position, mapLength, mapPosition);
409  EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon);
410  EXPECT_GE(15.0, position.x());
411  EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon);
412  EXPECT_GE(5.0, position.y());
413 
414  position << -16.0, -6.0;
415  boundPositionToRange(position, mapLength, mapPosition);
416  EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon);
417  EXPECT_LE(-15.0, position.x());
418  EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon);
419  EXPECT_LE(-5.0, position.y());
420 
421  position << 1e6, 1e6;
422  boundPositionToRange(position, mapLength, mapPosition);
423  EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon);
424  EXPECT_GE(15.0, position.x());
425  EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon);
426  EXPECT_GE(5.0, position.y());
427 
428  position << -1e6, -1e6;
429  boundPositionToRange(position, mapLength, mapPosition);
430  EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon);
431  EXPECT_LE(-15.0, position.x());
432  EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon);
433  EXPECT_LE(-5.0, position.y());
434 }
435 
436 TEST(boundPositionToRange, Position)
437 {
438  double epsilon = 11.0 * numeric_limits<double>::epsilon();
439 
440  Length mapLength(30.0, 10.0);
441  Position mapPosition(1.0, 2.0);
442  Position position;
443 
444  position << 0.0, 0.0;
445  boundPositionToRange(position, mapLength, mapPosition);
446  EXPECT_DOUBLE_EQ(0.0, position.x());
447  EXPECT_DOUBLE_EQ(0.0, position.y());
448 
449  position << 16.0, 7.0;
450  boundPositionToRange(position, mapLength, mapPosition);
451  EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon);
452  EXPECT_GE(16.0, position.x());
453  EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon);
454  EXPECT_GE(7.0, position.y());
455 
456  position << -14.0, -3.0;
457  boundPositionToRange(position, mapLength, mapPosition);
458  EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon);
459  EXPECT_LE(-14.0, position.x());
460  EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon);
461  EXPECT_LE(-3.0, position.y());
462 
463  position << 17.0, 8.0;
464  boundPositionToRange(position, mapLength, mapPosition);
465  EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon);
466  EXPECT_GE(16.0, position.x());
467  EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon);
468  EXPECT_GE(7.0, position.y());
469 
470  position << -15.0, -4.0;
471  boundPositionToRange(position, mapLength, mapPosition);
472  EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon);
473  EXPECT_LE(-14.0, position.x());
474  EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon);
475  EXPECT_LE(-3.0, position.y());
476 
477  position << 1e6, 1e6;
478  boundPositionToRange(position, mapLength, mapPosition);
479  EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon);
480  EXPECT_GE(16.0, position.x());
481  EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon);
482  EXPECT_GE(7.0, position.y());
483 
484  position << -1e6, -1e6;
485  boundPositionToRange(position, mapLength, mapPosition);
486  EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon);
487  EXPECT_LE(-14.0, position.x());
488  EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon);
489  EXPECT_LE(-3.0, position.y());
490 }
491 
492 TEST(getSubmapInformation, Simple)
493 {
494  // Map
495  Length mapLength(5.0, 4.0);
496  Position mapPosition(0.0, 0.0);
497  double resolution = 1.0;
498  Size bufferSize(5, 4);
499 
500  // Requested submap
501  Position requestedSubmapPosition;
502  Position requestedSubmapLength;
503 
504  // The returned submap indeces
505  Index submapTopLeftIndex;
506  Index submapSize;
507  Position submapPosition;
508  Length submapLength;
509  Index requestedIndexInSubmap;
510 
511  requestedSubmapPosition << 0.0, 0.5;
512  requestedSubmapLength << 0.9, 2.9;
513  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
514  requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize));
515  EXPECT_EQ(2, submapTopLeftIndex(0));
516  EXPECT_EQ(0, submapTopLeftIndex(1));
517  EXPECT_EQ(1, submapSize(0));
518  EXPECT_EQ(3, submapSize(1));
519  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
520  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
521  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
522  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
523  EXPECT_EQ(0, requestedIndexInSubmap(0));
524  EXPECT_EQ(1, requestedIndexInSubmap(1));
525 }
526 
527 TEST(getSubmapInformation, Zero)
528 {
529  // Map
530  Length mapLength(5.0, 4.0);
531  Position mapPosition(0.0, 0.0);
532  double resolution = 1.0;
533  Size bufferSize(5, 4);
534 
535  // Requested submap
536  Position requestedSubmapPosition;
537  Length requestedSubmapLength;
538 
539  // The returned submap indeces
540  Index submapTopLeftIndex;
541  Index submapSize;
542  Position submapPosition;
543  Length submapLength;
544  Index requestedIndexInSubmap;
545 
546  requestedSubmapPosition << -1.0, -0.5;
547  requestedSubmapLength << 0.0, 0.0;
548  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
549  requestedSubmapPosition, requestedSubmapLength,
550  mapLength, mapPosition, resolution, bufferSize));
551  EXPECT_EQ(3, submapTopLeftIndex(0));
552  EXPECT_EQ(2, submapTopLeftIndex(1));
553  EXPECT_EQ(1, submapSize(0));
554  EXPECT_EQ(1, submapSize(1));
555  EXPECT_DOUBLE_EQ(requestedSubmapPosition.x(), submapPosition.x());
556  EXPECT_DOUBLE_EQ(requestedSubmapPosition.y(), submapPosition.y());
557  EXPECT_DOUBLE_EQ(resolution, submapLength(0));
558  EXPECT_DOUBLE_EQ(resolution, submapLength(1));
559  EXPECT_EQ(0, requestedIndexInSubmap(0));
560  EXPECT_EQ(0, requestedIndexInSubmap(1));
561 }
562 
563 TEST(getSubmapInformation, ExceedingBoundaries)
564 {
565  // Map
566  Length mapLength(5.0, 4.0);
567  Position mapPosition(0.0, 0.0);
568  double resolution = 1.0;
569  Size bufferSize(5, 4);
570 
571  // Requested submap
572  Position requestedSubmapPosition;
573  Length requestedSubmapLength;
574 
575  // The returned submap indeces
576  Index submapTopLeftIndex;
577  Size submapSize;
578  Position submapPosition;
579  Length submapLength;
580  Index requestedIndexInSubmap;
581 
582  requestedSubmapPosition << 2.0, 1.5;
583  requestedSubmapLength << 2.9, 2.9;
584  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
585  requestedSubmapPosition, requestedSubmapLength,
586  mapLength, mapPosition, resolution, bufferSize));
587  EXPECT_EQ(0, submapTopLeftIndex(0));
588  EXPECT_EQ(0, submapTopLeftIndex(1));
589  EXPECT_EQ(2, submapSize(0));
590  EXPECT_EQ(2, submapSize(1));
591  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
592  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
593  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
594  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
595  EXPECT_EQ(0, requestedIndexInSubmap(0));
596  EXPECT_EQ(0, requestedIndexInSubmap(1));
597 
598  requestedSubmapPosition << 0.0, 0.0;
599  requestedSubmapLength << 1e6, 1e6;
600  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
601  requestedSubmapPosition, requestedSubmapLength,
602  mapLength, mapPosition, resolution, bufferSize));
603  EXPECT_EQ(0, submapTopLeftIndex(0));
604  EXPECT_EQ(0, submapTopLeftIndex(1));
605  EXPECT_EQ(bufferSize(0), submapSize(0));
606  EXPECT_EQ(bufferSize(1), submapSize(1));
607  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
608  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
609  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
610  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
611  EXPECT_EQ(2, requestedIndexInSubmap(0));
612  EXPECT_LE(1, requestedIndexInSubmap(1));
613  EXPECT_GE(2, requestedIndexInSubmap(1));
614 }
615 
616 TEST(getSubmapInformation, CircularBuffer)
617 {
618  // Map
619  Length mapLength(5.0, 4.0);
620  Position mapPosition(0.0, 0.0);
621  double resolution = 1.0;
622  Size bufferSize(5, 4);
623  Index bufferStartIndex(2, 1);
624 
625  // Requested submap
626  Position requestedSubmapPosition;
627  Length requestedSubmapLength;
628 
629  // The returned submap indeces
630  Index submapTopLeftIndex;
631  Size submapSize;
632  Position submapPosition;
633  Length submapLength;
634  Index requestedIndexInSubmap;
635 
636  requestedSubmapPosition << 0.0, 0.5;
637  requestedSubmapLength << 0.9, 2.9;
638  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
639  requestedSubmapPosition, requestedSubmapLength,
640  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
641  EXPECT_EQ(4, submapTopLeftIndex(0));
642  EXPECT_EQ(1, submapTopLeftIndex(1));
643  EXPECT_EQ(1, submapSize(0));
644  EXPECT_EQ(3, submapSize(1));
645  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
646  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
647  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
648  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
649  EXPECT_EQ(0, requestedIndexInSubmap(0));
650  EXPECT_EQ(1, requestedIndexInSubmap(1));
651 
652  requestedSubmapPosition << 2.0, 1.5;
653  requestedSubmapLength << 2.9, 2.9;
654  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
655  requestedSubmapPosition, requestedSubmapLength,
656  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
657  EXPECT_EQ(2, submapTopLeftIndex(0));
658  EXPECT_EQ(1, submapTopLeftIndex(1));
659  EXPECT_EQ(2, submapSize(0));
660  EXPECT_EQ(2, submapSize(1));
661  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
662  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
663  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
664  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
665  EXPECT_EQ(0, requestedIndexInSubmap(0));
666  EXPECT_EQ(0, requestedIndexInSubmap(1));
667 
668  requestedSubmapPosition << 0.0, 0.0;
669  requestedSubmapLength << 1e6, 1e6;
670  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
671  requestedSubmapPosition, requestedSubmapLength,
672  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
673  EXPECT_EQ(2, submapTopLeftIndex(0));
674  EXPECT_EQ(1, submapTopLeftIndex(1));
675  EXPECT_EQ(bufferSize(0), submapSize(0));
676  EXPECT_EQ(bufferSize(1), submapSize(1));
677  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
678  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
679  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
680  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
681  EXPECT_EQ(2, requestedIndexInSubmap(0));
682  EXPECT_LE(1, requestedIndexInSubmap(1));
683  EXPECT_GE(2, requestedIndexInSubmap(1));
684 }
685 
686 TEST(getSubmapInformation, Debug1)
687 {
688  // Map
689  Length mapLength(4.98, 4.98);
690  Position mapPosition(-4.98, -5.76);
691  double resolution = 0.06;
692  Size bufferSize(83, 83);
693  Index bufferStartIndex(0, 13);
694 
695  // Requested submap
696  Position requestedSubmapPosition(-7.44, -3.42);
697  Length requestedSubmapLength(0.12, 0.12);
698 
699  // The returned submap indeces
700  Index submapTopLeftIndex;
701  Size submapSize;
702  Position submapPosition;
703  Length submapLength;
704  Index requestedIndexInSubmap;
705 
706  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
707  requestedSubmapPosition, requestedSubmapLength,
708  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
709  EXPECT_EQ(2, submapSize(0));
710  EXPECT_EQ(3, submapSize(1));
711  EXPECT_DOUBLE_EQ(0.12, submapLength(0));
712  EXPECT_DOUBLE_EQ(0.18, submapLength(1));
713 }
714 
715 TEST(getSubmapInformation, Debug2)
716 {
717  // Map
718  Length mapLength(4.98, 4.98);
719  Position mapPosition(2.46, -25.26);
720  double resolution = 0.06;
721  Size bufferSize(83, 83);
722  Index bufferStartIndex(42, 6);
723 
724  // Requested submap
725  Position requestedSubmapPosition(0.24, -26.82);
726  Length requestedSubmapLength(0.624614, 0.462276);
727 
728  // The returned submap indeces
729  Index submapTopLeftIndex;
730  Size submapSize;
731  Position submapPosition;
732  Length submapLength;
733  Index requestedIndexInSubmap;
734 
735  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
736  requestedSubmapPosition, requestedSubmapLength,
737  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
738  EXPECT_LT(0, submapSize(0));
739  EXPECT_LT(0, submapSize(1));
740  EXPECT_LT(0.0, submapLength(0));
741  EXPECT_LT(0.0, submapLength(1));
742 }
743 
744 TEST(getBufferRegionsForSubmap, Trivial)
745 {
746  Size bufferSize(5, 4);
747  Index submapIndex(0, 0);
748  Size submapSize(0, 0);
749  std::vector<BufferRegion> regions;
750 
751  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
752  EXPECT_EQ(1, regions.size());
753  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
754  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
755  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
756  EXPECT_EQ(0, regions[0].getSize()[0]);
757  EXPECT_EQ(0, regions[0].getSize()[1]);
758 
759  submapSize << 0, 7;
760  EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
761 
762  submapSize << 6, 7;
763  EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
764 }
765 
766 TEST(getBufferRegionsForSubmap, Simple)
767 {
768  Size bufferSize(5, 4);
769  Index submapIndex(1, 2);
770  Size submapSize(3, 2);
771  std::vector<BufferRegion> regions;
772 
773  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
774  EXPECT_EQ(1, regions.size());
775  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
776  EXPECT_EQ(1, regions[0].getStartIndex()[0]);
777  EXPECT_EQ(2, regions[0].getStartIndex()[1]);
778  EXPECT_EQ(3, regions[0].getSize()[0]);
779  EXPECT_EQ(2, regions[0].getSize()[1]);
780 }
781 
782 TEST(getBufferRegionsForSubmap, CircularBuffer)
783 {
784  Size bufferSize(5, 4);
785  Index submapIndex;
786  Size submapSize;
787  Index bufferStartIndex(3, 1);
788  std::vector<BufferRegion> regions;
789 
790  submapIndex << 3, 1;
791  submapSize << 2, 3;
792  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
793  EXPECT_EQ(1, regions.size());
794  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
795  EXPECT_EQ(3, regions[0].getStartIndex()[0]);
796  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
797  EXPECT_EQ(2, regions[0].getSize()[0]);
798  EXPECT_EQ(3, regions[0].getSize()[1]);
799 
800  submapIndex << 4, 1;
801  submapSize << 2, 3;
802  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
803  EXPECT_EQ(2, regions.size());
804  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
805  EXPECT_EQ(4, regions[0].getStartIndex()[0]);
806  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
807  EXPECT_EQ(1, regions[0].getSize()[0]);
808  EXPECT_EQ(3, regions[0].getSize()[1]);
809  EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant());
810  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
811  EXPECT_EQ(1, regions[1].getStartIndex()[1]);
812  EXPECT_EQ(1, regions[1].getSize()[0]);
813  EXPECT_EQ(3, regions[1].getSize()[1]);
814 
815  submapIndex << 1, 0;
816  submapSize << 2, 1;
817  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
818  EXPECT_EQ(1, regions.size());
819  EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant());
820  EXPECT_EQ(1, regions[0].getStartIndex()[0]);
821  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
822  EXPECT_EQ(2, regions[0].getSize()[0]);
823  EXPECT_EQ(1, regions[0].getSize()[1]);
824 
825  submapIndex << 3, 1;
826  submapSize << 5, 4;
827  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));\
828  EXPECT_EQ(4, regions.size());
829  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
830  EXPECT_EQ(3, regions[0].getStartIndex()[0]);
831  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
832  EXPECT_EQ(2, regions[0].getSize()[0]);
833  EXPECT_EQ(3, regions[0].getSize()[1]);
834  EXPECT_EQ(BufferRegion::Quadrant::TopRight, regions[1].getQuadrant());
835  EXPECT_EQ(3, regions[1].getStartIndex()[0]);
836  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
837  EXPECT_EQ(2, regions[1].getSize()[0]);
838  EXPECT_EQ(1, regions[1].getSize()[1]);
839  EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant());
840  EXPECT_EQ(0, regions[2].getStartIndex()[0]);
841  EXPECT_EQ(1, regions[2].getStartIndex()[1]);
842  EXPECT_EQ(3, regions[2].getSize()[0]);
843  EXPECT_EQ(3, regions[2].getSize()[1]);
844  EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant());
845  EXPECT_EQ(0, regions[3].getStartIndex()[0]);
846  EXPECT_EQ(0, regions[3].getStartIndex()[1]);
847  EXPECT_EQ(3, regions[3].getSize()[0]);
848  EXPECT_EQ(1, regions[3].getSize()[1]);
849 }
850 
851 TEST(checkIncrementIndex, Simple)
852 {
853  Index index(0, 0);
854  Size bufferSize(4, 3);
855 
856  EXPECT_TRUE(incrementIndex(index, bufferSize));
857  EXPECT_EQ(0, index[0]);
858  EXPECT_EQ(1, index[1]);
859 
860  EXPECT_TRUE(incrementIndex(index, bufferSize));
861  EXPECT_EQ(0, index[0]);
862  EXPECT_EQ(2, index[1]);
863 
864  EXPECT_TRUE(incrementIndex(index, bufferSize));
865  EXPECT_EQ(1, index[0]);
866  EXPECT_EQ(0, index[1]);
867 
868  EXPECT_TRUE(incrementIndex(index, bufferSize));
869  EXPECT_EQ(1, index[0]);
870  EXPECT_EQ(1, index[1]);
871 
872  for (int i = 0; i < 6; i++) {
873  EXPECT_TRUE(incrementIndex(index, bufferSize));
874  }
875  EXPECT_EQ(3, index[0]);
876  EXPECT_EQ(1, index[1]);
877 
878  EXPECT_TRUE(incrementIndex(index, bufferSize));
879  EXPECT_EQ(3, index[0]);
880  EXPECT_EQ(2, index[1]);
881 
882  EXPECT_FALSE(incrementIndex(index, bufferSize));
883  EXPECT_EQ(index[0], index[0]);
884  EXPECT_EQ(index[1], index[1]);
885 }
886 
887 TEST(checkIncrementIndex, CircularBuffer)
888 {
889  Size bufferSize(4, 3);
890  Index bufferStartIndex(2, 1);
891  Index index(bufferStartIndex);
892 
893  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
894  EXPECT_EQ(2, index[0]);
895  EXPECT_EQ(2, index[1]);
896 
897  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
898  EXPECT_EQ(2, index[0]);
899  EXPECT_EQ(0, index[1]);
900 
901  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
902  EXPECT_EQ(3, index[0]);
903  EXPECT_EQ(1, index[1]);
904 
905  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
906  EXPECT_EQ(3, index[0]);
907  EXPECT_EQ(2, index[1]);
908 
909  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
910  EXPECT_EQ(3, index[0]);
911  EXPECT_EQ(0, index[1]);
912 
913  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
914  EXPECT_EQ(0, index[0]);
915  EXPECT_EQ(1, index[1]);
916 
917  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
918  EXPECT_EQ(0, index[0]);
919  EXPECT_EQ(2, index[1]);
920 
921  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
922  EXPECT_EQ(0, index[0]);
923  EXPECT_EQ(0, index[1]);
924 
925  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
926  EXPECT_EQ(1, index[0]);
927  EXPECT_EQ(1, index[1]);
928 
929  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
930  EXPECT_EQ(1, index[0]);
931  EXPECT_EQ(2, index[1]);
932 
933  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
934  EXPECT_EQ(1, index[0]);
935  EXPECT_EQ(0, index[1]);
936 
937  EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex));
938  EXPECT_EQ(index[0], index[0]);
939  EXPECT_EQ(index[1], index[1]);
940 }
941 
942 TEST(checkIncrementIndexForSubmap, Simple)
943 {
944  Index submapIndex(0, 0);
945  Index index;
946  Index submapTopLeftIndex(3, 1);
947  Size submapBufferSize(2, 4);
948  Size bufferSize(8, 5);
949 
950  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
951  EXPECT_EQ(0, submapIndex[0]);
952  EXPECT_EQ(1, submapIndex[1]);
953  EXPECT_EQ(3, index[0]);
954  EXPECT_EQ(2, index[1]);
955 
956  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
957  EXPECT_EQ(0, submapIndex[0]);
958  EXPECT_EQ(2, submapIndex[1]);
959  EXPECT_EQ(3, index[0]);
960  EXPECT_EQ(3, index[1]);
961 
962  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
963  EXPECT_EQ(0, submapIndex[0]);
964  EXPECT_EQ(3, submapIndex[1]);
965  EXPECT_EQ(3, index[0]);
966  EXPECT_EQ(4, index[1]);
967 
968  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
969  EXPECT_EQ(1, submapIndex[0]);
970  EXPECT_EQ(0, submapIndex[1]);
971  EXPECT_EQ(4, index[0]);
972  EXPECT_EQ(1, index[1]);
973 
974  submapIndex << 1, 2;
975  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
976  EXPECT_EQ(1, submapIndex[0]);
977  EXPECT_EQ(3, submapIndex[1]);
978  EXPECT_EQ(4, index[0]);
979  EXPECT_EQ(4, index[1]);
980 
981  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
982 
983  submapIndex << 2, 0;
984  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
985 }
986 
987 TEST(checkIncrementIndexForSubmap, CircularBuffer)
988 {
989  Index submapIndex(0, 0);
990  Index index;
991  Index submapTopLeftIndex(6, 3);
992  Size submapBufferSize(2, 4);
993  Size bufferSize(8, 5);
994  Index bufferStartIndex(3, 2);
995 
996  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
997  EXPECT_EQ(0, submapIndex[0]);
998  EXPECT_EQ(1, submapIndex[1]);
999  EXPECT_EQ(6, index[0]);
1000  EXPECT_EQ(4, index[1]);
1001 
1002  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
1003  EXPECT_EQ(0, submapIndex[0]);
1004  EXPECT_EQ(2, submapIndex[1]);
1005  EXPECT_EQ(6, index[0]);
1006  EXPECT_EQ(0, index[1]);
1007 
1008  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
1009  EXPECT_EQ(0, submapIndex[0]);
1010  EXPECT_EQ(3, submapIndex[1]);
1011  EXPECT_EQ(6, index[0]);
1012  EXPECT_EQ(1, index[1]);
1013 
1014  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
1015  EXPECT_EQ(1, submapIndex[0]);
1016  EXPECT_EQ(0, submapIndex[1]);
1017  EXPECT_EQ(7, index[0]);
1018  EXPECT_EQ(3, index[1]);
1019 
1020  submapIndex << 1, 2;
1021  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
1022  EXPECT_EQ(1, submapIndex[0]);
1023  EXPECT_EQ(3, submapIndex[1]);
1024  EXPECT_EQ(7, index[0]);
1025  EXPECT_EQ(1, index[1]);
1026 
1027  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
1028 
1029  submapIndex << 2, 0;
1030  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
1031 }
1032 
1033 TEST(getIndexFromLinearIndex, Simple)
1034 {
1035  EXPECT_TRUE((Index(0, 0) == getIndexFromLinearIndex(0, Size(8, 5), false)).all());
1036  EXPECT_TRUE((Index(1, 0) == getIndexFromLinearIndex(1, Size(8, 5), false)).all());
1037  EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(1, Size(8, 5), true)).all());
1038  EXPECT_TRUE((Index(2, 0) == getIndexFromLinearIndex(2, Size(8, 5), false)).all());
1039  EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all());
1040  EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all());
1041 }
grid_map::incrementIndexForSubmap
bool incrementIndexForSubmap(Index &submapIndex, Index &index, const Index &submapTopLeftIndex, const Size &submapBufferSize, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:459
grid_map::Index
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
grid_map::Vector
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19
grid_map::wrapIndexToRange
void wrapIndexToRange(Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:213
grid_map::getBufferRegionsForSubmap
bool getBufferRegionsForSubmap(std::vector< BufferRegion > &submapBufferRegions, const Index &submapIndex, const Size &submapBufferSize, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:329
grid_map::Size
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
grid_map::getSubmapInformation
bool getSubmapInformation(Index &submapTopLeftIndex, Size &submapBufferSize, Position &submapPosition, Length &submapLength, Index &requestedIndexInSubmap, const Position &requestedSubmapPosition, const Length &requestedSubmapLength, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:271
grid_map::getIndexFromPosition
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:129
grid_map
Definition: BufferRegion.hpp:13
grid_map::checkIfPositionWithinMap
bool checkIfPositionWithinMap(const Position &position, const Length &mapLength, const Position &mapPosition)
Definition: GridMapMath.cpp:144
grid_map::getPositionShiftFromIndexShift
bool getPositionShiftFromIndexShift(Vector &positionShift, const Index &indexShift, const double &resolution)
Definition: GridMapMath.cpp:183
grid_map::Position
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
grid_map::incrementIndex
bool incrementIndex(Index &index, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:437
TEST
TEST(PositionFromIndex, Simple)
Definition: GridMapMathTest.cpp:26
grid_map::getIndexFromLinearIndex
Index getIndexFromLinearIndex(const size_t linearIndex, const Size &bufferSize, const bool rowMajor=false)
Definition: GridMapMath.cpp:514
grid_map::internal::getQuadrant
BufferRegion::Quadrant getQuadrant(const Index &index, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:101
grid_map::boundIndexToRange
void boundIndexToRange(Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:200
grid_map::checkIfIndexInRange
bool checkIfIndexInRange(const Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:191
grid_map::getPositionFromIndex
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Definition: GridMapMath.cpp:114
grid_map::boundPositionToRange
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
Definition: GridMapMath.cpp:241
grid_map::getIndexShiftFromPositionShift
bool getIndexShiftFromPositionShift(Index &indexShift, const Vector &positionShift, const double &resolution)
Definition: GridMapMath.cpp:168
grid_map::Length
Eigen::Array2d Length
Definition: TypeDefs.hpp:24

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:27