diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index cbee49df..224da85a 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -819,12 +819,13 @@ Position GridMap::getClosestPositionInMap(const Position & position) const const double maxY = bottomLeftCorner.y(); const double minY = bottomRightCorner.y(); - // Clip to box constraints. - positionInMap.x() = std::fmin(positionInMap.x(), maxX); - positionInMap.y() = std::fmin(positionInMap.y(), maxY); + // Clip to box constraints and correct for indexing precision. + // Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...). + positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits::epsilon()); + positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits::epsilon()); - positionInMap.x() = std::fmax(positionInMap.x(), minX); - positionInMap.y() = std::fmax(positionInMap.y(), minY); + positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits::epsilon()); + positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits::epsilon()); return positionInMap; } diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp index 14162380..e621c0b1 100644 --- a/grid_map_core/test/GridMapTest.cpp +++ b/grid_map_core/test/GridMapTest.cpp @@ -154,6 +154,210 @@ TEST(GridMap, ClipToMap) EXPECT_TRUE(map.isInside(clippedPositionOutMap)); } + + +TEST(GridMap, ClipToMap2) +{ + GridMap map({"types"}); + map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0)); + + // Test 8 points outside of map. + /* + * A B C + * +---+ + * | | X + * D| |E ^ + * | | | + * +---+ Y<--+ + * F G H + * + * Note: Position to index alignment is an half open interval. + * An example position of 0.5 is assigned to the upper index. + * The interval in the current example is: + * Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...) + * Index: 8 9 10 11 + */ + + Index insideIndex; + Position outsidePosition; + + // Point A + outsidePosition = Position(1.0, 1.0); + auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + bool isInside = map.getIndex(closestInsidePosition, insideIndex); + + auto expectedPosition = Position(0.5, 0.5); + auto expectedIndex = Index(0, 0); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point B + outsidePosition = Position(1.0, 0.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(0.5, 0.0); + expectedIndex = Index(0, 10); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point C + outsidePosition = Position(1.0, -1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(0.5, -0.5); + expectedIndex = Index(0, 19); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point D + outsidePosition = Position(0.0, 1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(0.0, 0.5); + expectedIndex = Index(10, 0); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point E + outsidePosition = Position(0.0, -1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(0.0, -0.5); + expectedIndex = Index(10, 19); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point F + outsidePosition = Position(-1.0, 1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(-0.5, 0.5); + expectedIndex = Index(19, 0); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point G + outsidePosition = Position(-1.0, 0.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(-0.5, 0.0); + expectedIndex = Index(19, 10); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; + + // Point H + outsidePosition = Position(-1.0, -1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = Position(-0.5, -0.5); + expectedIndex = Index(19, 19); + + // Check position. + EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x()); + EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y()); + + // Check index. + EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition; + EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition; + + // Check if index is inside. + EXPECT_TRUE(isInside) << "position is: " << std::endl + << closestInsidePosition << std::endl + << " index is: " << std::endl + << insideIndex << std::endl; +} + TEST(AddDataFrom, ExtendMapAligned) { grid_map::GridMap map1, map2;