GridMapTest.cpp
Go to the documentation of this file.
1 /*
2  * GridMapTest.cpp
3  *
4  * Created on: Aug 26, 2015
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
11 // gtest
12 #include <gtest/gtest.h>
13 
14 // Math
15 #include <math.h>
16 
17 using namespace std;
18 using namespace grid_map;
19 
20 TEST(GridMap, CopyConstructor)
21 {
22  GridMap map({"layer_a", "layer_b"});
23  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
24  map["layer_a"].setConstant(1.0);
25  map["layer_b"].setConstant(2.0);
26  GridMap mapCopy(map);
27  EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
28  EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
29  EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
30  EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
31  EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
32  EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
33  EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
34  EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
35  EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
36 }
37 
38 TEST(GridMap, CopyAssign)
39 {
40  GridMap map({"layer_a", "layer_b"});
41  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
42  map["layer_a"].setConstant(1.0);
43  map["layer_b"].setConstant(2.0);
44  GridMap mapCopy;
45  mapCopy = map;
46  EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
47  EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
48  EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
49  EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
50  EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
51  EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
52  EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
53  EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
54  EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
55 }
56 
57 TEST(GridMap, Move)
58 {
59  GridMap map;
60  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
61  map.add("layer", 0.0);
62  map.setBasicLayers(map.getLayers());
63  std::vector<BufferRegion> regions;
64  map.move(Position(-3.0, -2.0), regions);
65  Index startIndex = map.getStartIndex();
66 
67  EXPECT_EQ(3, startIndex(0));
68  EXPECT_EQ(2, startIndex(1));
69 
70  EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map.
71  EXPECT_TRUE(map.isValid(Index(3, 2)));
72  EXPECT_FALSE(map.isValid(Index(2, 2)));
73  EXPECT_FALSE(map.isValid(Index(3, 1)));
74  EXPECT_TRUE(map.isValid(Index(7, 4)));
75 
76  EXPECT_EQ(2, regions.size());
77  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
78  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
79  EXPECT_EQ(3, regions[0].getSize()[0]);
80  EXPECT_EQ(5, regions[0].getSize()[1]);
81  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
82  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
83  EXPECT_EQ(8, regions[1].getSize()[0]);
84  EXPECT_EQ(2, regions[1].getSize()[1]);
85 }
86 
87 TEST(GridMap, Transform)
88 {
89  // Initial map.
90  GridMap map;
91  const auto heightLayerName = "height";
92 
93  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
94  map.add(heightLayerName, 0.0);
95  map.setBasicLayers(map.getLayers());
96  map.get(heightLayerName)(0,0) = 1.0;
97 
98  // Transformation (90° rotation).
99  Eigen::Isometry3d transform;
100 
101  transform.translation().x() = 0.0;
102  transform.translation().y() = 0.0;
103  transform.translation().z() = 0.0;
104 
105  transform.linear()(0,0) = 0.0;
106  transform.linear()(0,1) = -1.0;
107  transform.linear()(0,2) = 0.0;
108 
109  transform.linear()(1,0) = 1.0;
110  transform.linear()(1,1) = 0.0;
111  transform.linear()(1,2) = 0.0;
112 
113  transform.linear()(2,0) = 0.0;
114  transform.linear()(2,1) = 0.0;
115  transform.linear()(2,2) = 1.0;
116 
117  // Apply affine transformation.
118  const GridMap transformedMap = map.getTransformedMap(transform, heightLayerName, map.getFrameId(), 0.25);
119 
120  // Check if map has been rotated by 90° about z
121  EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6);
122  EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6);
123  EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size());
124  EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0,0), transformedMap.get(heightLayerName)(19,0));
125 }
126 
127 TEST(GridMap, ClipToMap)
128 {
129  GridMap map({"layer_a", "layer_b"});
130  map.setGeometry(Length(1.0, 1.0), 0.1, Position(0.5, 0.5));
131  map["layer_a"].setConstant(1.0);
132  map["layer_b"].setConstant(2.0);
133 
134  const Position positionInMap = Position(0.4, 0.3); // position located inside the map
135  const Position positionOutMap = Position(1.0, 2.0); // position located outside the map
136 
137  const Position clippedPositionInMap = map.getClosestPositionInMap(positionInMap);
138  const Position clippedPositionOutMap = map.getClosestPositionInMap(positionOutMap);
139 
140  // Check if position-in-map remains unchanged.
141  EXPECT_NEAR(clippedPositionInMap.x(),positionInMap.x(), 1e-6);
142  EXPECT_NEAR(clippedPositionInMap.y(), positionInMap.y(), 1e-6);
143 
144  // Check if position-out-map is indeed outside of the map.
145  EXPECT_TRUE(!map.isInside(positionOutMap));
146 
147  // Check if position-out-map has been projected into the map.
148  EXPECT_TRUE(map.isInside(clippedPositionOutMap));
149 }
150 
151 
152 
153 TEST(GridMap, ClipToMap2)
154 {
155  GridMap map({"types"});
156  map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
157 
158  // Test 8 points outside of map.
159  /*
160  * A B C
161  * +---+
162  * | | X
163  * D| |E ^
164  * | | |
165  * +---+ Y<--+
166  * F G H
167  *
168  * Note: Position to index alignment is an half open interval.
169  * An example position of 0.5 is assigned to the upper index.
170  * The interval in the current example is:
171  * Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...)
172  * Index: 8 9 10 11
173  */
174 
175  Index insideIndex;
176  Position outsidePosition;
177 
178  // Point A
179  outsidePosition = Position(1.0, 1.0);
180  auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
181  bool isInside = map.getIndex(closestInsidePosition, insideIndex);
182 
183  auto expectedPosition = Position(0.5, 0.5);
184  auto expectedIndex = Index(0, 0);
185 
186  // Check position.
187  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
188  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
189 
190  // Check index.
191  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
192  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
193 
194  // Check if index is inside.
195  EXPECT_TRUE(isInside) << "position is: " << std::endl
196  << closestInsidePosition << std::endl
197  << " index is: " << std::endl
198  << insideIndex << std::endl;
199 
200  // Point B
201  outsidePosition = Position(1.0, 0.0);
202  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
203  isInside = map.getIndex(closestInsidePosition, insideIndex);
204 
205  expectedPosition = Position(0.5, 0.0);
206  expectedIndex = Index(0, 10);
207 
208  // Check position.
209  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
210  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
211 
212  // Check index.
213  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
214  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
215 
216  // Check if index is inside.
217  EXPECT_TRUE(isInside) << "position is: " << std::endl
218  << closestInsidePosition << std::endl
219  << " index is: " << std::endl
220  << insideIndex << std::endl;
221 
222  // Point C
223  outsidePosition = Position(1.0, -1.0);
224  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
225  isInside = map.getIndex(closestInsidePosition, insideIndex);
226 
227  expectedPosition = Position(0.5, -0.5);
228  expectedIndex = Index(0, 19);
229 
230  // Check position.
231  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
232  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
233 
234  // Check index.
235  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
236  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
237 
238  // Check if index is inside.
239  EXPECT_TRUE(isInside) << "position is: " << std::endl
240  << closestInsidePosition << std::endl
241  << " index is: " << std::endl
242  << insideIndex << std::endl;
243 
244  // Point D
245  outsidePosition = Position(0.0, 1.0);
246  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
247  isInside = map.getIndex(closestInsidePosition, insideIndex);
248 
249  expectedPosition = Position(0.0, 0.5);
250  expectedIndex = Index(10, 0);
251 
252  // Check position.
253  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
254  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
255 
256  // Check index.
257  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
258  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
259 
260  // Check if index is inside.
261  EXPECT_TRUE(isInside) << "position is: " << std::endl
262  << closestInsidePosition << std::endl
263  << " index is: " << std::endl
264  << insideIndex << std::endl;
265 
266  // Point E
267  outsidePosition = Position(0.0, -1.0);
268  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
269  isInside = map.getIndex(closestInsidePosition, insideIndex);
270 
271  expectedPosition = Position(0.0, -0.5);
272  expectedIndex = Index(10, 19);
273 
274  // Check position.
275  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
276  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
277 
278  // Check index.
279  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
280  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
281 
282  // Check if index is inside.
283  EXPECT_TRUE(isInside) << "position is: " << std::endl
284  << closestInsidePosition << std::endl
285  << " index is: " << std::endl
286  << insideIndex << std::endl;
287 
288  // Point F
289  outsidePosition = Position(-1.0, 1.0);
290  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
291  isInside = map.getIndex(closestInsidePosition, insideIndex);
292 
293  expectedPosition = Position(-0.5, 0.5);
294  expectedIndex = Index(19, 0);
295 
296  // Check position.
297  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
298  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
299 
300  // Check index.
301  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
302  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
303 
304  // Check if index is inside.
305  EXPECT_TRUE(isInside) << "position is: " << std::endl
306  << closestInsidePosition << std::endl
307  << " index is: " << std::endl
308  << insideIndex << std::endl;
309 
310  // Point G
311  outsidePosition = Position(-1.0, 0.0);
312  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
313  isInside = map.getIndex(closestInsidePosition, insideIndex);
314 
315  expectedPosition = Position(-0.5, 0.0);
316  expectedIndex = Index(19, 10);
317 
318  // Check position.
319  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
320  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
321 
322  // Check index.
323  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
324  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
325 
326  // Check if index is inside.
327  EXPECT_TRUE(isInside) << "position is: " << std::endl
328  << closestInsidePosition << std::endl
329  << " index is: " << std::endl
330  << insideIndex << std::endl;
331 
332  // Point H
333  outsidePosition = Position(-1.0, -1.0);
334  closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
335  isInside = map.getIndex(closestInsidePosition, insideIndex);
336 
337  expectedPosition = Position(-0.5, -0.5);
338  expectedIndex = Index(19, 19);
339 
340  // Check position.
341  EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
342  EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
343 
344  // Check index.
345  EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
346  EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
347 
348  // Check if index is inside.
349  EXPECT_TRUE(isInside) << "position is: " << std::endl
350  << closestInsidePosition << std::endl
351  << " index is: " << std::endl
352  << insideIndex << std::endl;
353 }
354 
355 TEST(AddDataFrom, ExtendMapAligned)
356 {
357  GridMap map1, map2;
358  map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
359  map1.add("zero", 0.0);
360  map1.add("one", 1.0);
361  map1.setBasicLayers(map1.getLayers());
362 
363  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
364  map2.add("one", 1.1);
365  map2.add("two", 2.0);
366  map2.setBasicLayers(map1.getLayers());
367 
368  map1.addDataFrom(map2, true, true, true);
369 
370  EXPECT_TRUE(map1.exists("two"));
371  EXPECT_TRUE(map1.isInside(Position(3.0, 3.0)));
372  EXPECT_DOUBLE_EQ(6.0, map1.getLength().x());
373  EXPECT_DOUBLE_EQ(6.0, map1.getLength().y());
374  EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x());
375  EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y());
376  EXPECT_NEAR(1.1, map1.atPosition("one", Position(2, 2)), 1e-4);
377  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(-2, -2)));
378  EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
379 }
380 
381 TEST(AddDataFrom, ExtendMapNotAligned)
382 {
383  GridMap map1, map2;
384  map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0)); // bufferSize(6, 6)
385  map1.add("nan");
386  map1.add("one", 1.0);
387  map1.add("zero", 0.0);
388  map1.setBasicLayers(map1.getLayers());
389 
390  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2));
391  map2.add("nan", 1.0);
392  map2.add("one", 1.1);
393  map2.add("two", 2.0);
394  map2.setBasicLayers(map1.getLayers());
395 
396  std::vector<std::string> stringVector;
397  stringVector.push_back("nan");
398  map1.addDataFrom(map2, true, false, false, stringVector);
399  Index index;
400  map1.getIndex(Position(-2, -2), index);
401 
402  EXPECT_FALSE(map1.exists("two"));
403  EXPECT_TRUE(map1.isInside(Position(4.0, 4.0)));
404  EXPECT_DOUBLE_EQ(8.0, map1.getLength().x());
405  EXPECT_DOUBLE_EQ(8.0, map1.getLength().y());
406  EXPECT_DOUBLE_EQ(1.0, map1.getPosition().x());
407  EXPECT_DOUBLE_EQ(1.0, map1.getPosition().y());
408  EXPECT_FALSE(map1.isValid(index, "nan"));
409  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(0.0, 0.0)));
410  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("nan", Position(3.0, 3.0)));
411 }
412 
413 TEST(AddDataFrom, CopyData)
414 {
415  GridMap map1, map2;
416  map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
417  map1.add("zero", 0.0);
418  map1.add("one");
419  map1.setBasicLayers(map1.getLayers());
420 
421  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
422  map2.add("one", 1.0);
423  map2.add("two", 2.0);
424  map2.setBasicLayers(map1.getLayers());
425 
426  map1.addDataFrom(map2, false, false, true);
427  Index index;
428  map1.getIndex(Position(-2, -2), index);
429 
430  EXPECT_TRUE(map1.exists("two"));
431  EXPECT_FALSE(map1.isInside(Position(3.0, 3.0)));
432  EXPECT_DOUBLE_EQ(5.0, map1.getLength().x());
433  EXPECT_DOUBLE_EQ(5.0, map1.getLength().y());
434  EXPECT_DOUBLE_EQ(0.0, map1.getPosition().x());
435  EXPECT_DOUBLE_EQ(0.0, map1.getPosition().y());
436  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(2, 2)));
437  EXPECT_FALSE(map1.isValid(index, "one"));
438  EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
439 }
440 
441 TEST(ValueAtPosition, NearestNeighbor)
442 {
443  GridMap map( { "types" });
444  map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
445 
446  map.at("types", Index(0,0)) = 0.5;
447  map.at("types", Index(0,1)) = 3.8;
448  map.at("types", Index(0,2)) = 2.0;
449  map.at("types", Index(1,0)) = 2.1;
450  map.at("types", Index(1,1)) = 1.0;
451  map.at("types", Index(1,2)) = 2.0;
452  map.at("types", Index(2,0)) = 1.0;
453  map.at("types", Index(2,1)) = 2.0;
454  map.at("types", Index(2,2)) = 2.0;
455 
456  double value;
457 
458  value = map.atPosition("types", Position(1.35,-0.4));
459  EXPECT_DOUBLE_EQ((float)3.8, value);
460 
461  value = map.atPosition("types", Position(-0.3,0.0));
462  EXPECT_DOUBLE_EQ(1.0, value);
463 }
464 
465 TEST(ValueAtPosition, LinearInterpolated)
466 {
467  GridMap map( { "types" });
468  map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
469 
470  map.at("types", Index(0,0)) = 0.5;
471  map.at("types", Index(0,1)) = 3.8;
472  map.at("types", Index(0,2)) = 2.0;
473  map.at("types", Index(1,0)) = 2.1;
474  map.at("types", Index(1,1)) = 1.0;
475  map.at("types", Index(1,2)) = 2.0;
476  map.at("types", Index(2,0)) = 1.0;
477  map.at("types", Index(2,1)) = 2.0;
478  map.at("types", Index(2,2)) = 2.0;
479 
480  double value;
481 
482  // Close to the border -> reverting to INTER_NEAREST.
483  value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
484  EXPECT_DOUBLE_EQ(2.0, value);
485  // In between 1.0 and 2.0 field.
486  value = map.atPosition("types", Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR);
487  EXPECT_DOUBLE_EQ(1.5, value);
488  // Calculated "by Hand".
489  value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
490  EXPECT_NEAR(2.1963200, value, 0.0000001);
491 }
const Length & getLength() const
Definition: GridMap.cpp:644
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Definition: GridMap.cpp:45
float & atPosition(const std::string &layer, const Position &position)
Definition: GridMap.cpp:160
const Index & getStartIndex() const
Definition: GridMap.cpp:664
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:237
void setBasicLayers(const std::vector< std::string > &basicLayers)
Definition: GridMap.cpp:68
const std::string & getFrameId() const
Definition: GridMap.cpp:640
GridMap getTransformedMap(const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId, const double sampleRatio=0.0) const
Definition: GridMap.cpp:341
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
Definition: GridMap.cpp:451
bool addDataFrom(const GridMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >())
Definition: GridMap.cpp:523
bool isValid(const Index &index) const
Definition: GridMap.cpp:249
bool exists(const std::string &layer) const
Definition: GridMap.cpp:107
TEST(GridMap, CopyConstructor)
Definition: GridMapTest.cpp:20
const std::vector< std::string > & getLayers() const
Definition: GridMap.cpp:156
bool isInside(const Position &position) const
Definition: GridMap.cpp:241
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
const Matrix & get(const std::string &layer) const
Definition: GridMap.cpp:111
void add(const std::string &layer, const double value=NAN)
Definition: GridMap.cpp:89
bool getIndex(const Position &position, Index &index) const
Definition: GridMap.cpp:233
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
const Size & getSize() const
Definition: GridMap.cpp:656


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