496 lines
18 KiB
C++
496 lines
18 KiB
C++
/*
|
|
* GridMapTest.cpp
|
|
*
|
|
* Created on: Aug 26, 2015
|
|
* Author: Péter Fankhauser
|
|
* Institute: ETH Zurich, ANYbotics
|
|
*/
|
|
|
|
#include "grid_map_core/GridMap.hpp"
|
|
|
|
// gtest
|
|
#include <gtest/gtest.h>
|
|
|
|
namespace robot_grid_map {
|
|
|
|
TEST(GridMap, CopyConstructor) {
|
|
GridMap map({"layer_a", "layer_b"});
|
|
map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
|
|
map["layer_a"].setConstant(1.0);
|
|
map["layer_b"].setConstant(2.0);
|
|
GridMap mapCopy(map);
|
|
EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
|
|
EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
|
|
EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
|
|
EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
|
|
EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
|
|
EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
|
|
EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
|
|
EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
|
|
EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
|
|
}
|
|
|
|
TEST(GridMap, CopyAssign)
|
|
{
|
|
GridMap map({"layer_a", "layer_b"});
|
|
map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
|
|
map["layer_a"].setConstant(1.0);
|
|
map["layer_b"].setConstant(2.0);
|
|
GridMap mapCopy;
|
|
mapCopy = map;
|
|
EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
|
|
EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
|
|
EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
|
|
EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
|
|
EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
|
|
EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
|
|
EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
|
|
EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
|
|
EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
|
|
}
|
|
|
|
TEST(GridMap, Move)
|
|
{
|
|
GridMap map;
|
|
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
|
|
map.add("layer", 0.0);
|
|
map.setBasicLayers(map.getLayers());
|
|
std::vector<BufferRegion> regions;
|
|
map.move(Position(-3.0, -2.0), regions);
|
|
Index startIndex = map.getStartIndex();
|
|
|
|
EXPECT_EQ(3, startIndex(0));
|
|
EXPECT_EQ(2, startIndex(1));
|
|
|
|
Eigen::Matrix<bool, 8, 5> isValidExpected;
|
|
isValidExpected << false, false, false, false, false, // clang-format off
|
|
false, false, false, false, false,
|
|
false, false, false, false, false,
|
|
false, false, true, true, true,
|
|
false, false, true, true, true,
|
|
false, false, true, true, true,
|
|
false, false, true, true, true,
|
|
false, false, true, true, true; // clang-format on
|
|
for(int row{0}; row < 8; row++){
|
|
for(int col{0}; col < 5; col++){
|
|
EXPECT_EQ(map.isValid(Index(row, col)), isValidExpected(row, col)) << "Value of map.isValid at ["<<row << ", " << col <<"] is unexpected!";
|
|
}
|
|
}
|
|
|
|
EXPECT_EQ(2, regions.size());
|
|
EXPECT_EQ(0, regions[0].getStartIndex()[0]);
|
|
EXPECT_EQ(0, regions[0].getStartIndex()[1]);
|
|
EXPECT_EQ(3, regions[0].getSize()[0]);
|
|
EXPECT_EQ(5, regions[0].getSize()[1]);
|
|
EXPECT_EQ(0, regions[1].getStartIndex()[0]);
|
|
EXPECT_EQ(0, regions[1].getStartIndex()[1]);
|
|
EXPECT_EQ(8, regions[1].getSize()[0]);
|
|
EXPECT_EQ(2, regions[1].getSize()[1]);
|
|
}
|
|
|
|
TEST(GridMap, Transform)
|
|
{
|
|
// Initial map.
|
|
GridMap map;
|
|
constexpr auto heightLayerName = "height";
|
|
|
|
map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
|
|
map.add(heightLayerName, 0.0);
|
|
map.setBasicLayers(map.getLayers());
|
|
map.get(heightLayerName)(0,0) = 1.0;
|
|
|
|
// Transformation (90° rotation).
|
|
Eigen::Isometry3d transform;
|
|
|
|
transform.translation().x() = 0.0;
|
|
transform.translation().y() = 0.0;
|
|
transform.translation().z() = 0.0;
|
|
|
|
transform.linear()(0,0) = 0.0;
|
|
transform.linear()(0,1) = -1.0;
|
|
transform.linear()(0,2) = 0.0;
|
|
|
|
transform.linear()(1,0) = 1.0;
|
|
transform.linear()(1,1) = 0.0;
|
|
transform.linear()(1,2) = 0.0;
|
|
|
|
transform.linear()(2,0) = 0.0;
|
|
transform.linear()(2,1) = 0.0;
|
|
transform.linear()(2,2) = 1.0;
|
|
|
|
// Apply affine transformation.
|
|
const GridMap transformedMap = map.getTransformedMap(transform, heightLayerName, map.getFrameId(), 0.25);
|
|
|
|
// Check if map has been rotated by 90° about z
|
|
EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6);
|
|
EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6);
|
|
EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size());
|
|
EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0,0), transformedMap.get(heightLayerName)(19,0));
|
|
}
|
|
|
|
TEST(GridMap, ClipToMap)
|
|
{
|
|
GridMap map({"layer_a", "layer_b"});
|
|
map.setGeometry(Length(1.0, 1.0), 0.1, Position(0.5, 0.5));
|
|
map["layer_a"].setConstant(1.0);
|
|
map["layer_b"].setConstant(2.0);
|
|
|
|
const Position positionInMap = Position(0.4, 0.3); // position located inside the map
|
|
const Position positionOutMap = Position(1.0, 2.0); // position located outside the map
|
|
|
|
const Position clippedPositionInMap = map.getClosestPositionInMap(positionInMap);
|
|
const Position clippedPositionOutMap = map.getClosestPositionInMap(positionOutMap);
|
|
|
|
// Check if position-in-map remains unchanged.
|
|
EXPECT_NEAR(clippedPositionInMap.x(),positionInMap.x(), 1e-6);
|
|
EXPECT_NEAR(clippedPositionInMap.y(), positionInMap.y(), 1e-6);
|
|
|
|
// Check if position-out-map is indeed outside the map.
|
|
EXPECT_TRUE(!map.isInside(positionOutMap));
|
|
|
|
// Check if position-out-map has been projected into the map.
|
|
EXPECT_TRUE(map.isInside(clippedPositionOutMap));
|
|
}
|
|
|
|
|
|
|
|
TEST(GridMap, ClipToMap2)
|
|
{
|
|
GridMap map({"types"});
|
|
map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
|
|
|
|
// Test 8 points outside of map.
|
|
/*
|
|
* A B C
|
|
* +---+
|
|
* | | X
|
|
* D| |E ^
|
|
* | | |
|
|
* +---+ Y<--+
|
|
* F G H
|
|
*
|
|
* Note: Position to index alignment is a half open interval.
|
|
* An example position of 0.5 is assigned to the upper index.
|
|
* The interval in the current example is:
|
|
* Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...)
|
|
* Index: 8 9 10 11
|
|
*/
|
|
|
|
Index insideIndex;
|
|
Position outsidePosition;
|
|
|
|
// Point A
|
|
outsidePosition = Position(1.0, 1.0);
|
|
auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
bool isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
auto expectedPosition = Position(0.5, 0.5);
|
|
auto expectedIndex = Index(0, 0);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point B
|
|
outsidePosition = Position(1.0, 0.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(0.5, 0.0);
|
|
expectedIndex = Index(0, 10);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point C
|
|
outsidePosition = Position(1.0, -1.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(0.5, -0.5);
|
|
expectedIndex = Index(0, 19);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point D
|
|
outsidePosition = Position(0.0, 1.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(0.0, 0.5);
|
|
expectedIndex = Index(10, 0);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point E
|
|
outsidePosition = Position(0.0, -1.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(0.0, -0.5);
|
|
expectedIndex = Index(10, 19);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point F
|
|
outsidePosition = Position(-1.0, 1.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(-0.5, 0.5);
|
|
expectedIndex = Index(19, 0);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point G
|
|
outsidePosition = Position(-1.0, 0.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(-0.5, 0.0);
|
|
expectedIndex = Index(19, 10);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
|
|
// Point H
|
|
outsidePosition = Position(-1.0, -1.0);
|
|
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
|
|
isInside = map.getIndex(closestInsidePosition, insideIndex);
|
|
|
|
expectedPosition = Position(-0.5, -0.5);
|
|
expectedIndex = Index(19, 19);
|
|
|
|
// Check position.
|
|
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
|
|
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
|
|
|
|
// Check index.
|
|
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
|
|
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
|
|
|
|
// Check if index is inside.
|
|
EXPECT_TRUE(isInside) << "position is: " << std::endl
|
|
<< closestInsidePosition << std::endl
|
|
<< " index is: " << std::endl
|
|
<< insideIndex << std::endl;
|
|
}
|
|
|
|
TEST(AddDataFrom, ExtendMapAligned)
|
|
{
|
|
GridMap map1;
|
|
GridMap map2;
|
|
map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
|
|
map1.add("zero", 0.0);
|
|
map1.add("one", 1.0);
|
|
map1.setBasicLayers(map1.getLayers());
|
|
|
|
map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
|
|
map2.add("one", 1.1);
|
|
map2.add("two", 2.0);
|
|
map2.setBasicLayers(map1.getLayers());
|
|
|
|
map1.addDataFrom(map2, true, true, true);
|
|
|
|
EXPECT_TRUE(map1.exists("two"));
|
|
EXPECT_TRUE(map1.isInside(Position(3.0, 3.0)));
|
|
EXPECT_DOUBLE_EQ(6.0, map1.getLength().x());
|
|
EXPECT_DOUBLE_EQ(6.0, map1.getLength().y());
|
|
EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x());
|
|
EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y());
|
|
EXPECT_NEAR(1.1, map1.atPosition("one", Position(2, 2)), 1e-4);
|
|
EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(-2, -2)));
|
|
EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
|
|
}
|
|
|
|
TEST(AddDataFrom, ExtendMapNotAligned)
|
|
{
|
|
GridMap map1;
|
|
GridMap map2;
|
|
map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0)); // bufferSize(6, 6)
|
|
map1.add("nan");
|
|
map1.add("one", 1.0);
|
|
map1.add("zero", 0.0);
|
|
map1.setBasicLayers(map1.getLayers());
|
|
|
|
map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2));
|
|
map2.add("nan", 1.0);
|
|
map2.add("one", 1.1);
|
|
map2.add("two", 2.0);
|
|
map2.setBasicLayers(map1.getLayers());
|
|
|
|
std::vector<std::string> stringVector;
|
|
stringVector.emplace_back("nan");
|
|
map1.addDataFrom(map2, true, false, false, stringVector);
|
|
Index index;
|
|
map1.getIndex(Position(-2, -2), index);
|
|
|
|
EXPECT_FALSE(map1.exists("two"));
|
|
EXPECT_TRUE(map1.isInside(Position(4.0, 4.0)));
|
|
EXPECT_DOUBLE_EQ(8.0, map1.getLength().x());
|
|
EXPECT_DOUBLE_EQ(8.0, map1.getLength().y());
|
|
EXPECT_DOUBLE_EQ(1.0, map1.getPosition().x());
|
|
EXPECT_DOUBLE_EQ(1.0, map1.getPosition().y());
|
|
EXPECT_FALSE(map1.isValid(index, "nan"));
|
|
EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(0.0, 0.0)));
|
|
EXPECT_DOUBLE_EQ(1.0, map1.atPosition("nan", Position(3.0, 3.0)));
|
|
}
|
|
|
|
TEST(AddDataFrom, CopyData)
|
|
{
|
|
GridMap map1;
|
|
GridMap map2;
|
|
map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
|
|
map1.add("zero", 0.0);
|
|
map1.add("one");
|
|
map1.setBasicLayers(map1.getLayers());
|
|
|
|
map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
|
|
map2.add("one", 1.0);
|
|
map2.add("two", 2.0);
|
|
map2.setBasicLayers(map1.getLayers());
|
|
|
|
map1.addDataFrom(map2, false, false, true);
|
|
Index index;
|
|
map1.getIndex(Position(-2, -2), index);
|
|
|
|
EXPECT_TRUE(map1.exists("two"));
|
|
EXPECT_FALSE(map1.isInside(Position(3.0, 3.0)));
|
|
EXPECT_DOUBLE_EQ(5.0, map1.getLength().x());
|
|
EXPECT_DOUBLE_EQ(5.0, map1.getLength().y());
|
|
EXPECT_DOUBLE_EQ(0.0, map1.getPosition().x());
|
|
EXPECT_DOUBLE_EQ(0.0, map1.getPosition().y());
|
|
EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(2, 2)));
|
|
EXPECT_FALSE(map1.isValid(index, "one"));
|
|
EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
|
|
}
|
|
|
|
TEST(ValueAtPosition, NearestNeighbor)
|
|
{
|
|
GridMap map( { "types" });
|
|
map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
|
|
|
|
map.at("types", Index(0,0)) = 0.5;
|
|
map.at("types", Index(0,1)) = 3.8;
|
|
map.at("types", Index(0,2)) = 2.0;
|
|
map.at("types", Index(1,0)) = 2.1;
|
|
map.at("types", Index(1,1)) = 1.0;
|
|
map.at("types", Index(1,2)) = 2.0;
|
|
map.at("types", Index(2,0)) = 1.0;
|
|
map.at("types", Index(2,1)) = 2.0;
|
|
map.at("types", Index(2,2)) = 2.0;
|
|
|
|
double value = map.atPosition("types", Position(1.35,-0.4));
|
|
EXPECT_DOUBLE_EQ((float)3.8, value);
|
|
|
|
value = map.atPosition("types", Position(-0.3,0.0));
|
|
EXPECT_DOUBLE_EQ(1.0, value);
|
|
}
|
|
|
|
TEST(ValueAtPosition, LinearInterpolated)
|
|
{
|
|
GridMap map( { "types" });
|
|
map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
|
|
|
|
map.at("types", Index(0,0)) = 0.5;
|
|
map.at("types", Index(0,1)) = 3.8;
|
|
map.at("types", Index(0,2)) = 2.0;
|
|
map.at("types", Index(1,0)) = 2.1;
|
|
map.at("types", Index(1,1)) = 1.0;
|
|
map.at("types", Index(1,2)) = 2.0;
|
|
map.at("types", Index(2,0)) = 1.0;
|
|
map.at("types", Index(2,1)) = 2.0;
|
|
map.at("types", Index(2,2)) = 2.0;
|
|
|
|
// Close to the border -> reverting to INTER_NEAREST.
|
|
double value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
|
|
EXPECT_DOUBLE_EQ(2.0, value);
|
|
// In between 1.0 and 2.0 field.
|
|
value = map.atPosition("types", Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR);
|
|
EXPECT_DOUBLE_EQ(1.5, value);
|
|
// Calculated "by Hand".
|
|
value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
|
|
EXPECT_NEAR(2.1963200, value, 0.0000001);
|
|
}
|
|
|
|
} // namespace robot_grid_map
|