From 3be5358caa4c03ac8187ba87112e2a62f223a080 Mon Sep 17 00:00:00 2001 From: Maximilian Wulf Date: Wed, 16 Dec 2020 11:00:02 +0000 Subject: [PATCH 1/2] Merge branch 'fix/clip_points_to_map' into 'master' [grid_map] Fix getClosestPositionInMap GitOrigin-RevId: b7d32e76cec97e5c8d50b06aa17569ef085de0db --- grid_map_core/src/GridMap.cpp | 11 +- grid_map_core/test/GridMapTest.cpp | 204 +++++++++++++++++++++++++++++ 2 files changed, 210 insertions(+), 5 deletions(-) diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index cbee49dff..224da85a5 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 141623805..e621c0b17 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; From 9be0d06fadff12dfa5d414778d66b5d5fba26f5f Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Tue, 3 Dec 2024 11:36:42 +0100 Subject: [PATCH 2/2] Fix cpplint and uncrustify --- grid_map_core/src/GridMap.cpp | 3 +- grid_map_core/test/GridMapTest.cpp | 108 ++++++++++++++--------------- 2 files changed, 55 insertions(+), 56 deletions(-) diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index 224da85a5..c65687325 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -820,7 +820,8 @@ Position GridMap::getClosestPositionInMap(const Position & position) const const double minY = bottomRightCorner.y(); // 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. [...). + // 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()); diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp index e621c0b17..ccc5261de 100644 --- a/grid_map_core/test/GridMapTest.cpp +++ b/grid_map_core/test/GridMapTest.cpp @@ -154,12 +154,10 @@ 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)); + 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. /* @@ -173,187 +171,187 @@ TEST(GridMap, ClipToMap2) * * 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: + * 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; + grid_map::Index insideIndex; + grid_map::Position outsidePosition; // Point A - outsidePosition = Position(1.0, 1.0); + outsidePosition = grid_map::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); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point B - outsidePosition = Position(1.0, 0.0); + outsidePosition = grid_map::Position(1.0, 0.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.5, 0.0); - expectedIndex = Index(0, 10); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point C - outsidePosition = Position(1.0, -1.0); + outsidePosition = grid_map::Position(1.0, -1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.5, -0.5); - expectedIndex = Index(0, 19); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point D - outsidePosition = Position(0.0, 1.0); + outsidePosition = grid_map::Position(0.0, 1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.0, 0.5); - expectedIndex = Index(10, 0); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point E - outsidePosition = Position(0.0, -1.0); + outsidePosition = grid_map::Position(0.0, -1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(0.0, -0.5); - expectedIndex = Index(10, 19); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point F - outsidePosition = Position(-1.0, 1.0); + outsidePosition = grid_map::Position(-1.0, 1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(-0.5, 0.5); - expectedIndex = Index(19, 0); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point G - outsidePosition = Position(-1.0, 0.0); + outsidePosition = grid_map::Position(-1.0, 0.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(-0.5, 0.0); - expectedIndex = Index(19, 10); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; // Point H - outsidePosition = Position(-1.0, -1.0); + outsidePosition = grid_map::Position(-1.0, -1.0); closestInsidePosition = map.getClosestPositionInMap(outsidePosition); isInside = map.getIndex(closestInsidePosition, insideIndex); - expectedPosition = Position(-0.5, -0.5); - expectedIndex = Index(19, 19); + 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 + << closestInsidePosition << std::endl << " index is: " << std::endl << insideIndex << std::endl; }