Skip to content

Add NavMap::distance_field - #33

Merged
fmrico merged 1 commit into
rollingfrom
distance_field
Oct 15, 2026
Merged

fmrico merged 1 commit into
rollingfrom
distance_field

Conversation

@fmrico

@fmrico fmrico commented Oct 14, 2026

Copy link
Copy Markdown
Contributor

Fixes #32

New representation primitive:

std::vector<float> NavMap::distance_field(
  const std::vector<NavCelId> & seed_cells, bool from_boundary, float max_distance) const;

It returns the 3D distance from each NavCel centroid to the nearest seed:

  • the centroids of seed_cells;
  • if from_boundary, the open edges of the mesh, as real segments (walls, drops, unmapped space).

How it works:

  • A Dijkstra-like propagation carries the nearest seed's geometry over the adjacency and measures the distance straight to it, not as a sum of centroid hops.
  • It never crosses between disconnected surfaces, so an obstacle on one floor does not affect the floor above.
  • Cells beyond max_distance, or unreachable ones, get +inf.

The EasyNav InflationFilter will use it (separate PR in easynav_plugins). The likelihood-field localization on the NavMap and clearance-aware planning can reuse it.

Tests

test_distance_field (8 tests, all against brute-force distances):

  • rectangle borders (exact);
  • seed cells, and seeds together with the borders;
  • the max_distance cut-off;
  • stacked floors not seeing each other;
  • a 30° slope, where the distance is 3D;
  • interior hole edges;
  • irregular terrain with holes: never below the true distance, mean excess < 2 cm;
  • degenerate inputs.

They also pass under ASan/UBSan.

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 <fmrico@gmail.com>
@fmrico
fmrico merged commit f17c687 into rolling Oct 15, 2026
1 check passed
@fmrico
fmrico deleted the distance_field branch October 15, 2026 04:18
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Add a geodesic distance field primitive (distance to obstacles and to the map border)

1 participant