12 #include <gtest/gtest.h> 22 GridMap map({
"layer_a",
"layer_b"});
24 map[
"layer_a"].setConstant(1.0);
25 map[
"layer_b"].setConstant(2.0);
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));
40 GridMap map({
"layer_a",
"layer_b"});
42 map[
"layer_a"].setConstant(1.0);
43 map[
"layer_b"].setConstant(2.0);
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));
61 map.
add(
"layer", 0.0);
63 std::vector<BufferRegion> regions;
67 EXPECT_EQ(3, startIndex(0));
68 EXPECT_EQ(2, startIndex(1));
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]);
91 const auto heightLayerName =
"height";
94 map.
add(heightLayerName, 0.0);
96 map.
get(heightLayerName)(0,0) = 1.0;
99 Eigen::Isometry3d transform;
101 transform.translation().x() = 0.0;
102 transform.translation().y() = 0.0;
103 transform.translation().z() = 0.0;
105 transform.linear()(0,0) = 0.0;
106 transform.linear()(0,1) = -1.0;
107 transform.linear()(0,2) = 0.0;
109 transform.linear()(1,0) = 1.0;
110 transform.linear()(1,1) = 0.0;
111 transform.linear()(1,2) = 0.0;
113 transform.linear()(2,0) = 0.0;
114 transform.linear()(2,1) = 0.0;
115 transform.linear()(2,2) = 1.0;
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));
129 GridMap map({
"layer_a",
"layer_b"});
131 map[
"layer_a"].setConstant(1.0);
132 map[
"layer_b"].setConstant(2.0);
137 const Position clippedPositionInMap = map.getClosestPositionInMap(positionInMap);
138 const Position clippedPositionOutMap = map.getClosestPositionInMap(positionOutMap);
141 EXPECT_NEAR(clippedPositionInMap.x(),positionInMap.x(), 1e-6);
142 EXPECT_NEAR(clippedPositionInMap.y(), positionInMap.y(), 1e-6);
145 EXPECT_TRUE(!map.isInside(positionOutMap));
148 EXPECT_TRUE(map.isInside(clippedPositionOutMap));
179 outsidePosition =
Position(1.0, 1.0);
180 auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
181 bool isInside = map.getIndex(closestInsidePosition, insideIndex);
183 auto expectedPosition =
Position(0.5, 0.5);
184 auto expectedIndex =
Index(0, 0);
187 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
188 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
191 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
192 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
195 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
196 << closestInsidePosition << std::endl
197 <<
" index is: " << std::endl
198 << insideIndex << std::endl;
201 outsidePosition =
Position(1.0, 0.0);
202 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
203 isInside = map.getIndex(closestInsidePosition, insideIndex);
205 expectedPosition =
Position(0.5, 0.0);
206 expectedIndex =
Index(0, 10);
209 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
210 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
213 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
214 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
217 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
218 << closestInsidePosition << std::endl
219 <<
" index is: " << std::endl
220 << insideIndex << std::endl;
223 outsidePosition =
Position(1.0, -1.0);
224 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
225 isInside = map.getIndex(closestInsidePosition, insideIndex);
227 expectedPosition =
Position(0.5, -0.5);
228 expectedIndex =
Index(0, 19);
231 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
232 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
235 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
236 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
239 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
240 << closestInsidePosition << std::endl
241 <<
" index is: " << std::endl
242 << insideIndex << std::endl;
245 outsidePosition =
Position(0.0, 1.0);
246 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
247 isInside = map.getIndex(closestInsidePosition, insideIndex);
249 expectedPosition =
Position(0.0, 0.5);
250 expectedIndex =
Index(10, 0);
253 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
254 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
257 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
258 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
261 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
262 << closestInsidePosition << std::endl
263 <<
" index is: " << std::endl
264 << insideIndex << std::endl;
267 outsidePosition =
Position(0.0, -1.0);
268 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
269 isInside = map.getIndex(closestInsidePosition, insideIndex);
271 expectedPosition =
Position(0.0, -0.5);
272 expectedIndex =
Index(10, 19);
275 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
276 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
279 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
280 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
283 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
284 << closestInsidePosition << std::endl
285 <<
" index is: " << std::endl
286 << insideIndex << std::endl;
289 outsidePosition =
Position(-1.0, 1.0);
290 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
291 isInside = map.getIndex(closestInsidePosition, insideIndex);
293 expectedPosition =
Position(-0.5, 0.5);
294 expectedIndex =
Index(19, 0);
297 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
298 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
301 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
302 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
305 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
306 << closestInsidePosition << std::endl
307 <<
" index is: " << std::endl
308 << insideIndex << std::endl;
311 outsidePosition =
Position(-1.0, 0.0);
312 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
313 isInside = map.getIndex(closestInsidePosition, insideIndex);
315 expectedPosition =
Position(-0.5, 0.0);
316 expectedIndex =
Index(19, 10);
319 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
320 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
323 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
324 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
327 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
328 << closestInsidePosition << std::endl
329 <<
" index is: " << std::endl
330 << insideIndex << std::endl;
333 outsidePosition =
Position(-1.0, -1.0);
334 closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
335 isInside = map.getIndex(closestInsidePosition, insideIndex);
337 expectedPosition =
Position(-0.5, -0.5);
338 expectedIndex =
Index(19, 19);
341 EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
342 EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
345 EXPECT_EQ(expectedIndex.x(), insideIndex.x()) <<
"closestInsidePosition" << closestInsidePosition;
346 EXPECT_EQ(expectedIndex.y(), insideIndex.y()) <<
"closestInsidePosition" << closestInsidePosition;
349 EXPECT_TRUE(isInside) <<
"position is: " << std::endl
350 << closestInsidePosition << std::endl
351 <<
" index is: " << std::endl
352 << insideIndex << std::endl;
355 TEST(AddDataFrom, ExtendMapAligned)
359 map1.
add(
"zero", 0.0);
360 map1.
add(
"one", 1.0);
364 map2.
add(
"one", 1.1);
365 map2.
add(
"two", 2.0);
370 EXPECT_TRUE(map1.
exists(
"two"));
372 EXPECT_DOUBLE_EQ(6.0, map1.
getLength().x());
373 EXPECT_DOUBLE_EQ(6.0, map1.
getLength().y());
381 TEST(AddDataFrom, ExtendMapNotAligned)
386 map1.
add(
"one", 1.0);
387 map1.
add(
"zero", 0.0);
391 map2.
add(
"nan", 1.0);
392 map2.
add(
"one", 1.1);
393 map2.
add(
"two", 2.0);
396 std::vector<std::string> stringVector;
397 stringVector.push_back(
"nan");
398 map1.
addDataFrom(map2,
true,
false,
false, stringVector);
402 EXPECT_FALSE(map1.
exists(
"two"));
404 EXPECT_DOUBLE_EQ(8.0, map1.
getLength().x());
405 EXPECT_DOUBLE_EQ(8.0, map1.
getLength().y());
408 EXPECT_FALSE(map1.
isValid(index,
"nan"));
417 map1.
add(
"zero", 0.0);
422 map2.
add(
"one", 1.0);
423 map2.
add(
"two", 2.0);
430 EXPECT_TRUE(map1.
exists(
"two"));
432 EXPECT_DOUBLE_EQ(5.0, map1.
getLength().x());
433 EXPECT_DOUBLE_EQ(5.0, map1.
getLength().y());
437 EXPECT_FALSE(map1.
isValid(index,
"one"));
441 TEST(ValueAtPosition, NearestNeighbor)
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;
458 value = map.atPosition(
"types",
Position(1.35,-0.4));
459 EXPECT_DOUBLE_EQ((
float)3.8, value);
461 value = map.atPosition(
"types",
Position(-0.3,0.0));
462 EXPECT_DOUBLE_EQ(1.0, value);
465 TEST(ValueAtPosition, LinearInterpolated)
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;
483 value = map.atPosition(
"types",
Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
484 EXPECT_DOUBLE_EQ(2.0, value);
486 value = map.atPosition(
"types",
Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR);
487 EXPECT_DOUBLE_EQ(1.5, value);
489 value = map.atPosition(
"types",
Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
490 EXPECT_NEAR(2.1963200, value, 0.0000001);
const Length & getLength() const
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
float & atPosition(const std::string &layer, const Position &position)
const Index & getStartIndex() const
bool getPosition(const Index &index, Position &position) const
void setBasicLayers(const std::vector< std::string > &basicLayers)
const std::string & getFrameId() const
GridMap getTransformedMap(const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId, const double sampleRatio=0.0) const
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
bool addDataFrom(const GridMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >())
bool isValid(const Index &index) const
bool exists(const std::string &layer) const
TEST(GridMap, CopyConstructor)
const std::vector< std::string > & getLayers() const
bool isInside(const Position &position) const
const Matrix & get(const std::string &layer) const
void add(const std::string &layer, const double value=NAN)
bool getIndex(const Position &position, Index &index) const
const Size & getSize() const