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  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition));
188  EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition));
189  EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition));
190  EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition));
191  EXPECT_FALSE(checkIfPositionWithinMap(Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition));
192  EXPECT_TRUE(checkIfPositionWithinMap(Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition));
193 }
194 
195 TEST(getIndexShiftFromPositionShift, All)
196 {
197  double resolution = 1.0;
198  Index indexShift;
199 
200  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.0, 0.0), resolution));
201  EXPECT_EQ(0, indexShift(0));
202  EXPECT_EQ(0, indexShift(1));
203 
204  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.35, -0.45), resolution));
205  EXPECT_EQ(0, indexShift(0));
206  EXPECT_EQ(0, indexShift(1));
207 
208  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.55, -0.45), resolution));
209  EXPECT_EQ(-1, indexShift(0));
210  EXPECT_EQ(0, indexShift(1));
211 
212  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-1.3, -2.65), resolution));
213  EXPECT_EQ(1, indexShift(0));
214  EXPECT_EQ(3, indexShift(1));
215 
216  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-0.4, 0.09), 0.2));
217  EXPECT_EQ(2, indexShift(0));
218  EXPECT_EQ(0, indexShift(1));
219 }
220 
221 TEST(getPositionShiftFromIndexShift, All)
222 {
223  double resolution = 0.3;
224  Vector positionShift;
225 
226  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(0, 0), resolution));
227  EXPECT_DOUBLE_EQ(0.0, positionShift.x());
228  EXPECT_DOUBLE_EQ(0.0, positionShift.y());
229 
230  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(1, -1), resolution));
231  EXPECT_DOUBLE_EQ(-0.3, positionShift.x());
232  EXPECT_DOUBLE_EQ(0.3, positionShift.y());
233 
234  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(2, 1), resolution));
235  EXPECT_DOUBLE_EQ(-0.6, positionShift.x());
236  EXPECT_DOUBLE_EQ(-0.3, positionShift.y());
237 }
238 
239 TEST(checkIfIndexInRange, All)
240 {
241  Size bufferSize(10, 15);
242  EXPECT_TRUE(checkIfIndexInRange(Index(0, 0), bufferSize));
243  EXPECT_TRUE(checkIfIndexInRange(Index(9, 14), bufferSize));
244  EXPECT_FALSE(checkIfIndexInRange(Index(10, 5), bufferSize));
245  EXPECT_FALSE(checkIfIndexInRange(Index(5, 300), bufferSize));
246  EXPECT_FALSE(checkIfIndexInRange(Index(-1, 0), bufferSize));
247  EXPECT_FALSE(checkIfIndexInRange(Index(0, -300), bufferSize));
248 }
249 
250 TEST(boundIndexToRange, All)
251 {
252  int index;
253  int bufferSize = 10;
254 
255  index = 0;
256  boundIndexToRange(index, bufferSize);
257  EXPECT_EQ(0, index);
258 
259  index = 1;
260  boundIndexToRange(index, bufferSize);
261  EXPECT_EQ(1, index);
262 
263  index = -1;
264  boundIndexToRange(index, bufferSize);
265  EXPECT_EQ(0, index);
266 
267  index = 9;
268  boundIndexToRange(index, bufferSize);
269  EXPECT_EQ(9, index);
270 
271  index = 10;
272  boundIndexToRange(index, bufferSize);
273  EXPECT_EQ(9, index);
274 
275  index = 35;
276  boundIndexToRange(index, bufferSize);
277  EXPECT_EQ(9, index);
278 
279  index = -19;
280  boundIndexToRange(index, bufferSize);
281  EXPECT_EQ(0, index);
282 }
283 
284 TEST(wrapIndexToRange, All)
285 {
286  int index;
287  int bufferSize = 10;
288 
289  index = 0;
290  wrapIndexToRange(index, bufferSize);
291  EXPECT_EQ(0, index);
292 
293  index = 1;
294  wrapIndexToRange(index, bufferSize);
295  EXPECT_EQ(1, index);
296 
297  index = -1;
298  wrapIndexToRange(index, bufferSize);
299  EXPECT_EQ(9, index);
300 
301  index = 9;
302  wrapIndexToRange(index, bufferSize);
303  EXPECT_EQ(9, index);
304 
305  index = 10;
306  wrapIndexToRange(index, bufferSize);
307  EXPECT_EQ(0, index);
308 
309  index = 11;
310  wrapIndexToRange(index, bufferSize);
311  EXPECT_EQ(1, index);
312 
313  index = 35;
314  wrapIndexToRange(index, bufferSize);
315  EXPECT_EQ(5, index);
316 
317  index = -9;
318  wrapIndexToRange(index, bufferSize);
319  EXPECT_EQ(1, index);
320 
321  index = -19;
322  wrapIndexToRange(index, bufferSize);
323  EXPECT_EQ(1, index);
324 }
325 
326 TEST(boundPositionToRange, Simple)
327 {
328  double epsilon = 11.0 * numeric_limits<double>::epsilon();
329 
330  Length mapLength(30.0, 10.0);
331  Position mapPosition(0.0, 0.0);
332  Position position;
333 
334  position << 0.0, 0.0;
335  boundPositionToRange(position, mapLength, mapPosition);
336  EXPECT_DOUBLE_EQ(0.0, position.x());
337  EXPECT_DOUBLE_EQ(0.0, position.y());
338 
339  position << 15.0, 5.0;
340  boundPositionToRange(position, mapLength, mapPosition);
341  EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon);
342  EXPECT_GE(15.0, position.x());
343  EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon);
344  EXPECT_GE(5.0, position.y());
345 
346  position << -15.0, -5.0;
347  boundPositionToRange(position, mapLength, mapPosition);
348  EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon);
349  EXPECT_LE(-15.0, position.x());
350  EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon);
351  EXPECT_LE(-5.0, position.y());
352 
353  position << 16.0, 6.0;
354  boundPositionToRange(position, mapLength, mapPosition);
355  EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon);
356  EXPECT_GE(15.0, position.x());
357  EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon);
358  EXPECT_GE(5.0, position.y());
359 
360  position << -16.0, -6.0;
361  boundPositionToRange(position, mapLength, mapPosition);
362  EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon);
363  EXPECT_LE(-15.0, position.x());
364  EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon);
365  EXPECT_LE(-5.0, position.y());
366 
367  position << 1e6, 1e6;
368  boundPositionToRange(position, mapLength, mapPosition);
369  EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon);
370  EXPECT_GE(15.0, position.x());
371  EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon);
372  EXPECT_GE(5.0, position.y());
373 
374  position << -1e6, -1e6;
375  boundPositionToRange(position, mapLength, mapPosition);
376  EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon);
377  EXPECT_LE(-15.0, position.x());
378  EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon);
379  EXPECT_LE(-5.0, position.y());
380 }
381 
382 TEST(boundPositionToRange, Position)
383 {
384  double epsilon = 11.0 * numeric_limits<double>::epsilon();
385 
386  Length mapLength(30.0, 10.0);
387  Position mapPosition(1.0, 2.0);
388  Position position;
389 
390  position << 0.0, 0.0;
391  boundPositionToRange(position, mapLength, mapPosition);
392  EXPECT_DOUBLE_EQ(0.0, position.x());
393  EXPECT_DOUBLE_EQ(0.0, position.y());
394 
395  position << 16.0, 7.0;
396  boundPositionToRange(position, mapLength, mapPosition);
397  EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon);
398  EXPECT_GE(16.0, position.x());
399  EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon);
400  EXPECT_GE(7.0, position.y());
401 
402  position << -14.0, -3.0;
403  boundPositionToRange(position, mapLength, mapPosition);
404  EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon);
405  EXPECT_LE(-14.0, position.x());
406  EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon);
407  EXPECT_LE(-3.0, position.y());
408 
409  position << 17.0, 8.0;
410  boundPositionToRange(position, mapLength, mapPosition);
411  EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon);
412  EXPECT_GE(16.0, position.x());
413  EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon);
414  EXPECT_GE(7.0, position.y());
415 
416  position << -15.0, -4.0;
417  boundPositionToRange(position, mapLength, mapPosition);
418  EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon);
419  EXPECT_LE(-14.0, position.x());
420  EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon);
421  EXPECT_LE(-3.0, position.y());
422 
423  position << 1e6, 1e6;
424  boundPositionToRange(position, mapLength, mapPosition);
425  EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon);
426  EXPECT_GE(16.0, position.x());
427  EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon);
428  EXPECT_GE(7.0, position.y());
429 
430  position << -1e6, -1e6;
431  boundPositionToRange(position, mapLength, mapPosition);
432  EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon);
433  EXPECT_LE(-14.0, position.x());
434  EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon);
435  EXPECT_LE(-3.0, position.y());
436 }
437 
438 TEST(getSubmapInformation, Simple)
439 {
440  // Map
441  Length mapLength(5.0, 4.0);
442  Position mapPosition(0.0, 0.0);
443  double resolution = 1.0;
444  Size bufferSize(5, 4);
445 
446  // Requested submap
447  Position requestedSubmapPosition;
448  Position requestedSubmapLength;
449 
450  // The returned submap indeces
451  Index submapTopLeftIndex;
452  Index submapSize;
453  Position submapPosition;
454  Length submapLength;
455  Index requestedIndexInSubmap;
456 
457  requestedSubmapPosition << 0.0, 0.5;
458  requestedSubmapLength << 0.9, 2.9;
459  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
460  requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize));
461  EXPECT_EQ(2, submapTopLeftIndex(0));
462  EXPECT_EQ(0, submapTopLeftIndex(1));
463  EXPECT_EQ(1, submapSize(0));
464  EXPECT_EQ(3, submapSize(1));
465  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
466  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
467  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
468  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
469  EXPECT_EQ(0, requestedIndexInSubmap(0));
470  EXPECT_EQ(1, requestedIndexInSubmap(1));
471 }
472 
473 TEST(getSubmapInformation, Zero)
474 {
475  // Map
476  Length mapLength(5.0, 4.0);
477  Position mapPosition(0.0, 0.0);
478  double resolution = 1.0;
479  Size bufferSize(5, 4);
480 
481  // Requested submap
482  Position requestedSubmapPosition;
483  Length requestedSubmapLength;
484 
485  // The returned submap indeces
486  Index submapTopLeftIndex;
487  Index submapSize;
488  Position submapPosition;
489  Length submapLength;
490  Index requestedIndexInSubmap;
491 
492  requestedSubmapPosition << -1.0, -0.5;
493  requestedSubmapLength << 0.0, 0.0;
494  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
495  requestedSubmapPosition, requestedSubmapLength,
496  mapLength, mapPosition, resolution, bufferSize));
497  EXPECT_EQ(3, submapTopLeftIndex(0));
498  EXPECT_EQ(2, submapTopLeftIndex(1));
499  EXPECT_EQ(1, submapSize(0));
500  EXPECT_EQ(1, submapSize(1));
501  EXPECT_DOUBLE_EQ(requestedSubmapPosition.x(), submapPosition.x());
502  EXPECT_DOUBLE_EQ(requestedSubmapPosition.y(), submapPosition.y());
503  EXPECT_DOUBLE_EQ(resolution, submapLength(0));
504  EXPECT_DOUBLE_EQ(resolution, submapLength(1));
505  EXPECT_EQ(0, requestedIndexInSubmap(0));
506  EXPECT_EQ(0, requestedIndexInSubmap(1));
507 }
508 
509 TEST(getSubmapInformation, ExceedingBoundaries)
510 {
511  // Map
512  Length mapLength(5.0, 4.0);
513  Position mapPosition(0.0, 0.0);
514  double resolution = 1.0;
515  Size bufferSize(5, 4);
516 
517  // Requested submap
518  Position requestedSubmapPosition;
519  Length requestedSubmapLength;
520 
521  // The returned submap indeces
522  Index submapTopLeftIndex;
523  Size submapSize;
524  Position submapPosition;
525  Length submapLength;
526  Index requestedIndexInSubmap;
527 
528  requestedSubmapPosition << 2.0, 1.5;
529  requestedSubmapLength << 2.9, 2.9;
530  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
531  requestedSubmapPosition, requestedSubmapLength,
532  mapLength, mapPosition, resolution, bufferSize));
533  EXPECT_EQ(0, submapTopLeftIndex(0));
534  EXPECT_EQ(0, submapTopLeftIndex(1));
535  EXPECT_EQ(2, submapSize(0));
536  EXPECT_EQ(2, submapSize(1));
537  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
538  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
539  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
540  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
541  EXPECT_EQ(0, requestedIndexInSubmap(0));
542  EXPECT_EQ(0, requestedIndexInSubmap(1));
543 
544  requestedSubmapPosition << 0.0, 0.0;
545  requestedSubmapLength << 1e6, 1e6;
546  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
547  requestedSubmapPosition, requestedSubmapLength,
548  mapLength, mapPosition, resolution, bufferSize));
549  EXPECT_EQ(0, submapTopLeftIndex(0));
550  EXPECT_EQ(0, submapTopLeftIndex(1));
551  EXPECT_EQ(bufferSize(0), submapSize(0));
552  EXPECT_EQ(bufferSize(1), submapSize(1));
553  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
554  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
555  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
556  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
557  EXPECT_EQ(2, requestedIndexInSubmap(0));
558  EXPECT_LE(1, requestedIndexInSubmap(1));
559  EXPECT_GE(2, requestedIndexInSubmap(1));
560 }
561 
562 TEST(getSubmapInformation, CircularBuffer)
563 {
564  // Map
565  Length mapLength(5.0, 4.0);
566  Position mapPosition(0.0, 0.0);
567  double resolution = 1.0;
568  Size bufferSize(5, 4);
569  Index bufferStartIndex(2, 1);
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 << 0.0, 0.5;
583  requestedSubmapLength << 0.9, 2.9;
584  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
585  requestedSubmapPosition, requestedSubmapLength,
586  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
587  EXPECT_EQ(4, submapTopLeftIndex(0));
588  EXPECT_EQ(1, submapTopLeftIndex(1));
589  EXPECT_EQ(1, submapSize(0));
590  EXPECT_EQ(3, submapSize(1));
591  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
592  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
593  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
594  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
595  EXPECT_EQ(0, requestedIndexInSubmap(0));
596  EXPECT_EQ(1, requestedIndexInSubmap(1));
597 
598  requestedSubmapPosition << 2.0, 1.5;
599  requestedSubmapLength << 2.9, 2.9;
600  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
601  requestedSubmapPosition, requestedSubmapLength,
602  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
603  EXPECT_EQ(2, submapTopLeftIndex(0));
604  EXPECT_EQ(1, submapTopLeftIndex(1));
605  EXPECT_EQ(2, submapSize(0));
606  EXPECT_EQ(2, submapSize(1));
607  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
608  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
609  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
610  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
611  EXPECT_EQ(0, requestedIndexInSubmap(0));
612  EXPECT_EQ(0, requestedIndexInSubmap(1));
613 
614  requestedSubmapPosition << 0.0, 0.0;
615  requestedSubmapLength << 1e6, 1e6;
616  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
617  requestedSubmapPosition, requestedSubmapLength,
618  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
619  EXPECT_EQ(2, submapTopLeftIndex(0));
620  EXPECT_EQ(1, submapTopLeftIndex(1));
621  EXPECT_EQ(bufferSize(0), submapSize(0));
622  EXPECT_EQ(bufferSize(1), submapSize(1));
623  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
624  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
625  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
626  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
627  EXPECT_EQ(2, requestedIndexInSubmap(0));
628  EXPECT_LE(1, requestedIndexInSubmap(1));
629  EXPECT_GE(2, requestedIndexInSubmap(1));
630 }
631 
632 TEST(getSubmapInformation, Debug1)
633 {
634  // Map
635  Length mapLength(4.98, 4.98);
636  Position mapPosition(-4.98, -5.76);
637  double resolution = 0.06;
638  Size bufferSize(83, 83);
639  Index bufferStartIndex(0, 13);
640 
641  // Requested submap
642  Position requestedSubmapPosition(-7.44, -3.42);
643  Length requestedSubmapLength(0.12, 0.12);
644 
645  // The returned submap indeces
646  Index submapTopLeftIndex;
647  Size submapSize;
648  Position submapPosition;
649  Length submapLength;
650  Index requestedIndexInSubmap;
651 
652  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
653  requestedSubmapPosition, requestedSubmapLength,
654  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
655  EXPECT_EQ(2, submapSize(0));
656  EXPECT_EQ(3, submapSize(1));
657  EXPECT_DOUBLE_EQ(0.12, submapLength(0));
658  EXPECT_DOUBLE_EQ(0.18, submapLength(1));
659 }
660 
661 TEST(getSubmapInformation, Debug2)
662 {
663  // Map
664  Length mapLength(4.98, 4.98);
665  Position mapPosition(2.46, -25.26);
666  double resolution = 0.06;
667  Size bufferSize(83, 83);
668  Index bufferStartIndex(42, 6);
669 
670  // Requested submap
671  Position requestedSubmapPosition(0.24, -26.82);
672  Length requestedSubmapLength(0.624614, 0.462276);
673 
674  // The returned submap indeces
675  Index submapTopLeftIndex;
676  Size submapSize;
677  Position submapPosition;
678  Length submapLength;
679  Index requestedIndexInSubmap;
680 
681  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
682  requestedSubmapPosition, requestedSubmapLength,
683  mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
684  EXPECT_LT(0, submapSize(0));
685  EXPECT_LT(0, submapSize(1));
686  EXPECT_LT(0.0, submapLength(0));
687  EXPECT_LT(0.0, submapLength(1));
688 }
689 
690 TEST(getBufferRegionsForSubmap, Trivial)
691 {
692  Size bufferSize(5, 4);
693  Index submapIndex(0, 0);
694  Size submapSize(0, 0);
695  std::vector<BufferRegion> regions;
696 
697  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
698  EXPECT_EQ(1, regions.size());
699  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
700  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
701  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
702  EXPECT_EQ(0, regions[0].getSize()[0]);
703  EXPECT_EQ(0, regions[0].getSize()[1]);
704 
705  submapSize << 0, 7;
706  EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
707 
708  submapSize << 6, 7;
709  EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
710 }
711 
712 TEST(getBufferRegionsForSubmap, Simple)
713 {
714  Size bufferSize(5, 4);
715  Index submapIndex(1, 2);
716  Size submapSize(3, 2);
717  std::vector<BufferRegion> regions;
718 
719  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize));
720  EXPECT_EQ(1, regions.size());
721  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
722  EXPECT_EQ(1, regions[0].getStartIndex()[0]);
723  EXPECT_EQ(2, regions[0].getStartIndex()[1]);
724  EXPECT_EQ(3, regions[0].getSize()[0]);
725  EXPECT_EQ(2, regions[0].getSize()[1]);
726 }
727 
728 TEST(getBufferRegionsForSubmap, CircularBuffer)
729 {
730  Size bufferSize(5, 4);
731  Index submapIndex;
732  Size submapSize;
733  Index bufferStartIndex(3, 1);
734  std::vector<BufferRegion> regions;
735 
736  submapIndex << 3, 1;
737  submapSize << 2, 3;
738  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
739  EXPECT_EQ(1, regions.size());
740  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
741  EXPECT_EQ(3, regions[0].getStartIndex()[0]);
742  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
743  EXPECT_EQ(2, regions[0].getSize()[0]);
744  EXPECT_EQ(3, regions[0].getSize()[1]);
745 
746  submapIndex << 4, 1;
747  submapSize << 2, 3;
748  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
749  EXPECT_EQ(2, regions.size());
750  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
751  EXPECT_EQ(4, regions[0].getStartIndex()[0]);
752  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
753  EXPECT_EQ(1, regions[0].getSize()[0]);
754  EXPECT_EQ(3, regions[0].getSize()[1]);
755  EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant());
756  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
757  EXPECT_EQ(1, regions[1].getStartIndex()[1]);
758  EXPECT_EQ(1, regions[1].getSize()[0]);
759  EXPECT_EQ(3, regions[1].getSize()[1]);
760 
761  submapIndex << 1, 0;
762  submapSize << 2, 1;
763  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));
764  EXPECT_EQ(1, regions.size());
765  EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant());
766  EXPECT_EQ(1, regions[0].getStartIndex()[0]);
767  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
768  EXPECT_EQ(2, regions[0].getSize()[0]);
769  EXPECT_EQ(1, regions[0].getSize()[1]);
770 
771  submapIndex << 3, 1;
772  submapSize << 5, 4;
773  EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));\
774  EXPECT_EQ(4, regions.size());
775  EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant());
776  EXPECT_EQ(3, regions[0].getStartIndex()[0]);
777  EXPECT_EQ(1, regions[0].getStartIndex()[1]);
778  EXPECT_EQ(2, regions[0].getSize()[0]);
779  EXPECT_EQ(3, regions[0].getSize()[1]);
780  EXPECT_EQ(BufferRegion::Quadrant::TopRight, regions[1].getQuadrant());
781  EXPECT_EQ(3, regions[1].getStartIndex()[0]);
782  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
783  EXPECT_EQ(2, regions[1].getSize()[0]);
784  EXPECT_EQ(1, regions[1].getSize()[1]);
785  EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant());
786  EXPECT_EQ(0, regions[2].getStartIndex()[0]);
787  EXPECT_EQ(1, regions[2].getStartIndex()[1]);
788  EXPECT_EQ(3, regions[2].getSize()[0]);
789  EXPECT_EQ(3, regions[2].getSize()[1]);
790  EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant());
791  EXPECT_EQ(0, regions[3].getStartIndex()[0]);
792  EXPECT_EQ(0, regions[3].getStartIndex()[1]);
793  EXPECT_EQ(3, regions[3].getSize()[0]);
794  EXPECT_EQ(1, regions[3].getSize()[1]);
795 }
796 
797 TEST(checkIncrementIndex, Simple)
798 {
799  Index index(0, 0);
800  Size bufferSize(4, 3);
801 
802  EXPECT_TRUE(incrementIndex(index, bufferSize));
803  EXPECT_EQ(0, index[0]);
804  EXPECT_EQ(1, index[1]);
805 
806  EXPECT_TRUE(incrementIndex(index, bufferSize));
807  EXPECT_EQ(0, index[0]);
808  EXPECT_EQ(2, index[1]);
809 
810  EXPECT_TRUE(incrementIndex(index, bufferSize));
811  EXPECT_EQ(1, index[0]);
812  EXPECT_EQ(0, index[1]);
813 
814  EXPECT_TRUE(incrementIndex(index, bufferSize));
815  EXPECT_EQ(1, index[0]);
816  EXPECT_EQ(1, index[1]);
817 
818  for (int i = 0; i < 6; i++) {
819  EXPECT_TRUE(incrementIndex(index, bufferSize));
820  }
821  EXPECT_EQ(3, index[0]);
822  EXPECT_EQ(1, index[1]);
823 
824  EXPECT_TRUE(incrementIndex(index, bufferSize));
825  EXPECT_EQ(3, index[0]);
826  EXPECT_EQ(2, index[1]);
827 
828  EXPECT_FALSE(incrementIndex(index, bufferSize));
829  EXPECT_EQ(index[0], index[0]);
830  EXPECT_EQ(index[1], index[1]);
831 }
832 
833 TEST(checkIncrementIndex, CircularBuffer)
834 {
835  Size bufferSize(4, 3);
836  Index bufferStartIndex(2, 1);
837  Index index(bufferStartIndex);
838 
839  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
840  EXPECT_EQ(2, index[0]);
841  EXPECT_EQ(2, index[1]);
842 
843  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
844  EXPECT_EQ(2, index[0]);
845  EXPECT_EQ(0, index[1]);
846 
847  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
848  EXPECT_EQ(3, index[0]);
849  EXPECT_EQ(1, index[1]);
850 
851  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
852  EXPECT_EQ(3, index[0]);
853  EXPECT_EQ(2, index[1]);
854 
855  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
856  EXPECT_EQ(3, index[0]);
857  EXPECT_EQ(0, index[1]);
858 
859  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
860  EXPECT_EQ(0, index[0]);
861  EXPECT_EQ(1, index[1]);
862 
863  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
864  EXPECT_EQ(0, index[0]);
865  EXPECT_EQ(2, index[1]);
866 
867  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
868  EXPECT_EQ(0, index[0]);
869  EXPECT_EQ(0, index[1]);
870 
871  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
872  EXPECT_EQ(1, index[0]);
873  EXPECT_EQ(1, index[1]);
874 
875  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
876  EXPECT_EQ(1, index[0]);
877  EXPECT_EQ(2, index[1]);
878 
879  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
880  EXPECT_EQ(1, index[0]);
881  EXPECT_EQ(0, index[1]);
882 
883  EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex));
884  EXPECT_EQ(index[0], index[0]);
885  EXPECT_EQ(index[1], index[1]);
886 }
887 
888 TEST(checkIncrementIndexForSubmap, Simple)
889 {
890  Index submapIndex(0, 0);
891  Index index;
892  Index submapTopLeftIndex(3, 1);
893  Size submapBufferSize(2, 4);
894  Size bufferSize(8, 5);
895 
896  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
897  EXPECT_EQ(0, submapIndex[0]);
898  EXPECT_EQ(1, submapIndex[1]);
899  EXPECT_EQ(3, index[0]);
900  EXPECT_EQ(2, index[1]);
901 
902  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
903  EXPECT_EQ(0, submapIndex[0]);
904  EXPECT_EQ(2, submapIndex[1]);
905  EXPECT_EQ(3, index[0]);
906  EXPECT_EQ(3, index[1]);
907 
908  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
909  EXPECT_EQ(0, submapIndex[0]);
910  EXPECT_EQ(3, submapIndex[1]);
911  EXPECT_EQ(3, index[0]);
912  EXPECT_EQ(4, index[1]);
913 
914  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
915  EXPECT_EQ(1, submapIndex[0]);
916  EXPECT_EQ(0, submapIndex[1]);
917  EXPECT_EQ(4, index[0]);
918  EXPECT_EQ(1, index[1]);
919 
920  submapIndex << 1, 2;
921  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
922  EXPECT_EQ(1, submapIndex[0]);
923  EXPECT_EQ(3, submapIndex[1]);
924  EXPECT_EQ(4, index[0]);
925  EXPECT_EQ(4, index[1]);
926 
927  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
928 
929  submapIndex << 2, 0;
930  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
931 }
932 
933 TEST(checkIncrementIndexForSubmap, CircularBuffer)
934 {
935  Index submapIndex(0, 0);
936  Index index;
937  Index submapTopLeftIndex(6, 3);
938  Size submapBufferSize(2, 4);
939  Size bufferSize(8, 5);
940  Index bufferStartIndex(3, 2);
941 
942  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
943  EXPECT_EQ(0, submapIndex[0]);
944  EXPECT_EQ(1, submapIndex[1]);
945  EXPECT_EQ(6, index[0]);
946  EXPECT_EQ(4, index[1]);
947 
948  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
949  EXPECT_EQ(0, submapIndex[0]);
950  EXPECT_EQ(2, submapIndex[1]);
951  EXPECT_EQ(6, index[0]);
952  EXPECT_EQ(0, index[1]);
953 
954  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
955  EXPECT_EQ(0, submapIndex[0]);
956  EXPECT_EQ(3, submapIndex[1]);
957  EXPECT_EQ(6, index[0]);
958  EXPECT_EQ(1, index[1]);
959 
960  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
961  EXPECT_EQ(1, submapIndex[0]);
962  EXPECT_EQ(0, submapIndex[1]);
963  EXPECT_EQ(7, index[0]);
964  EXPECT_EQ(3, index[1]);
965 
966  submapIndex << 1, 2;
967  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
968  EXPECT_EQ(1, submapIndex[0]);
969  EXPECT_EQ(3, submapIndex[1]);
970  EXPECT_EQ(7, index[0]);
971  EXPECT_EQ(1, index[1]);
972 
973  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
974 
975  submapIndex << 2, 0;
976  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
977 }
978 
979 TEST(getIndexFromLinearIndex, Simple)
980 {
981  EXPECT_TRUE((Index(0, 0) == getIndexFromLinearIndex(0, Size(8, 5), false)).all());
982  EXPECT_TRUE((Index(1, 0) == getIndexFromLinearIndex(1, Size(8, 5), false)).all());
983  EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(1, Size(8, 5), true)).all());
984  EXPECT_TRUE((Index(2, 0) == getIndexFromLinearIndex(2, Size(8, 5), false)).all());
985  EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all());
986  EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all());
987 }
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:435
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:202
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:305
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:245
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:117
grid_map
Definition: BufferRegion.hpp:13
grid_map::checkIfPositionWithinMap
bool checkIfPositionWithinMap(const Position &position, const Length &mapLength, const Position &mapPosition)
Definition: GridMapMath.cpp:133
grid_map::getPositionShiftFromIndexShift
bool getPositionShiftFromIndexShift(Vector &positionShift, const Index &indexShift, const double &resolution)
Definition: GridMapMath.cpp:172
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:413
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:490
grid_map::internal::getQuadrant
BufferRegion::Quadrant getQuadrant(const Index &index, const Index &bufferStartIndex)
Definition: GridMapMath.cpp:89
grid_map::boundIndexToRange
void boundIndexToRange(Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:189
grid_map::checkIfIndexInRange
bool checkIfIndexInRange(const Index &index, const Size &bufferSize)
Definition: GridMapMath.cpp:180
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:102
grid_map::boundPositionToRange
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
Definition: GridMapMath.cpp:215
grid_map::getIndexShiftFromPositionShift
bool getIndexShiftFromPositionShift(Index &indexShift, const Vector &positionShift, const double &resolution)
Definition: GridMapMath.cpp:157
grid_map::Length
Eigen::Array2d Length
Definition: TypeDefs.hpp:24

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:08