PolygonIteratorTest.cpp
Go to the documentation of this file.
1 /*
2  * PolygonIteratorTest.cpp
3  *
4  * Created on: Sep 19, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
12 
13 // Eigen
14 #include <Eigen/Core>
15 
16 // gtest
17 #include <gtest/gtest.h>
18 
19 // Limits
20 #include <cfloat>
21 
22 // Vector
23 #include <vector>
24 
25 using namespace std;
26 using namespace Eigen;
27 using namespace grid_map;
28 
29 TEST(PolygonIterator, FullCover)
30 {
31  vector<string> types;
32  types.push_back("type");
33  GridMap map(types);
34  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
35 
36  Polygon polygon;
37  polygon.addVertex(Position(-100.0, 100.0));
38  polygon.addVertex(Position(100.0, 100.0));
39  polygon.addVertex(Position(100.0, -100.0));
40  polygon.addVertex(Position(-100.0, -100.0));
41 
42  PolygonIterator iterator(map, polygon);
43 
44  EXPECT_FALSE(iterator.isPastEnd());
45  EXPECT_EQ(0, (*iterator)(0));
46  EXPECT_EQ(0, (*iterator)(1));
47 
48  ++iterator;
49  EXPECT_FALSE(iterator.isPastEnd());
50  EXPECT_EQ(0, (*iterator)(0));
51  EXPECT_EQ(1, (*iterator)(1));
52 
53  ++iterator;
54  EXPECT_FALSE(iterator.isPastEnd());
55  EXPECT_EQ(0, (*iterator)(0));
56  EXPECT_EQ(2, (*iterator)(1));
57 
58  for (int i = 0; i < 37; ++i) ++iterator;
59 
60  EXPECT_FALSE(iterator.isPastEnd());
61  EXPECT_EQ(7, (*iterator)(0));
62  EXPECT_EQ(4, (*iterator)(1));
63 
64  ++iterator;
65  EXPECT_TRUE(iterator.isPastEnd());
66 }
67 
69 {
70  GridMap map({"types"});
71  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
72 
73  Polygon polygon;
74  polygon.addVertex(Position(99.0, 101.0));
75  polygon.addVertex(Position(101.0, 101.0));
76  polygon.addVertex(Position(101.0, 99.0));
77  polygon.addVertex(Position(99.0, 99.0));
78 
79  PolygonIterator iterator(map, polygon);
80 
81  EXPECT_TRUE(iterator.isPastEnd());
82 }
83 
85 {
86  GridMap map({"types"});
87  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
88 
89  Polygon polygon;
90  polygon.addVertex(Position(-1.0, 1.5));
91  polygon.addVertex(Position(1.0, 1.5));
92  polygon.addVertex(Position(1.0, -1.5));
93  polygon.addVertex(Position(-1.0, -1.5));
94 
95  PolygonIterator iterator(map, polygon);
96 
97  EXPECT_FALSE(iterator.isPastEnd());
98  EXPECT_EQ(3, (*iterator)(0));
99  EXPECT_EQ(1, (*iterator)(1));
100 
101  ++iterator;
102  EXPECT_FALSE(iterator.isPastEnd());
103  EXPECT_EQ(3, (*iterator)(0));
104  EXPECT_EQ(2, (*iterator)(1));
105 
106  ++iterator;
107  EXPECT_FALSE(iterator.isPastEnd());
108  EXPECT_EQ(3, (*iterator)(0));
109  EXPECT_EQ(3, (*iterator)(1));
110 
111  ++iterator;
112  EXPECT_FALSE(iterator.isPastEnd());
113  EXPECT_EQ(4, (*iterator)(0));
114  EXPECT_EQ(1, (*iterator)(1));
115 
116  ++iterator;
117  EXPECT_FALSE(iterator.isPastEnd());
118  EXPECT_EQ(4, (*iterator)(0));
119  EXPECT_EQ(2, (*iterator)(1));
120 
121  ++iterator;
122  EXPECT_FALSE(iterator.isPastEnd());
123  EXPECT_EQ(4, (*iterator)(0));
124  EXPECT_EQ(3, (*iterator)(1));
125 
126  ++iterator;
127  EXPECT_TRUE(iterator.isPastEnd());
128 }
129 
130 TEST(PolygonIterator, TopLeftTriangle)
131 {
132  GridMap map({"types"});
133  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
134 
135  Polygon polygon;
136  polygon.addVertex(Position(-40.1, 20.6));
137  polygon.addVertex(Position(40.1, 20.4));
138  polygon.addVertex(Position(-40.1, -20.6));
139 
140  PolygonIterator iterator(map, polygon);
141 
142  EXPECT_FALSE(iterator.isPastEnd());
143  EXPECT_EQ(0, (*iterator)(0));
144  EXPECT_EQ(0, (*iterator)(1));
145 
146  ++iterator;
147  EXPECT_FALSE(iterator.isPastEnd());
148  EXPECT_EQ(1, (*iterator)(0));
149  EXPECT_EQ(0, (*iterator)(1));
150 
151  // TODO Extend.
152 }
153 
155 {
156  GridMap map({"layer"});
157  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
158  map.move(Position(2.0, 0.0));
159 
160  Polygon polygon;
161  polygon.addVertex(Position(6.1, 1.6));
162  polygon.addVertex(Position(0.9, 1.6));
163  polygon.addVertex(Position(0.9, -1.6));
164  polygon.addVertex(Position(6.1, -1.6));
165  PolygonIterator iterator(map, polygon);
166 
167  EXPECT_FALSE(iterator.isPastEnd());
168  EXPECT_EQ(6, (*iterator)(0));
169  EXPECT_EQ(1, (*iterator)(1));
170 
171  ++iterator;
172  EXPECT_FALSE(iterator.isPastEnd());
173  EXPECT_EQ(6, (*iterator)(0));
174  EXPECT_EQ(2, (*iterator)(1));
175 
176  for (int i = 0; i < 4; ++i) ++iterator;
177 
178  EXPECT_FALSE(iterator.isPastEnd());
179  EXPECT_EQ(7, (*iterator)(0));
180  EXPECT_EQ(3, (*iterator)(1));
181 
182  ++iterator;
183  EXPECT_FALSE(iterator.isPastEnd());
184  EXPECT_EQ(0, (*iterator)(0));
185  EXPECT_EQ(1, (*iterator)(1));
186 
187  for (int i = 0; i < 8; ++i) ++iterator;
188 
189  EXPECT_FALSE(iterator.isPastEnd());
190  EXPECT_EQ(2, (*iterator)(0));
191  EXPECT_EQ(3, (*iterator)(1));
192 
193  ++iterator;
194  EXPECT_TRUE(iterator.isPastEnd());
195 }
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
Definition: GridMap.cpp:51
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
TEST(PolygonIterator, FullCover)
void addVertex(const Position &vertex)
Definition: Polygon.cpp:46
Eigen::Array2d Length
Definition: TypeDefs.hpp:24


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