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

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:35