diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index cbee49dff..c65687325 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -819,12 +819,14 @@ 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); - - positionInMap.x() = std::fmax(positionInMap.x(), minX); - positionInMap.y() = std::fmax(positionInMap.y(), minY); + // 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 + 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 141623805..ccc5261de 100644 --- a/grid_map_core/test/GridMapTest.cpp +++ b/grid_map_core/test/GridMapTest.cpp @@ -154,6 +154,208 @@ TEST(GridMap, ClipToMap) EXPECT_TRUE(map.isInside(clippedPositionOutMap)); } +TEST(GridMap, ClipToMap2) +{ + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(1.0, 1.0), 0.05, grid_map::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 + */ + + grid_map::Index insideIndex; + grid_map::Position outsidePosition; + + // Point A + outsidePosition = grid_map::Position(1.0, 1.0); + auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + bool isInside = map.getIndex(closestInsidePosition, insideIndex); + + auto expectedPosition = grid_map::Position(0.5, 0.5); + auto expectedIndex = grid_map::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 = grid_map::Position(1.0, 0.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(0.5, 0.0); + expectedIndex = grid_map::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 = grid_map::Position(1.0, -1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(0.5, -0.5); + expectedIndex = grid_map::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 = grid_map::Position(0.0, 1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(0.0, 0.5); + expectedIndex = grid_map::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 = grid_map::Position(0.0, -1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(0.0, -0.5); + expectedIndex = grid_map::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 = grid_map::Position(-1.0, 1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(-0.5, 0.5); + expectedIndex = grid_map::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 = grid_map::Position(-1.0, 0.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(-0.5, 0.0); + expectedIndex = grid_map::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 = grid_map::Position(-1.0, -1.0); + closestInsidePosition = map.getClosestPositionInMap(outsidePosition); + isInside = map.getIndex(closestInsidePosition, insideIndex); + + expectedPosition = grid_map::Position(-0.5, -0.5); + expectedIndex = grid_map::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;