From 50297c33f58e8d938d69426d5d6a74c9cdba5799 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 5 Nov 2023 00:49:35 +0100 Subject: [PATCH] Impl NN interface in grid2d --- .../src/maps/COccupancyGridMap2D_common.cpp | 117 ++++++++++++++++++ .../src/maps/COccupancyGridMap2D_unittest.cpp | 80 +++++++++++- 2 files changed, 194 insertions(+), 3 deletions(-) diff --git a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp index 99aa0febf8..0f59d9b86e 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_common.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_common.cpp @@ -821,6 +821,67 @@ void COccupancyGridMap2D::nn_multiple_search( std::vector& results, std::vector& out_dists_sqr) const { + results.clear(); + results.reserve(N); + out_dists_sqr.clear(); + out_dists_sqr.reserve(N); + + int cx_query = x2idx(query.x), cy_query = y2idx(query.y); + + mrpt::saturate(cx_query, 0, getSizeX() - 1); + mrpt::saturate(cy_query, 0, getSizeY() - 1); + + const cellType thresholdCellValue = p2l(0.5f); + const float resolutionSqr = mrpt::square(m_resolution); + + for (int searchRadiusInCells = 0; results.size() < N && + searchRadiusInCells < std::max(getSizeX(), getSizeY()); + searchRadiusInCells++) + { + int cx0 = cx_query - searchRadiusInCells; + int cx1 = cx_query + searchRadiusInCells; + int cy0 = cy_query - searchRadiusInCells; + int cy1 = cy_query + searchRadiusInCells; + + mrpt::saturate(cx0, 0, getSizeX() - 1); + mrpt::saturate(cy0, 0, getSizeY() - 1); + mrpt::saturate(cx1, 0, getSizeX() - 1); + mrpt::saturate(cy1, 0, getSizeY() - 1); + + std::map> dists2cells; + + auto lambdaAddCell = [&dists2cells, cx_query, cy_query]( + int cx, int cy) { + int distSqr = + mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query); + dists2cells[distSqr] = {cx, cy}; + }; + + for (int cx = cx0; cx <= cx1; cx++) + { + if (int cy = cy0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cy = cy1; + cy1 != cy0 && m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + for (int cy = cy0 + 1; cy < cy1; cy++) + { + if (int cx = cx0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cx = cx1; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + + // Add the top best "N" neighbors: + for (auto it = dists2cells.begin(); + it != dists2cells.end() && results.size() < N; ++it) + { + out_dists_sqr.push_back(it->first * resolutionSqr); + results.push_back( + {idx2x(it->second.first), idx2y(it->second.second)}); + } + } } void COccupancyGridMap2D::nn_radius_search( @@ -828,4 +889,60 @@ void COccupancyGridMap2D::nn_radius_search( std::vector& results, std::vector& out_dists_sqr) const { + results.clear(); + out_dists_sqr.clear(); + + if (search_radius_sqr == 0) return; + + int cx_query = x2idx(query.x), cy_query = y2idx(query.y); + + mrpt::saturate(cx_query, 0, getSizeX() - 1); + mrpt::saturate(cy_query, 0, getSizeY() - 1); + + const cellType thresholdCellValue = p2l(0.5f); + const float resolutionSqr = mrpt::square(m_resolution); + const int maxSearchRadiusInCells = static_cast( + std::ceil(std::sqrt(search_radius_sqr) / m_resolution)); + const int maxSearchRadiusSqrInCells = mrpt::square(maxSearchRadiusInCells); + + for (int searchRadiusInCells = 0; + searchRadiusInCells <= maxSearchRadiusInCells; searchRadiusInCells++) + { + int cx0 = cx_query - searchRadiusInCells; + int cx1 = cx_query + searchRadiusInCells; + int cy0 = cy_query - searchRadiusInCells; + int cy1 = cy_query + searchRadiusInCells; + + mrpt::saturate(cx0, 0, getSizeX() - 1); + mrpt::saturate(cy0, 0, getSizeY() - 1); + mrpt::saturate(cx1, 0, getSizeX() - 1); + mrpt::saturate(cy1, 0, getSizeY() - 1); + + auto lambdaAddCell = [maxSearchRadiusSqrInCells, cx_query, cy_query, + &out_dists_sqr, &results, resolutionSqr, + this](int cx, int cy) { + int distSqr = + mrpt::square(cx - cx_query) + mrpt::square(cy - cy_query); + if (distSqr > maxSearchRadiusSqrInCells) return; + + out_dists_sqr.push_back(distSqr * resolutionSqr); + results.push_back({idx2x(cx), idx2y(cy)}); + }; + + for (int cx = cx0; cx <= cx1; cx++) + { + if (int cy = cy0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cy = cy1; + cy1 != cy0 && m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + for (int cy = cy0 + 1; cy < cy1; cy++) + { + if (int cx = cx0; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + if (int cx = cx1; m_map[cx + cy * m_size_x] < thresholdCellValue) + lambdaAddCell(cx, cy); + } + } } diff --git a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp index 6159506c54..a57b69e6a0 100644 --- a/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp +++ b/libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp @@ -40,15 +40,89 @@ TEST(COccupancyGridMap2DTests, insert2DScan) TEST(COccupancyGridMap2DTests, NearestNeighborsCapable) { // low freeness=occupied - const float occupied = 0.2f; + const float occupied = 0.2f, resolution = 0.10f; const std::vector occupiedPoints = { {1.0f, 2.0f}, {1.1f, 2.0f}, {1.2f, 2.0f}, {2.0f, 3.0f}}; - COccupancyGridMap2D grid(-10.0f, 10.0f, -10.0f, 10.0f, 0.10f); + COccupancyGridMap2D grid(-10.0f, 10.0f, -10.0f, 10.0f, resolution); for (const auto& pt : occupiedPoints) grid.setCell(grid.x2idx(pt.x), grid.x2idx(pt.y), occupied); - MRPT_TODO("continue"); + mrpt::maps::NearestNeighborsCapable& nn = + dynamic_cast(grid); + + { + mrpt::math::TPoint2Df result; + float out_dist_sqr = 0; + bool found = nn.nn_single_search({0.90f, 1.95f}, result, out_dist_sqr); + EXPECT_TRUE(found); + EXPECT_NEAR(result.x, 1.0f, resolution); + EXPECT_NEAR(result.y, 2.0f, resolution); + EXPECT_NEAR(std::sqrt(out_dist_sqr), 0.15f, resolution); + } + { + mrpt::math::TPoint2Df result; + float out_dist_sqr = 0; + bool found = nn.nn_single_search({-4.0f, 2.1f}, result, out_dist_sqr); + EXPECT_TRUE(found); + EXPECT_NEAR(result.x, 1.0f, resolution); + EXPECT_NEAR(result.y, 2.0f, resolution); + EXPECT_NEAR(std::sqrt(out_dist_sqr), 5.0f, resolution); + } + + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_multiple_search({-2.0f, 5.0f}, 2, results, out_dists_sqr); + + EXPECT_EQ(results.size(), 2UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + + EXPECT_NEAR(results.at(0).x, 1.0f, resolution); + EXPECT_NEAR(results.at(0).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(0)), + std::hypot(-2.0f - 1.0f, 5.0f - 2.0f), resolution); + + EXPECT_NEAR(results.at(1).x, 1.1f, resolution); + EXPECT_NEAR(results.at(1).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(1)), + std::hypot(-2.0f - 1.1f, 5.0f - 2.0f), resolution); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {-2.0f, 5.0f}, mrpt::square(10.0f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), occupiedPoints.size()); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + + EXPECT_NEAR(results.at(0).x, 1.0f, resolution); + EXPECT_NEAR(results.at(0).y, 2.0f, resolution); + EXPECT_NEAR( + std::sqrt(out_dists_sqr.at(0)), + std::hypot(-2.0f - 1.0f, 5.0f - 2.0f), resolution); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {0.9f, 1.9f}, mrpt::square(1.0f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), 3UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + } + { + std::vector results; + std::vector out_dists_sqr; + nn.nn_radius_search( + {0.5f, 1.5f}, mrpt::square(0.5f), results, out_dists_sqr); + + EXPECT_EQ(results.size(), 0UL); + EXPECT_EQ(out_dists_sqr.size(), results.size()); + } } // We need OPENCV to read the image.