From 048153c1b8ffb7c7e635199e2e0782941b4668a2 Mon Sep 17 00:00:00 2001 From: Maximilian Wulf Date: Thu, 13 Aug 2020 16:54:01 +0000 Subject: [PATCH 1/2] Merge branch 'fix/get_transformed_map' into 'master' [grid_map] Fix for getIndex() function in cases of suboptimal position arguments Closes #UNKNOWN GitOrigin-RevId: 740443e4f3aad9fe495a565b557b3e46cd22e417 --- grid_map_core/src/GridMapMath.cpp | 3 +- grid_map_core/test/GridMapMathTest.cpp | 111 +++++++++++++++++++++++++ 2 files changed, 112 insertions(+), 2 deletions(-) diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp index b12162c42..cb85bd6d1 100644 --- a/grid_map_core/src/GridMapMath.cpp +++ b/grid_map_core/src/GridMapMath.cpp @@ -155,8 +155,7 @@ bool getIndexFromPosition( internal::getVectorToOrigin(offset, mapLength); Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix(); index = internal::getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex); - if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) {return false;} - return true; + return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, bufferSize); } bool checkIfPositionWithinMap( diff --git a/grid_map_core/test/GridMapMathTest.cpp b/grid_map_core/test/GridMapMathTest.cpp index be266af00..0f474ec40 100644 --- a/grid_map_core/test/GridMapMathTest.cpp +++ b/grid_map_core/test/GridMapMathTest.cpp @@ -286,10 +286,121 @@ TEST(checkIfPositionWithinMap, EdgeCases) grid_map::Length mapLength(2.0, 3.0); grid_map::Position mapPosition(0.0, 0.0); + /* + * + * A (is inside) B (is not inside) + * +-----------------------+ + * | | + * | | + * | X | + * | ^ | + * | | | + * | | | + * | <-----+ | + * | Y | + * | | + * | | + * | | + * +-----------------------+ + * C (is not inside) D (is not inside) + * + * Resulting coordinates are: + * A: (1.0, 1.5) + * B: (1.0, -1.5) + * C: (-1.0, 1.5) + * D: (-1.0, -1.5) + * + */ + + // Noise around A. + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0, 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0 + DBL_EPSILON, 1.5), mapLength, + mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0 - DBL_EPSILON, 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0, 1.5 + DBL_EPSILON), mapLength, + mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0, 1.5 - DBL_EPSILON), mapLength, + mapPosition)); + + // Noise around B. EXPECT_FALSE( grid_map::checkIfPositionWithinMap( grid_map::Position(1.0, -1.5), mapLength, mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0 + DBL_EPSILON, - 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0 - DBL_EPSILON, - 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0, - 1.5 + DBL_EPSILON), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0, - 1.5 - DBL_EPSILON), mapLength, + mapPosition)); + + // Noise around C. + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, 1.5), mapLength, + mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0 + DBL_EPSILON, 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0 - DBL_EPSILON, 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, 1.5 + DBL_EPSILON), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, 1.5 - DBL_EPSILON), mapLength, + mapPosition)); + + // Noise around D. + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, -1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0 + DBL_EPSILON, -1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0 - DBL_EPSILON, -1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, -1.5 + DBL_EPSILON), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, -1.5 - DBL_EPSILON), mapLength, + mapPosition)); + + // Extra tests. EXPECT_FALSE( grid_map::checkIfPositionWithinMap( grid_map::Position(-1.0, 1.5), mapLength, From 604b59efef671a654765f653db8bd49bc98a75f7 Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Tue, 3 Dec 2024 12:46:23 +0100 Subject: [PATCH 2/2] Fix cpplint and uncrustify --- grid_map_core/src/GridMapMath.cpp | 3 ++- grid_map_core/test/GridMapMathTest.cpp | 14 +++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp index cb85bd6d1..d4a72dc57 100644 --- a/grid_map_core/src/GridMapMath.cpp +++ b/grid_map_core/src/GridMapMath.cpp @@ -155,7 +155,8 @@ bool getIndexFromPosition( internal::getVectorToOrigin(offset, mapLength); Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix(); index = internal::getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex); - return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, bufferSize); + return checkIfPositionWithinMap(position, mapLength, mapPosition) && checkIfIndexInRange(index, + bufferSize); } bool checkIfPositionWithinMap( diff --git a/grid_map_core/test/GridMapMathTest.cpp b/grid_map_core/test/GridMapMathTest.cpp index 0f474ec40..97893dbf3 100644 --- a/grid_map_core/test/GridMapMathTest.cpp +++ b/grid_map_core/test/GridMapMathTest.cpp @@ -287,7 +287,7 @@ TEST(checkIfPositionWithinMap, EdgeCases) grid_map::Position mapPosition(0.0, 0.0); /* - * + * * A (is inside) B (is not inside) * +-----------------------+ * | | @@ -341,21 +341,21 @@ TEST(checkIfPositionWithinMap, EdgeCases) mapPosition)); EXPECT_FALSE( grid_map::checkIfPositionWithinMap( - grid_map::Position(1.0 + DBL_EPSILON, - 1.5), mapLength, + grid_map::Position(1.0 + DBL_EPSILON, -1.5), mapLength, mapPosition)); EXPECT_FALSE( grid_map::checkIfPositionWithinMap( - grid_map::Position(1.0 - DBL_EPSILON, - 1.5), mapLength, + grid_map::Position(1.0 - DBL_EPSILON, -1.5), mapLength, mapPosition)); EXPECT_FALSE( grid_map::checkIfPositionWithinMap( - grid_map::Position(1.0, - 1.5 + DBL_EPSILON), mapLength, + grid_map::Position(1.0, -1.5 + DBL_EPSILON), mapLength, mapPosition)); EXPECT_FALSE( grid_map::checkIfPositionWithinMap( - grid_map::Position(1.0, - 1.5 - DBL_EPSILON), mapLength, + grid_map::Position(1.0, -1.5 - DBL_EPSILON), mapLength, mapPosition)); - + // Noise around C. EXPECT_FALSE( grid_map::checkIfPositionWithinMap( @@ -400,7 +400,7 @@ TEST(checkIfPositionWithinMap, EdgeCases) grid_map::Position(-1.0, -1.5 - DBL_EPSILON), mapLength, mapPosition)); - // Extra tests. + // Extra tests. EXPECT_FALSE( grid_map::checkIfPositionWithinMap( grid_map::Position(-1.0, 1.5), mapLength,