97 lines
3.2 KiB
C++
97 lines
3.2 KiB
C++
/*
|
|
* 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
|