From 622ca91380d00fa5c39270049b6715f936204bf9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Magnus=20G=C3=A4rtner?= Date: Fri, 23 Oct 2020 17:08:38 +0000 Subject: [PATCH 1/2] Merge branch 'feature/speedup_submap_iterator' into 'master' [grid_map_core ] Speedup Submap Iterator Closes #UNKNOWN GitOrigin-RevId: a3b89f8a478a8609179ab4d992119e4b51f07cb8 --- grid_map_core/src/GridMapMath.cpp | 19 +++- grid_map_core/test/SubmapIteratorTest.cpp | 101 ++++++++++++++++++++++ 2 files changed, 117 insertions(+), 3 deletions(-) diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp index b12162c42..5fa9c5734 100644 --- a/grid_map_core/src/GridMapMath.cpp +++ b/grid_map_core/src/GridMapMath.cpp @@ -242,9 +242,22 @@ void wrapIndexToRange(Index & index, const Size & bufferSize) void wrapIndexToRange(int & index, int bufferSize) { - index = index % bufferSize; - if (index < 0) { - index += bufferSize; + // Try shortcuts before resorting to the expensive modulo operation. + if (index < bufferSize) { + if (index >= 0) { // within the wanted range + return; + } else if (index >= -bufferSize) { // Index is below range, but not more than one span of the range. + index +=bufferSize; + return; + } else { // Index is largely below range. + index = index % bufferSize; + index += bufferSize; + } + } else if (index < bufferSize*2) { // Index is above range, but not more than one span of the range. + index -= bufferSize; + return; + } else { // Index is largely above range. + index = index % bufferSize; } } diff --git a/grid_map_core/test/SubmapIteratorTest.cpp b/grid_map_core/test/SubmapIteratorTest.cpp index bfa594df8..70d3c7cd9 100644 --- a/grid_map_core/test/SubmapIteratorTest.cpp +++ b/grid_map_core/test/SubmapIteratorTest.cpp @@ -165,3 +165,104 @@ TEST(SubmapIterator, CircularBuffer) EXPECT_EQ(1, iterator.getSubmapIndex()(0)); EXPECT_EQ(3, iterator.getSubmapIndex()(1)); } + +/** + * The submap should contain the same elements as before even after moving the underlying map. + * + * +----------------------------+ + * | | + * | | + * +----------------------------+ | | + * |0 0 0 0 0 0 0 0 0 0| | 0 0 0 0 0 0 0 0| + * | +----+ | | +----+ | + * Submap |1 1 |1 1| 1 1 1 1 1 1| | 1 1 |1 1| 1 1 1 1| + * +------> | | | | | | | + * |2 2 |2 2| 2 2 2 2 2 2| | 2 2 |2 2| 2 2 2 2| + * | +----+ | | +----+ | + * |3 3 3 3 3 3 3 3 3 3| Move | 3 3 3 3 3 3 3 3| + * | | | | + * |4 4 4 4 4 4 4 4 4 4| +---------> | 4 4 4 4 4 4 4 4| + * | | | | + * |5 5 5 5 5 5 5 5 5 5| | 5 5 5 5 5 5 5 5| + * | | | | + * |6 6 6 6 6 6 6 6 6 6| | 6 6 6 6 6 6 6 6| + * | | | | + * |7 7 7 7 7 7 7 7 7 7| | 7 7 7 7 7 7 7 7| + * | | +----------------------------+ + * |8 8 8 8 8 8 8 8 8 8| + * | | + * |9 9 9 9 9 9 9 9 9 9| + * +----------------------------+ + */ +TEST(SubmapIterator, InterleavedExecutionWithMove) { + grid_map::Index submapTopLeftIndex(3, 1); + grid_map::Size submapSize(2, 2); + + grid_map::GridMap map({"layer"}); + + map.setGeometry(grid_map::Length(10, 10), 1.0, grid_map::Position(0.0, 0.0)); // bufferSize(8, 5) + + auto& layer = map.get("layer"); + + // Initialize the layer as sketched. + for (size_t colIndex = 0; colIndex < layer.cols(); colIndex++) { + layer.col(colIndex).setConstant(colIndex); + } + + std::cout << "(4,7) contains " << map.at("layer", {4, 7}) << std::endl; + // Instantiate the submap iterator as sketched. + grid_map::SubmapIterator iterator(map, submapTopLeftIndex, submapSize); + + // check that the submap iterator returns {1,1,2,2} + auto checkCorrectValues = [](std::array given) { + int countOnes = 0, countTwos = 0; + for (auto& value : given) { + if (std::abs(value - 1.0) < 1e-6) { + countOnes++; + } else if (std::abs(value - 2.0) < 1e-6) { + countTwos++; + } else { + FAIL() << "Submap iterator returned unexpected value."; + } + } + EXPECT_EQ(countOnes, 2); + EXPECT_EQ(countTwos, 2); + }; + + std::array returnedSequence; + returnedSequence.fill(0); + + for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) { + returnedSequence.at(submapIndex) = map.at("layer", *iterator); + ++iterator; + } + + checkCorrectValues(returnedSequence); + + // Reset the iterator and now check that it still returns the same sequence when we move the map interleaved with iterating. + iterator = grid_map::SubmapIterator(map, submapTopLeftIndex, submapSize); + returnedSequence.fill(0); + for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) { + if (submapIndex == 2) { + // Now move the map as depicted. + map.move(grid_map::Position(2.0, 2.0)); + } + returnedSequence.at(submapIndex) = map.at("layer", *iterator); + ++iterator; + } + checkCorrectValues(returnedSequence); + + // TODO (mwulf, mgaertner): This behavior is not yet implemented: + // + // // Reset the iterator and now check that the iterator throws? if the submap moved out of range. + // iterator = SubmapIterator(map, submapTopLeftIndex, submapSize); + // + // EXPECT_ANY_THROW(for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) { + // if (submapIndex == 2) { + // // Now move the map so that the submap gets out of range. + // map.move(Position(20.0, 20.0)); + // } + // returnedSequence.at(submapIndex) = map.at("layer", *iterator); + // ++iterator; + // }); +} \ No newline at end of file From 0c87086849d9f90041d284ecd3e5b994c40ebdac Mon Sep 17 00:00:00 2001 From: Khaled Jalloul Date: Tue, 3 Dec 2024 13:16:38 +0100 Subject: [PATCH 2/2] Fix cpplint and uncrustify --- grid_map_core/src/GridMapMath.cpp | 14 +++++---- grid_map_core/test/SubmapIteratorTest.cpp | 38 ++++++++++++----------- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp index 5fa9c5734..ed63bc687 100644 --- a/grid_map_core/src/GridMapMath.cpp +++ b/grid_map_core/src/GridMapMath.cpp @@ -244,19 +244,21 @@ void wrapIndexToRange(int & index, int bufferSize) { // Try shortcuts before resorting to the expensive modulo operation. if (index < bufferSize) { - if (index >= 0) { // within the wanted range + if (index >= 0) { // within the wanted range return; - } else if (index >= -bufferSize) { // Index is below range, but not more than one span of the range. - index +=bufferSize; + // Index is below range, but not more than one span of the range. + } else if (index >= -bufferSize) { + index += bufferSize; return; - } else { // Index is largely below range. + } else { // Index is largely below range. index = index % bufferSize; index += bufferSize; } - } else if (index < bufferSize*2) { // Index is above range, but not more than one span of the range. + // Index is above range, but not more than one span of the range. + } else if (index < bufferSize * 2) { index -= bufferSize; return; - } else { // Index is largely above range. + } else { // Index is largely above range. index = index % bufferSize; } } diff --git a/grid_map_core/test/SubmapIteratorTest.cpp b/grid_map_core/test/SubmapIteratorTest.cpp index 70d3c7cd9..75d827473 100644 --- a/grid_map_core/test/SubmapIteratorTest.cpp +++ b/grid_map_core/test/SubmapIteratorTest.cpp @@ -202,10 +202,10 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) { map.setGeometry(grid_map::Length(10, 10), 1.0, grid_map::Position(0.0, 0.0)); // bufferSize(8, 5) - auto& layer = map.get("layer"); + auto & layer = map.get("layer"); // Initialize the layer as sketched. - for (size_t colIndex = 0; colIndex < layer.cols(); colIndex++) { + for (size_t colIndex = 0; colIndex < static_cast(layer.cols()); colIndex++) { layer.col(colIndex).setConstant(colIndex); } @@ -215,19 +215,19 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) { // check that the submap iterator returns {1,1,2,2} auto checkCorrectValues = [](std::array given) { - int countOnes = 0, countTwos = 0; - for (auto& value : given) { - if (std::abs(value - 1.0) < 1e-6) { - countOnes++; - } else if (std::abs(value - 2.0) < 1e-6) { - countTwos++; - } else { - FAIL() << "Submap iterator returned unexpected value."; + int countOnes = 0, countTwos = 0; + for (auto & value : given) { + if (std::abs(value - 1.0) < 1e-6) { + countOnes++; + } else if (std::abs(value - 2.0) < 1e-6) { + countTwos++; + } else { + FAIL() << "Submap iterator returned unexpected value."; + } } - } - EXPECT_EQ(countOnes, 2); - EXPECT_EQ(countTwos, 2); - }; + EXPECT_EQ(countOnes, 2); + EXPECT_EQ(countTwos, 2); + }; std::array returnedSequence; returnedSequence.fill(0); @@ -239,7 +239,8 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) { checkCorrectValues(returnedSequence); - // Reset the iterator and now check that it still returns the same sequence when we move the map interleaved with iterating. + // Reset the iterator and now check that it still returns the same sequence + // when we move the map interleaved with iterating. iterator = grid_map::SubmapIterator(map, submapTopLeftIndex, submapSize); returnedSequence.fill(0); for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) { @@ -252,9 +253,10 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) { } checkCorrectValues(returnedSequence); - // TODO (mwulf, mgaertner): This behavior is not yet implemented: + // TODO(mwulf, mgaertner): This behavior is not yet implemented: // - // // Reset the iterator and now check that the iterator throws? if the submap moved out of range. + // // Reset the iterator and now check that the iterator throws? + // // if the submap moved out of range. // iterator = SubmapIterator(map, submapTopLeftIndex, submapSize); // // EXPECT_ANY_THROW(for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) { @@ -265,4 +267,4 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) { // returnedSequence.at(submapIndex) = map.at("layer", *iterator); // ++iterator; // }); -} \ No newline at end of file +}