SignedDistanceFieldTest.cpp
Go to the documentation of this file.
1 /*
2  * SignedDistanceFieldTest.cpp
3  *
4  * Created on: Aug 25, 2017
5  * Author: Takahiro Miki, Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #include "grid_map_core/GridMap.hpp"
10 #include "grid_map_sdf/SignedDistanceField.hpp"
11 
12 #include <gtest/gtest.h>
13 #include <math.h>
14 
15 using namespace std;
16 using namespace grid_map;
17 
18 TEST(SignedDistanceField, EmptyMap)
19 {
20  GridMap map({"layer"});
21  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0));
22 
23  SignedDistanceField sdf;
24  sdf.calculateSignedDistanceField(map, "layer", 1.0);
25  Position3 position(0.0, 0.0, 0.0);
26 
27  EXPECT_NO_THROW(sdf.getDistanceAt(position));
28  EXPECT_NO_THROW(sdf.getInterpolatedDistanceAt(position));
29  EXPECT_NO_THROW(sdf.getDistanceGradientAt(position));
30 }
31 
32 TEST(SignedDistanceField, GetDistanceFlat)
33 {
34  GridMap map({"layer"});
35  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
36  map["layer"].setConstant(1.0);
37  map.at("layer", Index(0,0)) = -1.0;
38 
39  SignedDistanceField sdf;
40  sdf.calculateSignedDistanceField(map, "layer", 2.5);
41  Position pos;
42  map.getPosition(Index(9, 9), pos);
43  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
44  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.1)), -0.9, 0.0001);
45  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.2)), -0.8, 0.0001);
46  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.3)), -0.7, 0.0001);
47  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.4)), -0.6, 0.0001);
48  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001);
49  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.6)), -0.4, 0.0001);
50  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.7)), -0.3, 0.0001);
51  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.8)), -0.2, 0.0001);
52  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.9)), -0.1, 0.0001);
53  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
54  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
55  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001);
56  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001);
57  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.4, 0.0001);
58  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.5, 0.0001);
59  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.6, 0.0001);
60  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.7, 0.0001);
61  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.8, 0.0001);
62  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.9, 0.0001);
63  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 1.0, 0.0001);
64  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 1.1, 0.0001);
65  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 1.2, 0.0001);
66  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 1.3, 0.0001);
67  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 1.4, 0.0001);
68  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 1.5, 0.0001);
69  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 2.5, 0.0001);
70 }
71 
72 
73 TEST(SignedDistanceField, GetDistance)
74 {
75  GridMap map({"layer"});
76  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
77  map["layer"].setConstant(1.0);
78 
79  map.at("layer", Index(3,4)) = 2.0;
80  map.at("layer", Index(3,5)) = 2.0;
81  map.at("layer", Index(3,6)) = 2.0;
82  map.at("layer", Index(4,4)) = 2.0;
83  map.at("layer", Index(4,5)) = 2.0;
84  map.at("layer", Index(4,6)) = 2.0;
85  map.at("layer", Index(5,4)) = 2.0;
86  map.at("layer", Index(5,5)) = 2.0;
87  map.at("layer", Index(5,6)) = 2.0;
88  map.at("layer", Index(6,4)) = 2.0;
89  map.at("layer", Index(6,5)) = 2.0;
90  map.at("layer", Index(6,6)) = 2.0;
91  map.at("layer", Index(7,4)) = 2.0;
92  map.at("layer", Index(7,5)) = 2.0;
93  map.at("layer", Index(7,6)) = 2.0;
94 
95  SignedDistanceField sdf;
96  sdf.calculateSignedDistanceField(map, "layer", 1.5);
97  Position pos;
98 
99  map.getPosition(Index(5, 5), pos);
100  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
101  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -1.0, 0.0001);
102  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.0, 0.0001);
103  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.1, 0.0001);
104  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.1, 0.0001);
105  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.1, 0.0001);
106  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.1, 0.0001);
107  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.1, 0.0001);
108  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.1, 0.0001);
109  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.1, 0.0001);
110  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001);
111  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001);
112  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001);
113  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001);
114  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001);
115  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001);
116  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001);
117  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001);
118  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.5, 0.0001);
119 
120  map.getPosition(Index(5, 2), pos);
121  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), 0.0, 0.0001);
122  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), 0.0, 0.0001);
123  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
124  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
125  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.1, 0.0001);
126  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.1, 0.0001);
127  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.1, 0.0001);
128  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.1, 0.0001);
129  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.1, 0.0001);
130  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.1, 0.0001);
131  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.1, 0.0001);
132  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.1, 0.0001);
133  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.2, 0.0001);
134  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.3, 0.0001);
135  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.4, 0.0001);
136  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.5, 0.0001);
137  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.6, 0.0001);
138  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.7, 0.0001);
139  EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.7, 0.0001);
140 }
141 
142 TEST(SignedDistanceField, GetDistanceGradient)
143 {
144  GridMap map({"layer"});
145  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
146  map["layer"].setConstant(1.0);
147 
148  map.at("layer", Index(3,4)) = 2.0;
149  map.at("layer", Index(3,5)) = 2.0;
150  map.at("layer", Index(3,6)) = 2.0;
151  map.at("layer", Index(4,4)) = 2.0;
152  map.at("layer", Index(4,5)) = 2.0;
153  map.at("layer", Index(4,6)) = 2.0;
154  map.at("layer", Index(5,4)) = 2.0;
155  map.at("layer", Index(5,5)) = 2.0;
156  map.at("layer", Index(5,6)) = 2.0;
157  map.at("layer", Index(6,4)) = 2.0;
158  map.at("layer", Index(6,5)) = 2.0;
159  map.at("layer", Index(6,6)) = 2.0;
160  map.at("layer", Index(7,4)) = 2.0;
161  map.at("layer", Index(7,5)) = 2.0;
162  map.at("layer", Index(7,6)) = 2.0;
163 
164  SignedDistanceField sdf;
165  sdf.calculateSignedDistanceField(map, "layer", 1.5);
166  Position pos;
167  Vector3 gradient;
168 
169  map.getPosition(Index(5, 6), pos);
170  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 0.0));
171  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
172  EXPECT_NEAR(gradient.y(), -1, 0.0001);
173  EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
174  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
175  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
176  EXPECT_NEAR(gradient.y(), -1, 0.0001);
177  EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
178  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0));
179  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
180  EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
181  EXPECT_NEAR(gradient.z(), 1, 0.0001);
182  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2));
183  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
184  EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
185  EXPECT_NEAR(gradient.z(), 1.0, 0.0001);
186  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 10.0));
187  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
188  EXPECT_NEAR(gradient.y(), -1.5, 0.0001);
189  EXPECT_NEAR(gradient.z(), 1.0, 0.0001);
190  map.getPosition(Index(2, 2), pos);
191  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
192  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
193  EXPECT_NEAR(gradient.y(), 1, 0.0001);
194  EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
195  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0));
196  EXPECT_NEAR(gradient.x(), 0.207107, 0.0001);
197  EXPECT_NEAR(gradient.y(), 1.5, 0.0001);
198  EXPECT_NEAR(gradient.z(), 1, 0.0001);
199  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2));
200  EXPECT_NEAR(gradient.x(), 0.207107, 0.0001);
201  EXPECT_NEAR(gradient.y(), 1.5, 0.0001);
202  EXPECT_NEAR(gradient.z(), 1, 0.0001);
203  map.getPosition(Index(12, 22), pos);
204  gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0));
205  EXPECT_NEAR(gradient.x(), 0.0, 0.0001);
206  EXPECT_NEAR(gradient.y(), 1.0, 0.0001);
207  EXPECT_NEAR(gradient.z(), 0.5, 0.0001);
208 }
209 
210 TEST(SignedDistanceField, GetInterpolatedDistance)
211 {
212  GridMap map({"layer"});
213  map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25));
214  map["layer"].setConstant(1.0);
215 
216  map.at("layer", Index(3,3)) = 2.0;
217  map.at("layer", Index(3,4)) = 2.0;
218  map.at("layer", Index(3,5)) = 2.0;
219  map.at("layer", Index(3,6)) = 2.0;
220  map.at("layer", Index(3,7)) = 2.0;
221  map.at("layer", Index(4,3)) = 2.0;
222  map.at("layer", Index(4,4)) = 2.0;
223  map.at("layer", Index(4,5)) = 2.0;
224  map.at("layer", Index(4,6)) = 2.0;
225  map.at("layer", Index(4,7)) = 2.0;
226  map.at("layer", Index(5,3)) = 2.0;
227  map.at("layer", Index(5,4)) = 2.0;
228  map.at("layer", Index(5,5)) = 2.0;
229  map.at("layer", Index(5,6)) = 2.0;
230  map.at("layer", Index(5,7)) = 2.0;
231  map.at("layer", Index(6,3)) = 2.0;
232  map.at("layer", Index(6,4)) = 2.0;
233  map.at("layer", Index(6,5)) = 2.0;
234  map.at("layer", Index(6,6)) = 2.0;
235  map.at("layer", Index(6,7)) = 2.0;
236  map.at("layer", Index(7,3)) = 2.0;
237  map.at("layer", Index(7,4)) = 2.0;
238  map.at("layer", Index(7,5)) = 2.0;
239  map.at("layer", Index(7,6)) = 2.0;
240  map.at("layer", Index(7,7)) = 2.0;
241 
242  SignedDistanceField sdf;
243  sdf.calculateSignedDistanceField(map, "layer", 1.5);
244  Position pos;
245 
246  map.getPosition(Index(5, 5), pos);
247  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -5.05, 0.0001);
248  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -3.05, 0.0001);
249  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.05, 0.0001);
250  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.25, 0.0001);
251  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.25, 0.0001);
252  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.25, 0.0001);
253  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.25, 0.0001);
254  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.25, 0.0001);
255  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.25, 0.0001);
256  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.25, 0.0001);
257  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001);
258  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), -0.1, 0.0001);
259  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001);
260  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001);
261  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001);
262  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001);
263  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001);
264  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001);
265  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8, 0.0001);
266 
267  map.getPosition(Index(5, 10), pos);
268  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001);
269  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001);
270  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001);
271  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001);
272  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001);
273  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001);
274  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.35, 0.0001);
275  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.35, 0.0001);
276  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.35, 0.0001);
277  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.45, 0.0001);
278  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.55, 0.0001);
279  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.65, 0.0001);
280  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.75, 0.0001);
281  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.85, 0.0001);
282  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.95, 0.0001);
283  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8.45, 0.0001);
284 
285  map.getPosition(Index(5, 0), pos);
286  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001);
287  map.getPosition(Index(5, 1), pos);
288  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.175, 0.0001);
289  map.getPosition(Index(5, 2), pos);
290  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.025, 0.0001);
291  map.getPosition(Index(5, 3), pos);
292  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.2, 0.0001);
293  map.getPosition(Index(5, 4), pos);
294  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.225, 0.0001);
295  map.getPosition(Index(5, 5), pos);
296  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001);
297  map.getPosition(Index(5, 6), pos);
298  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.175, 0.0001);
299  map.getPosition(Index(5, 7), pos);
300  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.025, 0.0001);
301  map.getPosition(Index(5, 8), pos);
302  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.15, 0.0001);
303  map.getPosition(Index(5, 9), pos);
304  EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001);
305 }
Index
Eigen::Array2i Index
grid_map::GridMap::setGeometry
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
TEST
TEST(SignedDistanceField, EmptyMap)
Definition: SignedDistanceFieldTest.cpp:18
grid_map::GridMap
grid_map
Position3
Eigen::Vector3d Position3
Position
Eigen::Vector2d Position
grid_map::SignedDistanceField::getInterpolatedDistanceAt
double getInterpolatedDistanceAt(const Position3 &position) const
Definition: SignedDistanceField.cpp:124
grid_map::SignedDistanceField::calculateSignedDistanceField
void calculateSignedDistanceField(const GridMap &gridMap, const std::string &layer, const double heightClearance)
Definition: SignedDistanceField.cpp:32
grid_map::SignedDistanceField::getDistanceGradientAt
Vector3 getDistanceGradientAt(const Position3 &position) const
Definition: SignedDistanceField.cpp:145
Vector3
Eigen::Vector3d Vector3
grid_map::SignedDistanceField
Definition: SignedDistanceField.hpp:21
grid_map::SignedDistanceField::getDistanceAt
double getDistanceAt(const Position3 &position) const
Definition: SignedDistanceField.cpp:108
Length
Eigen::Array2d Length

grid_map_sdf
Author(s): Takahiro Miki , Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:49