From b22d19be3afdfb30efe9e06eb134ff6bf4a27ced Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 00:30:31 +0200 Subject: [PATCH] Add NavMap::distance_field MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Distance (3D) from each NavCel centroid to the nearest seed: given cells and, optionally, the open edges of the mesh as real segments. The nearest seed is propagated over adjacency and the distance is measured straight to it, so it never jumps between disconnected surfaces such as stacked floors. Cells beyond max_distance get +inf. It is the representation primitive behind inflation, likelihood-field localization and clearance-aware planning. Fixes #32 Signed-off-by: Francisco Martín Rico --- navmap_core/include/navmap_core/NavMap.hpp | 17 ++ navmap_core/src/navmap_core/NavMap.cpp | 75 ++++++ navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_distance_field.cpp | 255 +++++++++++++++++++++ 4 files changed, 352 insertions(+) create mode 100644 navmap_core/tests/test_distance_field.cpp diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 8df6396..5c611c6 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -699,6 +699,23 @@ class NavMap /// \brief Return up to 3 neighbor cell ids (skips invalid entries). std::vector navcel_neighbors(NavCelId cid) const; + /** + * \brief Distance (m, 3D) from each NavCel centroid to the nearest seed. + * + * Seeds are the centroids of \p seed_cells and, if \p from_boundary, the open edges of the + * mesh (edges without a neighbor: walls, drops, unmapped space). The nearest seed is + * propagated over adjacency and the distance is measured straight to it, so it never jumps + * between disconnected surfaces (e.g. stacked floors). + * + * \param seed_cells Seed NavCels (out-of-range ids are ignored). + * \param from_boundary Also seed the open edges. + * \param max_distance Stop beyond this distance; farther or unreachable cells get +inf. + * \return One distance per NavCel. + */ + std::vector distance_field( + const std::vector & seed_cells, bool from_boundary, + float max_distance = std::numeric_limits::infinity()) const; + /// \brief Value returned by \ref surface_of() for a NavCel in no surface. static constexpr std::size_t kNoSurface = std::numeric_limits::max(); diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index cad726c..85efaac 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -16,6 +16,7 @@ #include "navmap_core/NavMap.hpp" #include +#include #include #include #include @@ -1098,6 +1099,80 @@ bool NavMap::locate_navcel( return true; } +// ---- Distance field ---- + +namespace +{ +inline float point_segment_distance(const Vec3 & p, const Vec3 & a, const Vec3 & b) +{ + const Vec3 ab = b - a; + const float len2 = ab.squaredNorm(); + const float t = len2 > 0.0f ? std::clamp((p - a).dot(ab) / len2, 0.0f, 1.0f) : 0.0f; + return (p - (a + t * ab)).norm(); +} +} // namespace + +std::vector NavMap::distance_field( + const std::vector & seed_cells, bool from_boundary, float max_distance) const +{ + constexpr float kInf = std::numeric_limits::infinity(); + constexpr auto kNone = std::numeric_limits::max(); + const std::size_t n = navcels.size(); + std::vector dist(n, kInf); + if (n == 0) {return dist;} + + std::vector centroid(n); + for (NavCelId c = 0; c < n; ++c) { + centroid[c] = navcel_centroid(c); + } + + // A seed is a point (a == b) or a segment. + struct Seed {Vec3 a, b;}; + std::vector seeds; + std::vector nearest(n, std::numeric_limits::max()); + using Item = std::pair; + std::priority_queue, std::greater> open; + + auto offer = [&](NavCelId c, std::uint32_t seed, float d) { + if (d < dist[c] && d <= max_distance) { + dist[c] = d; + nearest[c] = seed; + open.push({d, c}); + } + }; + auto distance_to = [&](NavCelId c, std::uint32_t seed) { + return point_segment_distance(centroid[c], seeds[seed].a, seeds[seed].b); + }; + + for (NavCelId c : seed_cells) { + if (c >= n) {continue;} + seeds.push_back({centroid[c], centroid[c]}); + offer(c, static_cast(seeds.size() - 1), 0.0f); + } + if (from_boundary) { + for (NavCelId c = 0; c < n; ++c) { + for (int e = 0; e < 3; ++e) { + if (navcels[c].neighbor[e] != kNone) {continue;} + seeds.push_back({positions.at(navcels[c].v[e]), positions.at(navcels[c].v[(e + 1) % 3])}); + const auto seed = static_cast(seeds.size() - 1); + offer(c, seed, distance_to(c, seed)); + } + } + } + + while (!open.empty()) { + const auto [d, u] = open.top(); + open.pop(); + if (d > dist[u]) {continue;} + for (int e = 0; e < 3; ++e) { + const NavCelId v = navcels[u].neighbor[e]; + if (v == kNone || v >= n) {continue;} + offer(v, nearest[u], distance_to(v, nearest[u])); + } + } + return dist; +} + // ---- Layers ---- bool NavMap::has_layer(const std::string & name) const { diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index 9c45368..e87418a 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -47,3 +47,8 @@ ament_add_gtest(test_adjacency test_adjacency.cpp) target_link_libraries(test_adjacency ${PROJECT_NAME} ) + +ament_add_gtest(test_distance_field test_distance_field.cpp) +target_link_libraries(test_distance_field + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_distance_field.cpp b/navmap_core/tests/test_distance_field.cpp new file mode 100644 index 0000000..497a9c0 --- /dev/null +++ b/navmap_core/tests/test_distance_field.cpp @@ -0,0 +1,255 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +constexpr float kInf = std::numeric_limits::infinity(); + +// nx x ny cells of `res` m at z = f(x, y), skipping cells where skip(i, j); returns the surface. +std::size_t add_grid( + NavMap & nm, int nx, int ny, float res, const std::function & f, + const std::function & skip = nullptr) +{ + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(nm.positions.size()); + for (int j = 0; j <= ny; ++j) { + for (int i = 0; i <= nx; ++i) { + nm.add_vertex({i * res, j * res, f(i * res, j * res)}); + } + } + auto v = [&](int i, int j) {return base + static_cast(j * (nx + 1) + i);}; + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + if (skip && skip(i, j)) {continue;} + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j), v(i + 1, j + 1))); + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j + 1), v(i, j + 1))); + } + } + return s; +} + +float point_segment(const Eigen::Vector3f & p, const Eigen::Vector3f & a, const Eigen::Vector3f & b) +{ + const Eigen::Vector3f ab = b - a; + const float t = std::clamp((p - a).dot(ab) / ab.squaredNorm(), 0.0f, 1.0f); + return (p - (a + t * ab)).norm(); +} + +// Exact nearest distance over all seeds (no connectivity). +std::vector brute(const NavMap & nm, const std::vector & seeds, bool boundary) +{ + std::vector d(nm.navcels.size(), kInf); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.navcel_centroid(c); + for (NavCelId s : seeds) { + d[c] = std::min(d[c], (p - nm.navcel_centroid(s)).norm()); + } + if (!boundary) {continue;} + for (NavCelId b = 0; b < nm.navcels.size(); ++b) { + for (int e = 0; e < 3; ++e) { + if (nm.navcels[b].neighbor[e] != std::numeric_limits::max()) {continue;} + d[c] = std::min( + d[c], point_segment( + p, nm.positions.at(nm.navcels[b].v[e]), nm.positions.at(nm.navcels[b].v[(e + 1) % 3]))); + } + } + } + return d; +} + +NavCelId cell_near(const NavMap & nm, float x, float y, float z) +{ + std::size_t s; NavCelId c; Eigen::Vector3f bary, hit; + EXPECT_TRUE(nm.locate_navcel({x, y, z}, s, c, bary, &hit)); + return c; +} + +auto flat(float h) {return [h](float, float) {return h;};} + +} // namespace + +TEST(NavMap_DistanceField, BoundaryOfARectangleIsExact) +{ + NavMap nm; + add_grid(nm, 20, 12, 0.5f, flat(0.0f)); + nm.rebuild_geometry_accels(); + const auto d = nm.distance_field({}, true); + const auto ref = brute(nm, {}, true); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.navcel_centroid(c); + const float expected = std::min({p.x(), 10.0f - p.x(), p.y(), 6.0f - p.y()}); + EXPECT_NEAR(d[c], expected, 1e-4f) << c; + EXPECT_NEAR(d[c], ref[c], 1e-4f) << c; + } +} + +TEST(NavMap_DistanceField, SeedCellsGiveStraightLineDistance) +{ + NavMap nm; + add_grid(nm, 20, 20, 0.5f, flat(0.0f)); + nm.rebuild_geometry_accels(); + const std::vector seeds{cell_near(nm, 2.1f, 2.2f, 0.0f), + cell_near(nm, 7.3f, 6.9f, 0.0f)}; + const auto d = nm.distance_field(seeds, false); + const auto ref = brute(nm, seeds, false); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_NEAR(d[c], ref[c], 1e-4f) << c; + } + for (NavCelId s : seeds) { + EXPECT_FLOAT_EQ(d[s], 0.0f); + } + + // Seeds and boundary together: the nearest of both. + const auto both = nm.distance_field(seeds, true); + const auto ref_both = brute(nm, seeds, true); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_NEAR(both[c], ref_both[c], 1e-4f) << c; + } +} + +TEST(NavMap_DistanceField, MaxDistanceCutsOff) +{ + NavMap nm; + add_grid(nm, 20, 20, 0.5f, flat(0.0f)); + nm.rebuild_geometry_accels(); + const NavCelId seed = cell_near(nm, 5.1f, 5.2f, 0.0f); + const auto full = nm.distance_field({seed}, false); + const auto cut = nm.distance_field({seed}, false, 2.0f); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (full[c] <= 2.0f) { + EXPECT_NEAR(cut[c], full[c], 1e-5f); + } else { + EXPECT_EQ(cut[c], kInf); + } + } +} + +TEST(NavMap_DistanceField, StackedFloorsDoNotSeeEachOther) +{ + NavMap nm; + add_grid(nm, 10, 10, 0.5f, flat(0.0f)); + add_grid(nm, 10, 10, 0.5f, flat(2.5f)); + nm.rebuild_geometry_accels(); + const NavCelId low = cell_near(nm, 2.6f, 2.6f, 0.0f); + const auto d = nm.distance_field({low}, false); + for (auto c : nm.surfaces[1].navcels) { + EXPECT_EQ(d[c], kInf); + } + for (auto c : nm.surfaces[0].navcels) { + EXPECT_LT(d[c], kInf); + } + + // Boundary of the upper floor only reaches the upper floor's own cells. + const NavCelId high = cell_near(nm, 2.6f, 2.6f, 2.5f); + const auto b = nm.distance_field({}, true); + EXPECT_NEAR(b[high], b[low], 1e-4f); // same geometry, same distance to its own border +} + +TEST(NavMap_DistanceField, SlopeDistanceIs3D) +{ + // 30 deg ramp along x: the distance to a seed grows with the slope length, not the XY one. + const float k = std::tan(30.0f * static_cast(M_PI) / 180.0f); + NavMap nm; + add_grid(nm, 20, 4, 0.5f, [k](float x, float) {return k * x;}); + nm.rebuild_geometry_accels(); + const NavCelId seed = cell_near(nm, 0.4f, 1.0f, 0.2f); + const auto d = nm.distance_field({seed}, false); + const auto ref = brute(nm, {seed}, false); + const NavCelId far = cell_near(nm, 8.4f, 1.0f, k * 8.4f); + EXPECT_NEAR(d[far], ref[far], 1e-4f); + const auto pf = nm.navcel_centroid(far), ps = nm.navcel_centroid(seed); + EXPECT_GT(d[far], (pf.head<2>() - ps.head<2>()).norm() * 1.1f); +} + +TEST(NavMap_DistanceField, InteriorHoleEdgesAreBoundary) +{ + NavMap nm; + auto hole = [](int i, int j) {return i >= 9 && i <= 10 && j >= 9 && j <= 10;}; + add_grid(nm, 20, 20, 0.5f, flat(0.0f), hole); + nm.rebuild_geometry_accels(); + const auto d = nm.distance_field({}, true); + const auto ref = brute(nm, {}, true); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_NEAR(d[c], ref[c], 1e-4f) << c; + } + // Next to the 1x1 m hole (x, y in [4.5, 5.5]). + EXPECT_LT(d[cell_near(nm, 4.2f, 5.0f, 0.0f)], 0.31f); +} + +TEST(NavMap_DistanceField, NeverBelowTheTrueDistanceOnIrregularTerrain) +{ + std::mt19937 rng(17); + std::uniform_real_distribution U(0.0f, 1.0f); + for (int m = 0; m < 4; ++m) { + const float a = 0.3f * U(rng), b = 0.3f * U(rng); + NavMap nm; + add_grid( + nm, 30, 25, 0.4f, [&](float x, float y) {return a * std::sin(x) + b * std::cos(0.7f * y);}, + [&](int, int) {return U(rng) < 0.05f;}); // scattered holes + nm.rebuild_geometry_accels(); + std::vector seeds; + for (int k = 0; k < 5; ++k) { + seeds.push_back(static_cast(U(rng) * static_cast(nm.navcels.size() - 1))); + } + const auto d = nm.distance_field(seeds, true); + const auto ref = brute(nm, seeds, true); + double sum = 0.0; + int n = 0; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (!std::isfinite(d[c])) {continue;} + EXPECT_GE(d[c], ref[c] - 1e-4f) << "map " << m << " cell " << c; + sum += d[c] - ref[c]; + ++n; + } + EXPECT_LT(sum / n, 0.02) << "map " << m; // propagation is close to the exact distance + } +} + +TEST(NavMap_DistanceField, DegenerateInputs) +{ + NavMap empty; + empty.rebuild_geometry_accels(); + EXPECT_TRUE(empty.distance_field({0, 5}, true).empty()); + + NavMap nm; + add_grid(nm, 4, 4, 0.5f, flat(0.0f)); + nm.rebuild_geometry_accels(); + for (float v : nm.distance_field({}, false)) { + EXPECT_EQ(v, kInf); + } + const auto d = nm.distance_field({9999}, false); // out of range: ignored + for (float v : d) { + EXPECT_EQ(v, kInf); + } + const auto z = nm.distance_field({0}, false, 0.0f); // only the seed itself + EXPECT_FLOAT_EQ(z[0], 0.0f); + EXPECT_EQ(std::count_if(z.begin(), z.end(), [](float v) {return v < kInf;}), 1); +}