DuongTD/grid_map_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2026-01-14 15:04:54 +07:00
commit 99662d6d47
79 changed files with 9930 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
test/CubicConvolutionInterpolationTest.cpp Normal file
View File

@@ -0,0 +1,130 @@
/*
* CubicConvolutionInterpolationTest.cpp
*
* Created on: Mar 3, 2020
* Author: Edo Jelavic
* Institute: ETH Zurich, Robotic Systems Lab
*/
#include "test_helpers.hpp"
#include "grid_map_core/GridMap.hpp"
// gtest
#include <gtest/gtest.h>
namespace gm = grid_map;
namespace gmt = grid_map_test;
TEST(CubicConvolutionInterpolation, FlatWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(2.0, 2.0), 0.1, gm::Position(0.0, 0.0));
auto trueValues = gmt::createFlatWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 100);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, FlatWorld failed with seed: " << seed
<< std::endl;
}
}
TEST(CubicConvolutionInterpolation, RationalFunctionWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
auto trueValues = gmt::createRationalFunctionWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, RationalFunctionWorld failed with seed: "
<< seed << std::endl;
}
}
TEST(CubicConvolutionInterpolation, SaddleWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
auto trueValues = gmt::createSaddleWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, SaddleWorld failed with seed: " << seed
<< std::endl;
}
}
TEST(CubicConvolutionInterpolation, SecondOrderPolyWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
auto trueValues = gmt::createSecondOrderPolyWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, SecondOrderPolyWorld failed with seed: "
<< seed << std::endl;
}
}
TEST(CubicConvolutionInterpolation, SineWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
auto trueValues = gmt::createSineWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, SineWorld failed with seed: " << seed
<< std::endl;
}
}
TEST(CubicConvolutionInterpolation, TanhWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(2.5, 2.5), 0.02, gm::Position(0.0, 0.0));
auto trueValues = gmt::createTanhWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, TanhWorld failed with seed: " << seed
<< std::endl;
}
}
TEST(CubicConvolutionInterpolation, GaussianWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.02, gm::Position(0.0, 0.0));
auto trueValues = gmt::createGaussianWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicConvolutionInterpolation, GaussianWorld failed with seed: " << seed
<< std::endl;
}
}

126
test/CubicInterpolationTest.cpp Normal file
View File

@@ -0,0 +1,126 @@
/*
* CubicInterpolationTest.cpp
*
* Created on: Mar 12, 2020
* Author: Edo Jelavic
* Institute: ETH Zurich, Robotic Systems Lab
*/
#include "test_helpers.hpp"
#include "grid_map_core/GridMap.hpp"
// gtest
#include <gtest/gtest.h>
namespace gm = grid_map;
namespace gmt = grid_map_test;
TEST(CubicInterpolation, FlatWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(2.0, 2.0), 0.1, gm::Position(0.0, 0.0));
auto trueValues = gmt::createFlatWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 100);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, FlatWorld failed with seed: " << seed << std::endl;
}
}
TEST(CubicInterpolation, RationalFunctionWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
auto trueValues = gmt::createRationalFunctionWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, RationalFunctionWorld failed with seed: " << seed
<< std::endl;
}
}
TEST(CubicInterpolation, SaddleWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
auto trueValues = gmt::createSaddleWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, SaddleWorld failed with seed: " << seed << std::endl;
}
}
TEST(CubicInterpolation, SecondOrderPolyWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0));
auto trueValues = gmt::createSecondOrderPolyWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, SecondOrderPolyWorld failed with seed: " << seed
<< std::endl;
}
}
TEST(CubicInterpolation, SineWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0));
auto trueValues = gmt::createSineWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, SineWorld failed with seed: " << seed << std::endl;
}
}
TEST(CubicInterpolation, TanhWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(2.5, 2.5), 0.02, gm::Position(0.0, 0.0));
auto trueValues = gmt::createTanhWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, TanhWorld failed with seed: " << seed << std::endl;
}
}
TEST(CubicInterpolation, GaussianWorld)
{
const int seed = rand();
gmt::rndGenerator.seed(seed);
auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.02, gm::Position(0.0, 0.0));
auto trueValues = gmt::createGaussianWorld(&map);
const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000);
verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC);
if (::testing::Test::HasFailure()) {
std::cout << "\n Test CubicInterpolation, GaussianWorld failed with seed: " << seed
<< std::endl;
}
}

115
test/EigenPluginsTest.cpp Normal file
View File

@@ -0,0 +1,115 @@
/*
* EigenMatrixBaseAddonsTest.cpp
*
* Created on: Sep 23, 2015
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/grid_map_core.hpp"
// gtest
#include <gtest/gtest.h>
// Eigen
#include <Eigen/Core>
using Eigen::Matrix;
TEST(EigenMatrixBaseAddons, numberOfFinites)
{
Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
matrix(0, 0) = NAN;
matrix(1, 0) = NAN;
EXPECT_EQ(7, matrix.numberOfFinites());
Matrix<double, 13, 10> matrix2;
matrix2.setOnes();
EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites());
Matrix<double, 13, 10> matrix3;
matrix3.setConstant(NAN);
matrix3.col(3).setConstant(0.0);
EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites());
}
TEST(EigenMatrixBaseAddons, sumOfFinites)
{
Matrix<double, 7, 18> matrix;
matrix.setRandom();
EXPECT_NEAR(matrix.sum(), matrix.sumOfFinites(), 1e-10);
double finiteSum = matrix.sum() - matrix(0, 0) - matrix(1, 2) - matrix(3, 6) - matrix(6, 12);
matrix(0, 0) = NAN;
matrix(1, 2) = NAN;
matrix(3, 6) = NAN;
matrix(6, 12) = NAN;
EXPECT_NEAR(finiteSum, matrix.sumOfFinites(), 1e-10);
matrix.setConstant(NAN);
EXPECT_TRUE(std::isnan(matrix.sumOfFinites()));
matrix(5, 7) = 1.0;
EXPECT_NEAR(1.0, matrix.sumOfFinites(), 1e-10);
}
TEST(EigenMatrixBaseAddons, meanOfFinites)
{
Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
matrix(0, 0) = NAN;
matrix(1, 1) = NAN;
EXPECT_DOUBLE_EQ(1.0, matrix.meanOfFinites());
Matrix<double, 13, 10> matrix2;
matrix2.setRandom();
EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10);
}
TEST(EigenMatrixBaseAddons, minCoeffOfFinites)
{
Matrix<double, 7, 18> matrix;
matrix.setRandom();
double min = matrix.minCoeff();
EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
int i;
int j;
matrix.maxCoeff(&i, &j);
matrix(i, j) = NAN;
EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
matrix.setConstant(NAN);
EXPECT_TRUE(std::isnan(matrix.minCoeffOfFinites()));
matrix(i, j) = -1.0;
EXPECT_NEAR(-1.0, matrix.minCoeffOfFinites(), 1e-10);
}
TEST(EigenMatrixBaseAddons, maxCoeffOfFinites)
{
Matrix<double, 7, 18> matrix;
matrix.setRandom();
double max = matrix.maxCoeff();
EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
int i;
int j;
matrix.minCoeff(&i, &j);
matrix(i, j) = NAN;
EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
matrix.setConstant(NAN);
EXPECT_TRUE(std::isnan(matrix.maxCoeffOfFinites()));
matrix(i, j) = -1.0;
EXPECT_NEAR(-1.0, matrix.maxCoeffOfFinites(), 1e-10);
}
TEST(EigenMatrixBaseAddons, clamp)
{
Eigen::VectorXf vector(Eigen::VectorXf::LinSpaced(9, 1.0, 9.0));
Eigen::Matrix3f matrix;
matrix << vector.segment(0, 3), vector.segment(3, 3), vector.segment(6, 3);
matrix(1, 1) = NAN;
matrix = matrix.unaryExpr(grid_map::Clamp<float>(2.1, 7.0));
EXPECT_NEAR(2.1, matrix(0, 0), 1e-7);
EXPECT_NEAR(2.1, matrix(1, 0), 1e-7);
EXPECT_NEAR(3.0, matrix(2, 0), 1e-7);
EXPECT_TRUE(std::isnan(matrix(1, 1)));
EXPECT_NEAR(7.0, matrix(2, 2), 1e-7);
}

55
test/EllipseIteratorTest.cpp Normal file
View File

@@ -0,0 +1,55 @@
/*
* EllipseIteratorTest.cpp
*
* Created on: Dec 2, 2015
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/iterators/EllipseIterator.hpp"
#include "grid_map_core/GridMap.hpp"
// gtest
#include <gtest/gtest.h>
// Vector
#include <vector>
using grid_map::GridMap;
using grid_map::Length;
using grid_map::Position;
using grid_map::EllipseIterator;
TEST(EllipseIterator, OneCellWideEllipse)
{
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
EllipseIterator iterator(map, Position(0.0, 0.0), Length(8.0, 1.0));
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(1, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(2, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
++iterator;
++iterator;
++iterator;
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}

59
test/GridMapIteratorTest.cpp Normal file
View File

@@ -0,0 +1,59 @@
/*
* GridMapDataIterator.cpp
*
* Created on: Feb 16, 2016
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/iterators/GridMapIterator.hpp"
#include "grid_map_core/GridMap.hpp"
// gtest
#include <gtest/gtest.h>
// Vector
#include <vector>
using grid_map::GridMap;
using grid_map::Length;
using grid_map::Position;
using grid_map::GridMapIterator;
TEST(GridMapIterator, Simple)
{
GridMap map;
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.add("layer", 0.0);
GridMapIterator iterator(map);
unsigned int i = 0;
for (; !iterator.isPastEnd(); ++iterator, ++i) {
map.at("layer", *iterator) = 1.0;
EXPECT_FALSE(iterator.isPastEnd());
}
EXPECT_EQ(40, i);
EXPECT_TRUE(iterator.isPastEnd());
EXPECT_TRUE((map["layer"].array() == 1.0f).all());
}
TEST(GridMapIterator, LinearIndex)
{
GridMap map;
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.add("layer", 0.0);
GridMapIterator iterator(map);
auto& data = map["layer"];
unsigned int i = 0;
for (; !iterator.isPastEnd(); ++iterator, ++i) {
data(static_cast<long>(iterator.getLinearIndex())) = 1.0;
EXPECT_EQ(i, iterator.getLinearIndex());
EXPECT_FALSE(iterator.isPastEnd());
}
EXPECT_EQ(40, i);
EXPECT_TRUE(iterator.isPastEnd());
EXPECT_TRUE((map["layer"].array() == 1.0f).all());
}

1039
test/GridMapMathTest.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

496
test/GridMapTest.cpp Normal file
View File

@@ -0,0 +1,496 @@
/*
* 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 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 grid_map

178
test/LineIteratorTest.cpp Normal file
View File

@@ -0,0 +1,178 @@
/*
* LineIteratorTest.cpp
*
* Created on: Sep 14, 2016
* Author: Dominic Jud
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/iterators/LineIterator.hpp"
#include "grid_map_core/GridMap.hpp"
// gtest
#include <gtest/gtest.h>
namespace grid_map {
TEST(LineIterator, StartOutsideMap) {
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
EXPECT_NO_THROW(LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0)));
LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0));
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(2, (*iterator)(0));
EXPECT_EQ(0, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(LineIterator, EndOutsideMap)
{
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0)));
LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0));
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(2, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(LineIterator, StartAndEndOutsideMap)
{
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
EXPECT_NO_THROW(LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0)));
LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0));
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(5, (*iterator)(0));
EXPECT_EQ(4, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(3, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
++iterator;
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(LineIterator, StartAndEndOutsideMapWithoutIntersectingMap)
{
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
EXPECT_THROW(LineIterator iterator(map, Position(-8.0, 8.0), Position(8.0, 8.0)), std::invalid_argument);
}
TEST(LineIterator, MovedMap)
{
GridMap map( { "types" });
map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0));
map.move(Position(2.0, 2.0));
EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0)));
LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0));
Position point;
EXPECT_FALSE(iterator.isPastEnd());
map.getPosition(*iterator, point);
EXPECT_EQ(0, point.x());
EXPECT_EQ(0, point.y());
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
map.getPosition(*iterator, point);
EXPECT_EQ(1, point.x());
EXPECT_EQ(1, point.y());
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
map.getPosition(*iterator, point);
EXPECT_EQ(2, point.x());
EXPECT_EQ(2, point.y());
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(LineIterator, StartAndEndOutsideMovedMap)
{
GridMap map( { "types" });
map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0));
map.move(Position(2.0, 2.0));
EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0)));
LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0));
Position point;
EXPECT_FALSE(iterator.isPastEnd());
map.getPosition(*iterator, point);
EXPECT_EQ(0, point.x());
EXPECT_EQ(0, point.y());
++iterator;
map.getPosition(*iterator, point);
EXPECT_EQ(1, point.x());
EXPECT_EQ(1, point.y());
//
++iterator;
map.getPosition(*iterator, point);
EXPECT_EQ(2, point.x());
EXPECT_EQ(2, point.y());
//
++iterator;
map.getPosition(*iterator, point);
EXPECT_EQ(3, point.x());
EXPECT_EQ(3, point.y());
++iterator;
map.getPosition(*iterator, point);
EXPECT_EQ(4, point.x());
EXPECT_EQ(4, point.y());
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
} // namespace grid_map

196
test/PolygonIteratorTest.cpp Normal file
View File

@@ -0,0 +1,196 @@
/*
* PolygonIteratorTest.cpp
*
* Created on: Sep 19, 2014
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/iterators/PolygonIterator.hpp"
#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/Polygon.hpp"
// gtest
#include <gtest/gtest.h>
// Vector
#include <vector>
using grid_map::GridMap;
using grid_map::Length;
using grid_map::Polygon;
using grid_map::PolygonIterator;
using grid_map::Position;
TEST(PolygonIterator, FullCover) {
std::vector<std::string> types;
types.emplace_back("type");
GridMap map(types);
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
Polygon polygon;
polygon.addVertex(Position(-100.0, 100.0));
polygon.addVertex(Position(100.0, 100.0));
polygon.addVertex(Position(100.0, -100.0));
polygon.addVertex(Position(-100.0, -100.0));
PolygonIterator iterator(map, polygon);
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(0, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
for (int i = 0; i < 37; ++i) {
++iterator;
}
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(4, (*iterator)(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(PolygonIterator, Outside)
{
GridMap map({"types"});
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
Polygon polygon;
polygon.addVertex(Position(99.0, 101.0));
polygon.addVertex(Position(101.0, 101.0));
polygon.addVertex(Position(101.0, 99.0));
polygon.addVertex(Position(99.0, 99.0));
PolygonIterator iterator(map, polygon);
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(PolygonIterator, Square)
{
GridMap map({"types"});
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
Polygon polygon;
polygon.addVertex(Position(-1.0, 1.5));
polygon.addVertex(Position(1.0, 1.5));
polygon.addVertex(Position(1.0, -1.5));
polygon.addVertex(Position(-1.0, -1.5));
PolygonIterator iterator(map, polygon);
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(3, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(3, (*iterator)(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(PolygonIterator, TopLeftTriangle)
{
GridMap map({"types"});
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
Polygon polygon;
polygon.addVertex(Position(-40.1, 20.6));
polygon.addVertex(Position(40.1, 20.4));
polygon.addVertex(Position(-40.1, -20.6));
PolygonIterator iterator(map, polygon);
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(0, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(1, (*iterator)(0));
EXPECT_EQ(0, (*iterator)(1));
// TODO Extend.
}
TEST(PolygonIterator, MoveMap)
{
GridMap map({"layer"});
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.move(Position(2.0, 0.0));
Polygon polygon;
polygon.addVertex(Position(6.1, 1.6));
polygon.addVertex(Position(0.9, 1.6));
polygon.addVertex(Position(0.9, -1.6));
polygon.addVertex(Position(6.1, -1.6));
PolygonIterator iterator(map, polygon);
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(6, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(6, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
for (int i = 0; i < 4; ++i) {
++iterator;
}
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(3, (*iterator)(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
for (int i = 0; i < 8; ++i) {
++iterator;
}
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(2, (*iterator)(0));
EXPECT_EQ(3, (*iterator)(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}

268
test/PolygonTest.cpp Normal file
View File

@@ -0,0 +1,268 @@
/*
* PolygonTest.cpp
*
* Created on: Mar 24, 2015
* Author: Martin Wermelinger, Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/Polygon.hpp"
// gtest
#include <gtest/gtest.h>
// Eigen
#include <Eigen/Core>
using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;
using grid_map::Length;
using grid_map::Polygon;
using grid_map::Position;
TEST(Polygon, getCentroidTriangle)
{
Polygon triangle;
triangle.addVertex(Vector2d(0.0, 0.0));
triangle.addVertex(Vector2d(1.0, 0.0));
triangle.addVertex(Vector2d(0.5, 1.0));
Position expectedCentroid;
expectedCentroid.x() = 1.0 / 3.0 * (1.0 + 0.5);
expectedCentroid.y() = 1.0 / 3.0;
Position centroid = triangle.getCentroid();
EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
}
TEST(Polygon, getCentroidRectangle)
{
Polygon rectangle;
rectangle.addVertex(Vector2d(-2.0, -1.0));
rectangle.addVertex(Vector2d(-2.0, 2.0));
rectangle.addVertex(Vector2d(1.0, 2.0));
rectangle.addVertex(Vector2d(1.0, -1.0));
Position expectedCentroid(-0.5, 0.5);
Position centroid = rectangle.getCentroid();
EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
}
TEST(Polygon, getBoundingBox)
{
Polygon triangle;
triangle.addVertex(Vector2d(0.0, 0.0));
triangle.addVertex(Vector2d(0.5, -1.2));
triangle.addVertex(Vector2d(1.0, 0.0));
Position expectedCenter(0.5, -0.6);
Length expectedLength(1.0, 1.2);
Position center;
Length length;
triangle.getBoundingBox(center, length);
EXPECT_DOUBLE_EQ(expectedCenter.x(), center.x());
EXPECT_DOUBLE_EQ(expectedCenter.y(), center.y());
EXPECT_DOUBLE_EQ(expectedLength.x(), length.x());
EXPECT_DOUBLE_EQ(expectedLength.y(), length.y());
}
TEST(Polygon, convexHullPoints)
{
// Test that points which already create a convex shape (square) can be used to create a convex polygon.
std::vector<Position> points1;
points1.emplace_back(0.0, 0.0);
points1.emplace_back(1.0, 0.0);
points1.emplace_back(1.0, 1.0);
points1.emplace_back(0.0, 1.0);
Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1);
EXPECT_EQ(4, polygon1.nVertices());
EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5)));
EXPECT_FALSE(polygon1.isInside(Vector2d(-0.01, 0.5)));
// Test that a random set of points can be used to create a convex polygon.
std::vector<Position> points2;
points2.emplace_back(0.0, 0.0);
points2.emplace_back(1.0, 0.0);
points2.emplace_back(2.0, 1.0);
points2.emplace_back(1.0, 2.0);
points2.emplace_back(-1.0, 2.0);
points2.emplace_back(-1.0, -2.0);
points2.emplace_back(0.0, 1.0);
points2.emplace_back(1.0, 1.0);
Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2);
EXPECT_EQ(4, polygon2.nVertices());
EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5)));
EXPECT_TRUE(polygon2.isInside(Vector2d(0.0, 1.0)));
EXPECT_TRUE(polygon2.isInside(Vector2d(-0.5, -0.5)));
EXPECT_FALSE(polygon2.isInside(Vector2d(2.0, 0.0)));
EXPECT_FALSE(polygon2.isInside(Vector2d(-0.5, -2)));
EXPECT_FALSE(polygon2.isInside(Vector2d(1.75, 1.75)));
}
TEST(Polygon, convexHullPolygon)
{
Polygon polygon1;
polygon1.addVertex(Vector2d(0.0, 0.0));
polygon1.addVertex(Vector2d(1.0, 1.0));
polygon1.addVertex(Vector2d(0.0, 1.0));
polygon1.addVertex(Vector2d(1.0, 0.0));
Polygon polygon2;
polygon2.addVertex(Vector2d(0.5, 0.5));
polygon2.addVertex(Vector2d(0.5, 1.5));
polygon2.addVertex(Vector2d(1.5, 0.5));
polygon2.addVertex(Vector2d(1.5, 1.5));
Polygon hull = Polygon::convexHull(polygon1, polygon2);
EXPECT_EQ(6, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.5)));
EXPECT_FALSE(hull.isInside(Vector2d(0.01, 1.49)));
}
TEST(Polygon, convexHullCircles)
{
Position center1(0.0, 0.0);
Position center2(1.0, 0.0);
double radius = 0.5;
const int nVertices = 15;
Polygon hull = Polygon::convexHullOfTwoCircles(center1, center2, radius);
EXPECT_EQ(20, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));
hull = Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices);
EXPECT_EQ(nVertices + 1, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));
hull = Polygon::convexHullOfTwoCircles(center1, center1, radius);
EXPECT_EQ(20, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
hull = Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices);
EXPECT_EQ(nVertices, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
}
TEST(Polygon, convexHullCircle)
{
Position center(0.0, 0.0);
double radius = 0.5;
const int nVertices = 15;
Polygon hull = Polygon::fromCircle(center, radius);
EXPECT_EQ(20, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
hull = Polygon::fromCircle(center, radius, nVertices);
EXPECT_EQ(nVertices, hull.nVertices());
EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
}
TEST(convertToInequalityConstraints, triangle1)
{
Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.1, -1.1)});
MatrixXd A;
VectorXd b;
ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
EXPECT_NEAR(-1.3636, A(0, 0), 1e-4);
EXPECT_NEAR( 1.3636, A(0, 1), 1e-4);
EXPECT_NEAR(-1.5000, A(1, 0), 1e-4);
EXPECT_NEAR(-1.5000, A(1, 1), 1e-4);
EXPECT_NEAR( 2.8636, A(2, 0), 1e-4);
EXPECT_NEAR( 0.1364, A(2, 1), 1e-4);
EXPECT_NEAR( 0.0000, b(0), 1e-4);
EXPECT_NEAR( 0.0000, b(1), 1e-4);
EXPECT_NEAR( 3.0000, b(2), 1e-4);
}
TEST(convertToInequalityConstraints, triangle2)
{
Polygon polygon({Position(-1.0, 0.5), Position(-1.0, -0.5), Position(1.0, -0.5)});
MatrixXd A;
VectorXd b;
ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
EXPECT_NEAR(-1.5000, A(0, 0), 1e-4);
EXPECT_NEAR( 0.0000, A(0, 1), 1e-4);
EXPECT_NEAR( 0.0000, A(1, 0), 1e-4);
EXPECT_NEAR(-3.0000, A(1, 1), 1e-4);
EXPECT_NEAR( 1.5000, A(2, 0), 1e-4);
EXPECT_NEAR( 3.0000, A(2, 1), 1e-4);
EXPECT_NEAR( 1.5000, b(0), 1e-4);
EXPECT_NEAR( 1.5000, b(1), 1e-4);
EXPECT_NEAR( 0.0000, b(2), 1e-4);
}
TEST(offsetInward, triangle)
{
Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
polygon.offsetInward(0.1);
EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4);
EXPECT_NEAR(0.758579, polygon.getVertex(0)(1), 1e-4);
EXPECT_NEAR(0.141421, polygon.getVertex(1)(0), 1e-4);
EXPECT_NEAR(0.0, polygon.getVertex(1)(1), 1e-4);
EXPECT_NEAR(0.9, polygon.getVertex(2)(0), 1e-4);
EXPECT_NEAR(-0.758579, polygon.getVertex(2)(1), 1e-4);
}
TEST(triangulation, triangle)
{
Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
std::vector<Polygon> polygons;
polygons = polygon.triangulate();
ASSERT_EQ(1, polygons.size());
EXPECT_EQ(polygon.getVertex(0).x(), polygons[0].getVertex(0).x());
EXPECT_EQ(polygon.getVertex(0).y(), polygons[0].getVertex(0).y());
EXPECT_EQ(polygon.getVertex(1).x(), polygons[0].getVertex(1).x());
EXPECT_EQ(polygon.getVertex(1).y(), polygons[0].getVertex(1).y());
EXPECT_EQ(polygon.getVertex(2).x(), polygons[0].getVertex(2).x());
EXPECT_EQ(polygon.getVertex(2).y(), polygons[0].getVertex(2).y());
}
TEST(triangulation, rectangle)
{
Polygon rectangle;
rectangle.addVertex(Vector2d(-2.0, -1.0));
rectangle.addVertex(Vector2d(-2.0, 2.0));
rectangle.addVertex(Vector2d(1.0, 2.0));
rectangle.addVertex(Vector2d(1.0, -1.0));
std::vector<Polygon> polygons;
polygons = rectangle.triangulate();
ASSERT_EQ(2, polygons.size());
// TODO Extend.
}

153
test/SlidingWindowIteratorTest.cpp Normal file
View File

@@ -0,0 +1,153 @@
/*
* SlidingWindowIteratorTest.cpp
*
* Created on: Aug 16, 2017
* Author: Péter Fankhauser
* Institute: ETH Zurich
*/
#include "grid_map_core/iterators/SlidingWindowIterator.hpp"
#include "grid_map_core/GridMap.hpp"
#include <gtest/gtest.h>
#include <vector>
using grid_map::GridMap;
using grid_map::Index;
using grid_map::Length;
using grid_map::Position;
using grid_map::SlidingWindowIterator;
TEST(SlidingWindowIterator, WindowSize3Cutoff)
{
GridMap map;
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.add("layer");
map["layer"].setRandom();
SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 3);
EXPECT_EQ(iterator.getData().rows(), 2);
EXPECT_EQ(iterator.getData().cols(), 2);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 2, 2)));
++iterator;
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 2);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 2)));
++iterator;
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 2);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(1, 0, 3, 2)));
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
if ((*iterator == Index(3, 2)).all()) {
break;
}
}
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(2, 1, 3, 3)));
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
if ((*iterator == Index(7, 4)).all()) {
break;
}
}
EXPECT_EQ(iterator.getData().rows(), 2);
EXPECT_EQ(iterator.getData().cols(), 2);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(6, 3, 2, 2)));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(SlidingWindowIterator, WindowSize5)
{
GridMap map;
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.add("layer");
map["layer"].setRandom();
SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 5);
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3)));
++iterator;
EXPECT_EQ(iterator.getData().rows(), 4);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 4, 3)));
++iterator;
EXPECT_EQ(iterator.getData().rows(), 5);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 5, 3)));
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
if ((*iterator == Index(3, 2)).all()) {
break;
}
}
EXPECT_EQ(iterator.getData().rows(), 5);
EXPECT_EQ(iterator.getData().cols(), 5);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(1, 0, 5, 5)));
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
if ((*iterator == Index(7, 4)).all()) {
break;
}
}
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(5, 2, 3, 3)));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
TEST(SlidingWindowIterator, WindowSize3Inside)
{
GridMap map;
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.add("layer");
map["layer"].setRandom();
SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::INSIDE, 3);
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3)));
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
if ((*iterator == Index(3, 2)).all()) {
break;
}
}
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(2, 1, 3, 3)));
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
if ((*iterator == Index(6, 3)).all()) {
break;
}
}
EXPECT_EQ(iterator.getData().rows(), 3);
EXPECT_EQ(iterator.getData().cols(), 3);
EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(5, 2, 3, 3)));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}

36
test/SpiralIteratorTest.cpp Normal file
View File

@@ -0,0 +1,36 @@
/*
* SpiralIteratorTest.cpp
*
* Created on: Jul 26, 2017
* Author: Benjamin Scholz
* Institute: University of Hamburg, TAMS
*/
#include "grid_map_core/iterators/SpiralIterator.hpp"
#include "grid_map_core/GridMap.hpp"
// gtest
#include <gtest/gtest.h>
// Vector
#include <vector>
using grid_map::GridMap;
using grid_map::Length;
using grid_map::Position;
using grid_map::SpiralIterator;
TEST(SpiralIterator, CenterOutOfMap)
{
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
Position center(8.0, 0.0);
double radius = 5.0;
SpiralIterator iterator(map, center, radius);
Position iterator_position;
map.getPosition(*iterator, iterator_position);
EXPECT_TRUE(map.isInside(iterator_position));
}

267
test/SubmapIteratorTest.cpp Normal file
View File

@@ -0,0 +1,267 @@
/*
* SubmapIteratorTest.cpp
*
* Created on: Sep 15, 2014
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/SubmapIterator.hpp"
// Eigen
#include <Eigen/Core>
// gtest
#include <gtest/gtest.h>
// Vector
#include <vector>
using std::vector;
using std::string;
namespace grid_map{
TEST(SubmapIterator, Simple) {
Eigen::Array2i submapTopLeftIndex(3, 1);
Eigen::Array2i submapBufferSize(3, 2);
Eigen::Array2i index;
Eigen::Array2i submapIndex;
vector<string> types;
types.emplace_back("type");
GridMap map(types);
map.setGeometry(Eigen::Array2d(8.1, 5.1), 1.0, Eigen::Vector2d(0.0, 0.0)); // bufferSize(8, 5)
SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
EXPECT_EQ(0, iterator.getSubmapIndex()(0));
EXPECT_EQ(0, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(3, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
EXPECT_EQ(0, iterator.getSubmapIndex()(0));
EXPECT_EQ(1, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(0, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(4, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(1, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(5, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
EXPECT_EQ(2, iterator.getSubmapIndex()(0));
EXPECT_EQ(0, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(5, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
EXPECT_EQ(2, iterator.getSubmapIndex()(0));
EXPECT_EQ(1, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
EXPECT_EQ(5, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
EXPECT_EQ(2, iterator.getSubmapIndex()(0));
EXPECT_EQ(1, iterator.getSubmapIndex()(1));
}
TEST(SubmapIterator, CircularBuffer) {
Eigen::Array2i submapTopLeftIndex(6, 3);
Eigen::Array2i submapBufferSize(2, 4);
Eigen::Array2i index;
Eigen::Array2i submapIndex;
vector<string> types;
types.emplace_back("type");
GridMap map(types);
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.move(Position(-3.0, -2.0)); // bufferStartIndex(3, 2)
SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0));
EXPECT_EQ(submapTopLeftIndex(1), (*iterator)(1));
EXPECT_EQ(0, iterator.getSubmapIndex()(0));
EXPECT_EQ(0, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(6, (*iterator)(0));
EXPECT_EQ(4, (*iterator)(1));
EXPECT_EQ(0, iterator.getSubmapIndex()(0));
EXPECT_EQ(1, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(6, (*iterator)(0));
EXPECT_EQ(0, (*iterator)(1));
EXPECT_EQ(0, iterator.getSubmapIndex()(0));
EXPECT_EQ(2, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(6, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
EXPECT_EQ(0, iterator.getSubmapIndex()(0));
EXPECT_EQ(3, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(3, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(0, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(4, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(1, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(0, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(2, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(3, iterator.getSubmapIndex()(1));
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
EXPECT_EQ(1, iterator.getSubmapIndex()(0));
EXPECT_EQ(3, iterator.getSubmapIndex()(1));
}
/**
* The submap should contain the same elements as before even after moving the underlying map.
*
* +----------------------------+
* | |
* | |
* +----------------------------+ | |
* |0 0 0 0 0 0 0 0 0 0| | 0 0 0 0 0 0 0 0|
* | +----+ | | +----+ |
* Submap |1 1 |1 1| 1 1 1 1 1 1| | 1 1 |1 1| 1 1 1 1|
* +------> | | | | | | |
* |2 2 |2 2| 2 2 2 2 2 2| | 2 2 |2 2| 2 2 2 2|
* | +----+ | | +----+ |
* |3 3 3 3 3 3 3 3 3 3| Move | 3 3 3 3 3 3 3 3|
* | | | |
* |4 4 4 4 4 4 4 4 4 4| +---------> | 4 4 4 4 4 4 4 4|
* | | | |
* |5 5 5 5 5 5 5 5 5 5| | 5 5 5 5 5 5 5 5|
* | | | |
* |6 6 6 6 6 6 6 6 6 6| | 6 6 6 6 6 6 6 6|
* | | | |
* |7 7 7 7 7 7 7 7 7 7| | 7 7 7 7 7 7 7 7|
* | | +----------------------------+
* |8 8 8 8 8 8 8 8 8 8|
* | |
* |9 9 9 9 9 9 9 9 9 9|
* +----------------------------+
*/
TEST(SubmapIterator, InterleavedExecutionWithMove) {
grid_map::Index submapTopLeftIndex(3, 1);
grid_map::Size submapSize(2, 2);
GridMap map({"layer"});
map.setGeometry(Length(10, 10), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
auto& layer = map.get("layer");
// Initialize the layer as sketched.
for (long colIndex = 0; colIndex < layer.cols(); colIndex++) {
layer.col(colIndex).setConstant(static_cast<DataType>(colIndex));
}
std::cout << "(4,7) contains " << map.at("layer", {4, 7}) << std::endl;
// Instantiate the submap iterator as sketched.
SubmapIterator iterator(map, submapTopLeftIndex, submapSize);
// check that the submap iterator returns {1,1,2,2}
auto checkCorrectValues = [](std::array<double, 4> given) {
int countOnes = 0;
int countTwos = 0;
for (auto& value : given) {
if (std::abs(value - 1.0) < 1e-6) {
countOnes++;
} else if (std::abs(value - 2.0) < 1e-6) {
countTwos++;
} else {
FAIL() << "Submap iterator returned unexpected value.";
}
}
EXPECT_EQ(countOnes, 2);
EXPECT_EQ(countTwos, 2);
};
std::array<double, 4> returnedSequence{};
returnedSequence.fill(0);
for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
returnedSequence.at(submapIndex) = map.at("layer", *iterator);
++iterator;
}
checkCorrectValues(returnedSequence);
// Reset the iterator and now check that it still returns the same sequence when we move the map interleaved with iterating.
iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
returnedSequence.fill(0);
for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
if (submapIndex == 2) {
// Now move the map as depicted.
map.move(Position(2.0, 2.0));
}
returnedSequence.at(submapIndex) = map.at("layer", *iterator);
++iterator;
}
checkCorrectValues(returnedSequence);
// TODO (mwulf, mgaertner): This behavior is not yet implemented:
//
// // Reset the iterator and now check that the iterator throws? if the submap moved out of range.
// iterator = SubmapIterator(map, submapTopLeftIndex, submapSize);
//
// EXPECT_ANY_THROW(for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
// if (submapIndex == 2) {
// // Now move the map so that the submap gets out of range.
// map.move(Position(20.0, 20.0));
// }
// returnedSequence.at(submapIndex) = map.at("layer", *iterator);
// ++iterator;
// });
}
} // namespace grid_map

18
test/test_grid_map_core.cpp Normal file
View File

@@ -0,0 +1,18 @@
/*
* test_grid_map.cpp
*
* Created on: Feb 10, 2014
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
// gtest
#include <gtest/gtest.h>
// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
srand((int)time(nullptr));
return RUN_ALL_TESTS();
}

232
test/test_helpers.cpp Normal file
View File

@@ -0,0 +1,232 @@
/*
* test_helpers.cpp
*
* Created on: Mar 3, 2020
* Author: Edo Jelavic
* Institute: ETH Zurich, Robotic Systems Lab
*/
#include "test_helpers.hpp"
#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/GridMapIterator.hpp"
// gtest
#include <gtest/gtest.h>
namespace grid_map_test {
std::mt19937 rndGenerator;
AnalyticalFunctions createFlatWorld(grid_map::GridMap *map)
{
AnalyticalFunctions func;
func.f_ = [](double /*x*/, double /*y*/) {
return 0.0;
};
fillGridMap(map, func);
return func;
}
AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map)
{
AnalyticalFunctions func;
std::uniform_real_distribution<double> shift(-3.0, 3.0);
std::uniform_real_distribution<double> scale(1.0, 20.0);
const double x0 = shift(rndGenerator);
const double y0 = shift(rndGenerator);
const double s = scale(rndGenerator);
func.f_ = [x0, y0,s](double x, double y) {
return s / (1 + std::pow(x-x0, 2.0) + std::pow(y-y0, 2.0));
};
fillGridMap(map, func);
return func;
}
AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map)
{
AnalyticalFunctions func;
func.f_ = [](double x,double y) {
return (-x*x -y*y +2.0*x*y +x*x*y*y);
};
fillGridMap(map, func);
return func;
}
AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map)
{
AnalyticalFunctions func;
func.f_ = [](double x,double y) {
return (x*x-y*y);
};
fillGridMap(map, func);
return func;
}
AnalyticalFunctions createSineWorld(grid_map::GridMap *map)
{
AnalyticalFunctions func;
std::uniform_real_distribution<double> Uw(0.1, 4.0);
const double w1 = Uw(rndGenerator);
const double w2 = Uw(rndGenerator);
const double w3 = Uw(rndGenerator);
const double w4 = Uw(rndGenerator);
func.f_ = [w1,w2,w3,w4](double x,double y) {
return std::cos(w1*x) + std::sin(w2*y) + std::cos(w3*x) + std::sin(w4*y);
};
fillGridMap(map, func);
return func;
}
AnalyticalFunctions createTanhWorld(grid_map::GridMap *map)
{
AnalyticalFunctions func;
std::uniform_real_distribution<double> scaling(0.1, 2.0);
const double s = scaling(rndGenerator);
func.f_ = [s](double x,double /*y*/) {
const double expZ = std::exp(2 *s* x);
return (expZ - 1) / (expZ + 1);
};
fillGridMap(map, func);
return func;
}
AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
{
struct Gaussian
{
double x0, y0;
double varX, varY;
double s;
};
AnalyticalFunctions func;
std::uniform_real_distribution<double> var(0.1, 3.0);
std::uniform_real_distribution<double> mean(-4.0, 4.0);
std::uniform_real_distribution<double> scale(-3.0, 3.0);
constexpr int numGaussians = 3;
std::array<Gaussian, numGaussians> g;
for (int i = 0; i < numGaussians; ++i) {
g.at(i).x0 = mean(rndGenerator);
g.at(i).y0 = mean(rndGenerator);
g.at(i).varX = var(rndGenerator);
g.at(i).varY = var(rndGenerator);
g.at(i).s = scale(rndGenerator);
}
func.f_ = [g](double x,double y) {
double value = 0.0;
for (const auto & i : g) {
const double x0 = i.x0;
const double y0 = i.y0;
const double varX = i.varX;
const double varY = i.varY;
const double s = i.s;
value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY));
}
return value;
};
fillGridMap(map, func);
return func;
}
void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions)
{
using grid_map::DataType;
using grid_map::GridMapIterator;
using grid_map::Index;
using grid_map::Matrix;
using grid_map::Position;
Matrix& data = (*map)[testLayer];
for (GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) {
const Index index(*iterator);
Position pos;
map->getPosition(index, pos);
data(index(0), index(1)) = static_cast<DataType>(functions.f_(pos.x(), pos.y()));
}
}
grid_map::GridMap createMap(const grid_map::Length &length, double resolution,
const grid_map::Position &pos)
{
grid_map::GridMap map;
map.setGeometry(length, resolution, pos);
map.add(testLayer, 0.0);
map.setFrameId("map");
return map;
}
std::vector<Point2D> uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map,
unsigned int numPoints)
{
// stay away from the edges
// on the edges the cubic interp is invalid. Not enough points.
const double dimX = map.getLength().x() / 2.0 - 3.0 * map.getResolution();
const double dimY = map.getLength().y() / 2.0 - 3.0 * map.getResolution();
std::uniform_real_distribution<double> Ux(-dimX, dimX);
std::uniform_real_distribution<double> Uy(-dimY, dimY);
std::vector<Point2D> points(numPoints);
for (auto &point : points) {
point.x_ = Ux(rndGenerator);
point.y_ = Uy(rndGenerator);
}
return points;
}
void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues,
const std::vector<Point2D> &queryPoints,
grid_map::InterpolationMethods interpolationMethod){
for (const auto point : queryPoints) {
const grid_map::Position p(point.x_, point.y_);
const double trueValue = trueValues.f_(p.x(), p.y());
const double interpolatedValue = map.atPosition(
grid_map_test::testLayer, p, interpolationMethod);
EXPECT_NEAR(trueValue, interpolatedValue, grid_map_test::maxAbsErrorValue);
}
}
} /* namespace grid_map_test */

96
test/test_helpers.hpp Normal file
View File

@@ -0,0 +1,96 @@
/*
* test_helpers.hpp
*
* Created on: Mar 3, 2020
* Author: Edo Jelavic
* Institute: ETH Zurich, Robotic Systems Lab
*/
#pragma once
#include "grid_map_core/TypeDefs.hpp"
#include <functional>
#include <vector>
#include <random>
namespace grid_map {
class GridMap;
}
namespace grid_map_test {
/*
* Name of the layer that is used in all tests.
* It has no special meaning.
*/
static const std::string testLayer = "test";
/*
* Class that holds a function pointer to analytical
* functions used for evaluation. Analytical functions
* are in the form of f = f(x,y). One could also add
* derivatives to this class, e.g. for testing the
* accuracy of the normal estimation.
*/
struct AnalyticalFunctions
{
std::function<double(double, double)> f_;
};
struct Point2D
{
double x_ = 0.0;
double y_ = 0.0;
};
// Random generator engine.
extern std::mt19937 rndGenerator;
// Maximal tolerance when comparing doubles in tests.
const double maxAbsErrorValue = 1e-3;
grid_map::GridMap createMap(const grid_map::Length &length, double resolution,
const grid_map::Position &pos);
/*
* Collections of methods that modify the grid map.
* All these methods create an analytical function that
* describes the value of the layer "testLayer" as a function
* of coordinates. That can be any mathematical function. Inside the test,
* sinusoidal, polynomial functions and exponential functions are used.
* e.g. f(x,y) = sin(x) + cos(y), f(x,y) = exp(-x*x - y*y)
* Grid map is then filled by evaluating
* that analytical function over the entire length and width of the
* grid map. Grid map thus contains spatially sampled mathematical
* function.
* Each method returns a structure containing the analytical function.
*/
AnalyticalFunctions createFlatWorld(grid_map::GridMap *map);
AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map);
AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map);
AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map);
AnalyticalFunctions createSineWorld(grid_map::GridMap *map);
AnalyticalFunctions createTanhWorld(grid_map::GridMap *map);
AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map);
/*
* Iterates over the grid map and fill it with values.
* values are calculated by evaluating analytical function.
*/
void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions);
/*
* Create numPoints uniformly distributed random points that lie within the grid map.
*/
std::vector<Point2D> uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map,
unsigned int numPoints);
/*
* For each point in queryPoints, verify that the interpolated value of the grid map
* is close to the ground truth which is contained in Analytical functions structure.
* Called inside the tests. Calls macros from gtest.
*/
void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues,
const std::vector<Point2D> &queryPoints,
grid_map::InterpolationMethods interpolationMethod);
} // namespace grid_map_test