15 #include <gtest/gtest.h> 26 func.
f_ = [](
double x,
double y) {
41 std::uniform_real_distribution<double> shift(-3.0, 3.0);
42 std::uniform_real_distribution<double> scale(1.0, 20.0);
43 const double x0 = shift(rndGenerator);
44 const double y0 = shift(rndGenerator);
45 const double s = scale(rndGenerator);
47 func.
f_ = [x0, y0,s](
double x,
double y) {
48 return s / (1 + std::pow(x-x0, 2.0) + std::pow(y-y0, 2.0));
62 func.
f_ = [](
double x,
double y) {
63 return (-x*x -y*y +2.0*x*y +x*x*y*y);
76 func.
f_ = [](
double x,
double y) {
91 std::uniform_real_distribution<double> Uw(0.1, 4.0);
92 const double w1 = Uw(rndGenerator);
93 const double w2 = Uw(rndGenerator);
94 const double w3 = Uw(rndGenerator);
95 const double w4 = Uw(rndGenerator);
97 func.
f_ = [w1,w2,w3,w4](
double x,
double y) {
98 return std::cos(w1*x) + std::sin(w2*y) + std::cos(w3*x) + std::sin(w4*y);
112 std::uniform_real_distribution<double> scaling(0.1, 2.0);
113 const double s = scaling(rndGenerator);
114 func.
f_ = [s](
double x,
double y) {
115 const double expZ = std::exp(2 *s* x);
116 return (expZ - 1) / (expZ + 1);
136 std::uniform_real_distribution<double> var(0.1, 3.0);
137 std::uniform_real_distribution<double> mean(-4.0, 4.0);
138 std::uniform_real_distribution<double> scale(-3.0, 3.0);
139 constexpr
int numGaussians = 3;
140 std::array<Gaussian, numGaussians> g;
142 for (
int i = 0; i < numGaussians; ++i) {
143 g.at(i).x0 = mean(rndGenerator);
144 g.at(i).y0 = mean(rndGenerator);
145 g.at(i).varX = var(rndGenerator);
146 g.at(i).varY = var(rndGenerator);
147 g.at(i).s = scale(rndGenerator);
150 func.
f_ = [g](
double x,
double y) {
152 for (
int i = 0; i < g.size(); ++i) {
153 const double x0 = g.at(i).x0;
154 const double y0 = g.at(i).y0;
155 const double varX = g.at(i).varX;
156 const double varY = g.at(i).varY;
157 const double s = g.at(i).s;
158 value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY));
176 data(index(0), index(1)) = functions.
f_(pos.x(), pos.y());
193 unsigned int numPoints)
200 std::uniform_real_distribution<double> Ux(-dimX, dimX);
201 std::uniform_real_distribution<double> Uy(-dimY, dimY);
203 std::vector<Point2D> points(numPoints);
204 for (
auto &point : points) {
205 point.x_ = Ux(rndGenerator);
206 point.y_ = Uy(rndGenerator);
213 const std::vector<Point2D> &queryPoints,
215 for (
const auto point : queryPoints) {
217 const double trueValue = trueValues.
f_(p.x(), p.y());
218 const double interpolatedValue = map.
atPosition(
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)
std::mt19937 rndGenerator
bool getPosition(const Index &index, Position &position) const
void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions)
AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map)
AnalyticalFunctions createTanhWorld(grid_map::GridMap *map)
AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map)
std::vector< Point2D > uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map, unsigned int numPoints)
double getResolution() const
static const std::string testLayer
AnalyticalFunctions createSineWorld(grid_map::GridMap *map)
grid_map::GridMap createMap(const grid_map::Length &length, double resolution, const grid_map::Position &pos)
std::function< double(double, double)> f_
AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map)
void add(const std::string &layer, const double value=NAN)
void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues, const std::vector< Point2D > &queryPoints, grid_map::InterpolationMethods interpolationMethod)
const double maxAbsErrorValue
void setFrameId(const std::string &frameId)
AnalyticalFunctions createFlatWorld(grid_map::GridMap *map)