Skip to content

Commit

Permalink
Impl NN interface in grid2d
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Nov 4, 2023
1 parent b6f6871 commit 50297c3
Show file tree
Hide file tree
Showing 2 changed files with 194 additions and 3 deletions.
117 changes: 117 additions & 0 deletions libs/maps/src/maps/COccupancyGridMap2D_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -821,11 +821,128 @@ void COccupancyGridMap2D::nn_multiple_search(
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& 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<int>(cx_query, 0, getSizeX() - 1);
mrpt::saturate<int>(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<int>(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<int>(cx0, 0, getSizeX() - 1);
mrpt::saturate<int>(cy0, 0, getSizeY() - 1);
mrpt::saturate<int>(cx1, 0, getSizeX() - 1);
mrpt::saturate<int>(cy1, 0, getSizeY() - 1);

std::map<int, std::pair<int, int>> 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(
const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
std::vector<mrpt::math::TPoint2Df>& results,
std::vector<float>& 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<int>(cx_query, 0, getSizeX() - 1);
mrpt::saturate<int>(cy_query, 0, getSizeY() - 1);

const cellType thresholdCellValue = p2l(0.5f);
const float resolutionSqr = mrpt::square(m_resolution);
const int maxSearchRadiusInCells = static_cast<int>(
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<int>(cx0, 0, getSizeX() - 1);
mrpt::saturate<int>(cy0, 0, getSizeY() - 1);
mrpt::saturate<int>(cx1, 0, getSizeX() - 1);
mrpt::saturate<int>(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);
}
}
}
80 changes: 77 additions & 3 deletions libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<mrpt::math::TPoint2Df> 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<mrpt::maps::NearestNeighborsCapable&>(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<mrpt::math::TPoint2Df> results;
std::vector<float> 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<mrpt::math::TPoint2Df> results;
std::vector<float> 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<mrpt::math::TPoint2Df> results;
std::vector<float> 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<mrpt::math::TPoint2Df> results;
std::vector<float> 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.
Expand Down

0 comments on commit 50297c3

Please sign in to comment.