test_helpers.cpp
Go to the documentation of this file.
1 /*
2  * test_helpers.cpp
3  *
4  * Created on: Mar 3, 2020
5  * Author: Edo Jelavic
6  * Institute: ETH Zurich, Robotic Systems Lab
7  */
8 
9 #include "test_helpers.hpp"
10 
13 
14 // gtest
15 #include <gtest/gtest.h>
16 
17 namespace grid_map_test {
18 
19 std::mt19937 rndGenerator;
20 
22 {
23 
25 
26  func.f_ = [](double x, double y) {
27  return 0.0;
28  };
29 
30  fillGridMap(map, func);
31 
32  return func;
33 
34 }
35 
37 {
38 
40 
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);
46 
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));
49  };
50 
51  fillGridMap(map, func);
52 
53  return func;
54 
55 }
56 
58 {
59 
61 
62  func.f_ = [](double x,double y) {
63  return (-x*x -y*y +2.0*x*y +x*x*y*y);
64  };
65 
66  fillGridMap(map, func);
67 
68  return func;
69 
70 }
71 
73 {
75 
76  func.f_ = [](double x,double y) {
77  return (x*x-y*y);
78  };
79 
80  fillGridMap(map, func);
81 
82  return func;
83 
84 }
85 
87 {
88 
90 
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);
96 
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);
99  };
100 
101  fillGridMap(map, func);
102 
103  return func;
104 
105 }
106 
108 {
109 
110  AnalyticalFunctions func;
111 
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);
117  };
118 
119  fillGridMap(map, func);
120 
121  return func;
122 }
123 
125 {
126 
127  struct Gaussian
128  {
129  double x0, y0;
130  double varX, varY;
131  double s;
132  };
133 
134  AnalyticalFunctions func;
135 
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;
141 
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);
148  }
149 
150  func.f_ = [g](double x,double y) {
151  double value = 0.0;
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));
159  }
160 
161  return value;
162  };
163 
164  fillGridMap(map, func);
165 
166  return func;
167 }
168 
170 {
171  grid_map::Matrix& data = (*map)[testLayer];
172  for (grid_map::GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) {
173  const grid_map::Index index(*iterator);
174  grid_map::Position pos;
175  map->getPosition(index, pos);
176  data(index(0), index(1)) = functions.f_(pos.x(), pos.y());
177  }
178 }
179 
180 grid_map::GridMap createMap(const grid_map::Length &length, double resolution,
181  const grid_map::Position &pos)
182 {
183  grid_map::GridMap map;
184 
185  map.setGeometry(length, resolution, pos);
186  map.add(testLayer, 0.0);
187  map.setFrameId("map");
188 
189  return map;
190 }
191 
193  unsigned int numPoints)
194 {
195 
196  // stay away from the edges
197  // on the edges the cubic interp is invalid. Not enough points.
198  const double dimX = map.getLength().x() / 2.0 - 3.0 * map.getResolution();
199  const double dimY = map.getLength().y() / 2.0 - 3.0 * map.getResolution();
200  std::uniform_real_distribution<double> Ux(-dimX, dimX);
201  std::uniform_real_distribution<double> Uy(-dimY, dimY);
202 
203  std::vector<Point2D> points(numPoints);
204  for (auto &point : points) {
205  point.x_ = Ux(rndGenerator);
206  point.y_ = Uy(rndGenerator);
207  }
208 
209  return points;
210 }
211 
213  const std::vector<Point2D> &queryPoints,
214  grid_map::InterpolationMethods interpolationMethod){
215  for (const auto point : queryPoints) {
216  const grid_map::Position p(point.x_, point.y_);
217  const double trueValue = trueValues.f_(p.x(), p.y());
218  const double interpolatedValue = map.atPosition(
219  grid_map_test::testLayer, p, interpolationMethod);
220  EXPECT_NEAR(trueValue, interpolatedValue, grid_map_test::maxAbsErrorValue);
221  }
222 }
223 
224 
225 } /* namespace grid_map_test */
226 
const Length & getLength() const
Definition: GridMap.cpp:644
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Definition: GridMap.cpp:45
float & atPosition(const std::string &layer, const Position &position)
Definition: GridMap.cpp:160
std::mt19937 rndGenerator
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:237
Eigen::MatrixXf Matrix
Definition: TypeDefs.hpp:16
void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions)
AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map)
AnalyticalFunctions createTanhWorld(grid_map::GridMap *map)
InterpolationMethods
Definition: TypeDefs.hpp:39
AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map)
std::vector< Point2D > uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map, unsigned int numPoints)
double getResolution() const
Definition: GridMap.cpp:652
static const std::string testLayer
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
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)
Definition: GridMap.cpp:89
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)
Definition: GridMap.cpp:636
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
AnalyticalFunctions createFlatWorld(grid_map::GridMap *map)


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:27