Skip to content

Commit f1344bd

Browse files
maximilianwulfanybotics-sync-runner
maximilianwulf
authored and
anybotics-sync-runner
committed
Merge branch 'fix/clip_points_to_map' into 'master'
[grid_map] Fix getClosestPositionInMap GitOrigin-RevId: b7d32e76cec97e5c8d50b06aa17569ef085de0db
1 parent 61972c8 commit f1344bd

2 files changed

Lines changed: 210 additions & 5 deletions

File tree

‎grid_map_core/src/GridMap.cpp‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -720,12 +720,13 @@ Position GridMap::getClosestPositionInMap(const Position& position) const {
720720
const double maxY = bottomLeftCorner.y();
721721
const double minY = bottomRightCorner.y();
722722

723-
// Clip to box constraints.
724-
positionInMap.x() = std::fmin(positionInMap.x(), maxX);
725-
positionInMap.y() = std::fmin(positionInMap.y(), maxY);
723+
// Clip to box constraints and correct for indexing precision.
724+
// Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
725+
positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
726+
positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());
726727

727-
positionInMap.x() = std::fmax(positionInMap.x(), minX);
728-
positionInMap.y() = std::fmax(positionInMap.y(), minY);
728+
positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
729+
positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());
729730

730731
return positionInMap;
731732
}

‎grid_map_core/test/GridMapTest.cpp‎

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -148,6 +148,210 @@ TEST(GridMap, ClipToMap)
148148
EXPECT_TRUE(map.isInside(clippedPositionOutMap));
149149
}
150150

151+
152+
153+
TEST(GridMap, ClipToMap2)
154+
{
155+
GridMap map({"types"});
156+
map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
157+
158+
// Test 8 points outside of map.
159+
/*
160+
* A B C
161+
* +---+
162+
* | | X
163+
* D| |E ^
164+
* | | |
165+
* +---+ Y<--+
166+
* F G H
167+
*
168+
* Note: Position to index alignment is an half open interval.
169+
* An example position of 0.5 is assigned to the upper index.
170+
* The interval in the current example is:
171+
* Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...)
172+
* Index: 8 9 10 11
173+
*/
174+
175+
Index insideIndex;
176+
Position outsidePosition;
177+
178+
// Point A
179+
outsidePosition = Position(1.0, 1.0);
180+
auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
181+
bool isInside = map.getIndex(closestInsidePosition, insideIndex);
182+
183+
auto expectedPosition = Position(0.5, 0.5);
184+
auto expectedIndex = Index(0, 0);
185+
186+
// Check position.
187+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
188+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
189+
190+
// Check index.
191+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
192+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
193+
194+
// Check if index is inside.
195+
EXPECT_TRUE(isInside) << "position is: " << std::endl
196+
<< closestInsidePosition << std::endl
197+
<< " index is: " << std::endl
198+
<< insideIndex << std::endl;
199+
200+
// Point B
201+
outsidePosition = Position(1.0, 0.0);
202+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
203+
isInside = map.getIndex(closestInsidePosition, insideIndex);
204+
205+
expectedPosition = Position(0.5, 0.0);
206+
expectedIndex = Index(0, 10);
207+
208+
// Check position.
209+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
210+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
211+
212+
// Check index.
213+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
214+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
215+
216+
// Check if index is inside.
217+
EXPECT_TRUE(isInside) << "position is: " << std::endl
218+
<< closestInsidePosition << std::endl
219+
<< " index is: " << std::endl
220+
<< insideIndex << std::endl;
221+
222+
// Point C
223+
outsidePosition = Position(1.0, -1.0);
224+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
225+
isInside = map.getIndex(closestInsidePosition, insideIndex);
226+
227+
expectedPosition = Position(0.5, -0.5);
228+
expectedIndex = Index(0, 19);
229+
230+
// Check position.
231+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
232+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
233+
234+
// Check index.
235+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
236+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
237+
238+
// Check if index is inside.
239+
EXPECT_TRUE(isInside) << "position is: " << std::endl
240+
<< closestInsidePosition << std::endl
241+
<< " index is: " << std::endl
242+
<< insideIndex << std::endl;
243+
244+
// Point D
245+
outsidePosition = Position(0.0, 1.0);
246+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
247+
isInside = map.getIndex(closestInsidePosition, insideIndex);
248+
249+
expectedPosition = Position(0.0, 0.5);
250+
expectedIndex = Index(10, 0);
251+
252+
// Check position.
253+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
254+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
255+
256+
// Check index.
257+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
258+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
259+
260+
// Check if index is inside.
261+
EXPECT_TRUE(isInside) << "position is: " << std::endl
262+
<< closestInsidePosition << std::endl
263+
<< " index is: " << std::endl
264+
<< insideIndex << std::endl;
265+
266+
// Point E
267+
outsidePosition = Position(0.0, -1.0);
268+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
269+
isInside = map.getIndex(closestInsidePosition, insideIndex);
270+
271+
expectedPosition = Position(0.0, -0.5);
272+
expectedIndex = Index(10, 19);
273+
274+
// Check position.
275+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
276+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
277+
278+
// Check index.
279+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
280+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
281+
282+
// Check if index is inside.
283+
EXPECT_TRUE(isInside) << "position is: " << std::endl
284+
<< closestInsidePosition << std::endl
285+
<< " index is: " << std::endl
286+
<< insideIndex << std::endl;
287+
288+
// Point F
289+
outsidePosition = Position(-1.0, 1.0);
290+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
291+
isInside = map.getIndex(closestInsidePosition, insideIndex);
292+
293+
expectedPosition = Position(-0.5, 0.5);
294+
expectedIndex = Index(19, 0);
295+
296+
// Check position.
297+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
298+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
299+
300+
// Check index.
301+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
302+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
303+
304+
// Check if index is inside.
305+
EXPECT_TRUE(isInside) << "position is: " << std::endl
306+
<< closestInsidePosition << std::endl
307+
<< " index is: " << std::endl
308+
<< insideIndex << std::endl;
309+
310+
// Point G
311+
outsidePosition = Position(-1.0, 0.0);
312+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
313+
isInside = map.getIndex(closestInsidePosition, insideIndex);
314+
315+
expectedPosition = Position(-0.5, 0.0);
316+
expectedIndex = Index(19, 10);
317+
318+
// Check position.
319+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
320+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
321+
322+
// Check index.
323+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
324+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
325+
326+
// Check if index is inside.
327+
EXPECT_TRUE(isInside) << "position is: " << std::endl
328+
<< closestInsidePosition << std::endl
329+
<< " index is: " << std::endl
330+
<< insideIndex << std::endl;
331+
332+
// Point H
333+
outsidePosition = Position(-1.0, -1.0);
334+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
335+
isInside = map.getIndex(closestInsidePosition, insideIndex);
336+
337+
expectedPosition = Position(-0.5, -0.5);
338+
expectedIndex = Index(19, 19);
339+
340+
// Check position.
341+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
342+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
343+
344+
// Check index.
345+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
346+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
347+
348+
// Check if index is inside.
349+
EXPECT_TRUE(isInside) << "position is: " << std::endl
350+
<< closestInsidePosition << std::endl
351+
<< " index is: " << std::endl
352+
<< insideIndex << std::endl;
353+
}
354+
151355
TEST(AddDataFrom, ExtendMapAligned)
152356
{
153357
GridMap map1, map2;

0 commit comments

Comments
 (0)