GridMapTest.cpp
Go to the documentation of this file.
1 /*
2  * GridMapTest.cpp
3  *
4  * Created on: Aug 26, 2015
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #include "grid_map_core/GridMap.hpp"
10 
11 // gtest
12 #include <gtest/gtest.h>
13 
14 // Math
15 #include <math.h>
16 
17 using namespace std;
18 using namespace grid_map;
19 
20 TEST(GridMap, CopyConstructor)
21 {
22  GridMap map({"layer_a", "layer_b"});
23  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
24  map["layer_a"].setConstant(1.0);
25  map["layer_b"].setConstant(2.0);
26  GridMap mapCopy(map);
27  EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
28  EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
29  EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
30  EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
31  EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
32  EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
33  EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
34  EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
35  EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
36 }
37 
38 TEST(GridMap, CopyAssign)
39 {
40  GridMap map({"layer_a", "layer_b"});
41  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
42  map["layer_a"].setConstant(1.0);
43  map["layer_b"].setConstant(2.0);
44  GridMap mapCopy;
45  mapCopy = map;
46  EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]);
47  EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]);
48  EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x());
49  EXPECT_EQ(map.getLength().y(), mapCopy.getLength().y());
50  EXPECT_EQ(map.getPosition().x(), mapCopy.getPosition().x());
51  EXPECT_EQ(map.getPosition().y(), mapCopy.getPosition().y());
52  EXPECT_EQ(map.getLayers().size(), mapCopy.getLayers().size());
53  EXPECT_EQ(map["layer_a"](0, 0), mapCopy["layer_a"](0, 0));
54  EXPECT_EQ(map["layer_b"](0, 0), mapCopy["layer_b"](0, 0));
55 }
56 
57 TEST(GridMap, Move)
58 {
59  GridMap map;
60  map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
61  map.add("layer", 0.0);
62  map.setBasicLayers(map.getLayers());
63  std::vector<BufferRegion> regions;
64  map.move(Position(-3.0, -2.0), regions);
65  Index startIndex = map.getStartIndex();
66 
67  EXPECT_EQ(3, startIndex(0));
68  EXPECT_EQ(2, startIndex(1));
69 
70  EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map.
71  EXPECT_TRUE(map.isValid(Index(3, 2)));
72  EXPECT_FALSE(map.isValid(Index(2, 2)));
73  EXPECT_FALSE(map.isValid(Index(3, 1)));
74  EXPECT_TRUE(map.isValid(Index(7, 4)));
75 
76  EXPECT_EQ(2, regions.size());
77  EXPECT_EQ(0, regions[0].getStartIndex()[0]);
78  EXPECT_EQ(0, regions[0].getStartIndex()[1]);
79  EXPECT_EQ(3, regions[0].getSize()[0]);
80  EXPECT_EQ(5, regions[0].getSize()[1]);
81  EXPECT_EQ(0, regions[1].getStartIndex()[0]);
82  EXPECT_EQ(0, regions[1].getStartIndex()[1]);
83  EXPECT_EQ(8, regions[1].getSize()[0]);
84  EXPECT_EQ(2, regions[1].getSize()[1]);
85 }
86 
87 TEST(GridMap, Transform)
88 {
89  // Initial map.
90  GridMap map;
91  const auto heightLayerName = "height";
92 
93  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
94  map.add(heightLayerName, 0.0);
95  map.setBasicLayers(map.getLayers());
96  map.get(heightLayerName)(0,0) = 1.0;
97 
98  // Transformation (90° rotation).
99  Eigen::Isometry3d transform;
100 
101  transform.translation().x() = 0.0;
102  transform.translation().y() = 0.0;
103  transform.translation().z() = 0.0;
104 
105  transform.linear()(0,0) = 0.0;
106  transform.linear()(0,1) = -1.0;
107  transform.linear()(0,2) = 0.0;
108 
109  transform.linear()(1,0) = 1.0;
110  transform.linear()(1,1) = 0.0;
111  transform.linear()(1,2) = 0.0;
112 
113  transform.linear()(2,0) = 0.0;
114  transform.linear()(2,1) = 0.0;
115  transform.linear()(2,2) = 1.0;
116 
117  // Apply affine transformation.
118  const GridMap transformedMap = map.getTransformedMap(transform, heightLayerName, map.getFrameId(), 0.25);
119 
120  // Check if map has been rotated by 90° about z
121  EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6);
122  EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6);
123  EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size());
124  EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0,0), transformedMap.get(heightLayerName)(19,0));
125 }
126 
127 TEST(AddDataFrom, ExtendMapAligned)
128 {
129  GridMap map1, map2;
130  map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
131  map1.add("zero", 0.0);
132  map1.add("one", 1.0);
133  map1.setBasicLayers(map1.getLayers());
134 
135  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
136  map2.add("one", 1.1);
137  map2.add("two", 2.0);
138  map2.setBasicLayers(map1.getLayers());
139 
140  map1.addDataFrom(map2, true, true, true);
141 
142  EXPECT_TRUE(map1.exists("two"));
143  EXPECT_TRUE(map1.isInside(Position(3.0, 3.0)));
144  EXPECT_DOUBLE_EQ(6.0, map1.getLength().x());
145  EXPECT_DOUBLE_EQ(6.0, map1.getLength().y());
146  EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x());
147  EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y());
148  EXPECT_NEAR(1.1, map1.atPosition("one", Position(2, 2)), 1e-4);
149  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(-2, -2)));
150  EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
151 }
152 
153 TEST(AddDataFrom, ExtendMapNotAligned)
154 {
155  GridMap map1, map2;
156  map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0)); // bufferSize(6, 6)
157  map1.add("nan");
158  map1.add("one", 1.0);
159  map1.add("zero", 0.0);
160  map1.setBasicLayers(map1.getLayers());
161 
162  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2));
163  map2.add("nan", 1.0);
164  map2.add("one", 1.1);
165  map2.add("two", 2.0);
166  map2.setBasicLayers(map1.getLayers());
167 
168  std::vector<std::string> stringVector;
169  stringVector.push_back("nan");
170  map1.addDataFrom(map2, true, false, false, stringVector);
171  Index index;
172  map1.getIndex(Position(-2, -2), index);
173 
174  EXPECT_FALSE(map1.exists("two"));
175  EXPECT_TRUE(map1.isInside(Position(4.0, 4.0)));
176  EXPECT_DOUBLE_EQ(8.0, map1.getLength().x());
177  EXPECT_DOUBLE_EQ(8.0, map1.getLength().y());
178  EXPECT_DOUBLE_EQ(1.0, map1.getPosition().x());
179  EXPECT_DOUBLE_EQ(1.0, map1.getPosition().y());
180  EXPECT_FALSE(map1.isValid(index, "nan"));
181  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(0.0, 0.0)));
182  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("nan", Position(3.0, 3.0)));
183 }
184 
185 TEST(AddDataFrom, CopyData)
186 {
187  GridMap map1, map2;
188  map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
189  map1.add("zero", 0.0);
190  map1.add("one");
191  map1.setBasicLayers(map1.getLayers());
192 
193  map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0));
194  map2.add("one", 1.0);
195  map2.add("two", 2.0);
196  map2.setBasicLayers(map1.getLayers());
197 
198  map1.addDataFrom(map2, false, false, true);
199  Index index;
200  map1.getIndex(Position(-2, -2), index);
201 
202  EXPECT_TRUE(map1.exists("two"));
203  EXPECT_FALSE(map1.isInside(Position(3.0, 3.0)));
204  EXPECT_DOUBLE_EQ(5.0, map1.getLength().x());
205  EXPECT_DOUBLE_EQ(5.0, map1.getLength().y());
206  EXPECT_DOUBLE_EQ(0.0, map1.getPosition().x());
207  EXPECT_DOUBLE_EQ(0.0, map1.getPosition().y());
208  EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(2, 2)));
209  EXPECT_FALSE(map1.isValid(index, "one"));
210  EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0)));
211 }
212 
213 TEST(ValueAtPosition, NearestNeighbor)
214 {
215  GridMap map( { "types" });
216  map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
217 
218  map.at("types", Index(0,0)) = 0.5;
219  map.at("types", Index(0,1)) = 3.8;
220  map.at("types", Index(0,2)) = 2.0;
221  map.at("types", Index(1,0)) = 2.1;
222  map.at("types", Index(1,1)) = 1.0;
223  map.at("types", Index(1,2)) = 2.0;
224  map.at("types", Index(2,0)) = 1.0;
225  map.at("types", Index(2,1)) = 2.0;
226  map.at("types", Index(2,2)) = 2.0;
227 
228  double value;
229 
230  value = map.atPosition("types", Position(1.35,-0.4));
231  EXPECT_DOUBLE_EQ((float)3.8, value);
232 
233  value = map.atPosition("types", Position(-0.3,0.0));
234  EXPECT_DOUBLE_EQ(1.0, value);
235 }
236 
237 TEST(ValueAtPosition, LinearInterpolated)
238 {
239  GridMap map( { "types" });
240  map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0));
241 
242  map.at("types", Index(0,0)) = 0.5;
243  map.at("types", Index(0,1)) = 3.8;
244  map.at("types", Index(0,2)) = 2.0;
245  map.at("types", Index(1,0)) = 2.1;
246  map.at("types", Index(1,1)) = 1.0;
247  map.at("types", Index(1,2)) = 2.0;
248  map.at("types", Index(2,0)) = 1.0;
249  map.at("types", Index(2,1)) = 2.0;
250  map.at("types", Index(2,2)) = 2.0;
251 
252  double value;
253 
254  // Close to the border -> reverting to INTER_NEAREST.
255  value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
256  EXPECT_DOUBLE_EQ(2.0, value);
257  // In between 1.0 and 2.0 field.
258  value = map.atPosition("types", Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR);
259  EXPECT_DOUBLE_EQ(1.5, value);
260  // Calculated "by Hand".
261  value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
262  EXPECT_NEAR(2.1963200, value, 0.0000001);
263 }
grid_map::GridMap::getLength
const Length & getLength() const
Definition: GridMap.cpp:655
grid_map::Index
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
grid_map::GridMap::setGeometry
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Definition: GridMap.cpp:51
grid_map::GridMap::atPosition
float & atPosition(const std::string &layer, const Position &position)
Definition: GridMap.cpp:174
grid_map::GridMap::getStartIndex
const Index & getStartIndex() const
Definition: GridMap.cpp:679
grid_map::GridMap::getPosition
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:232
grid_map::GridMap::setBasicLayers
void setBasicLayers(const std::vector< std::string > &basicLayers)
Definition: GridMap.cpp:77
grid_map::GridMap::getFrameId
const std::string & getFrameId() const
Definition: GridMap.cpp:650
grid_map::GridMap::getTransformedMap
GridMap getTransformedMap(const Eigen::Isometry3d &transform, const std::string &heightLayerName, const std::string &newFrameId, const double sampleRatio=0.0) const
Definition: GridMap.cpp:341
grid_map::GridMap
Definition: GridMap.hpp:40
grid_map::GridMap::move
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
Definition: GridMap.cpp:453
grid_map::GridMap::addDataFrom
bool addDataFrom(const GridMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >())
Definition: GridMap.cpp:527
grid_map::GridMap::isValid
bool isValid(const Index &index) const
Definition: GridMap.cpp:242
grid_map
Definition: BufferRegion.hpp:13
grid_map::GridMap::exists
bool exists(const std::string &layer) const
Definition: GridMap.cpp:120
TEST
TEST(GridMap, CopyConstructor)
Definition: GridMapTest.cpp:20
grid_map::GridMap::getLayers
const std::vector< std::string > & getLayers() const
Definition: GridMap.cpp:169
grid_map::GridMap::isInside
bool isInside(const Position &position) const
Definition: GridMap.cpp:237
grid_map::Position
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
grid_map::GridMap::get
const Matrix & get(const std::string &layer) const
Definition: GridMap.cpp:125
grid_map::GridMap::add
void add(const std::string &layer, const double value=NAN)
Definition: GridMap.cpp:100
grid_map::GridMap::getIndex
bool getIndex(const Position &position, Index &index) const
Definition: GridMap.cpp:227
grid_map::Length
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
grid_map::GridMap::getSize
const Size & getSize() const
Definition: GridMap.cpp:670

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:08