Skip to content

navmap_core: simplify() with a bound that changes from place to place - #46

Open
jmguerreroh wants to merge 1 commit into
EasyNavigation:rollingfrom
jmguerreroh:navmap-core-simplify-bounds
Open

jmguerreroh wants to merge 1 commit into
EasyNavigation:rollingfrom
jmguerreroh:navmap-core-simplify-bounds

Conversation

@jmguerreroh

@jmguerreroh jmguerreroh commented Nov 17, 2026 •

Copy link
Copy Markdown
Contributor

navmap_core: simplify() with a bound that changes from place to place

Branch base: rolling. Files: navmap_core (Simplify.hpp/.cpp and its tests). Independent of
the navmap_tools buildings PR; neither needs the other.

What

Two optional fields of SimplifyParams:

std::function<float(const Eigen::Vector3f &)> max_error_at;        // m, at a point
std::function<float(const Eigen::Vector3f &)> max_edge_length_at;  // m, at a point

When set, a collapse is bounded by the smaller of the values at the two vertices of its edge,
instead of max_error / max_edge_length everywhere. That bound is used wherever the fixed one
was: the deviation of the open edges, the deviation of the surface, the quadric cost and the edge
length. A value that is not finite or is negative stands for the fixed one. Unset, nothing changes.

The use: a map exact near a robot and coarse far from it, e.g. an error that grows with the
distance to the robot.

navmap::SimplifyParams sp;
sp.max_error = 4.0f;
sp.max_error_at = [robot](const Eigen::Vector3f & q) {
    const float d = (q - robot).head<2>().norm();
    return d < 20.0f ? 0.4f : std::min(4.0f, 0.4f * std::pow(10.0f, (d - 20.0f) / 280.0f));
  };
navmap::simplify(map, sp);

Why

Planning on such a map against fixed grids, on 2 x 2 km around the Puerta del Sol (Madrid; ground
from the IGN, buildings from the Catastro), 30 routes of 200 to 1500 m, easynav_navmap_planner's
A*, a robot of radius 0.5 m replanning every 30 m on a map rebuilt around it:

Map NavCels MB Success Through a building Length vs 0.5 m A* per plan NavCels expanded
grid 0.5 m 13,765,636 321 30/30 0 1.000 369 ms 1,166,402
grid 1 m 3,275,294 77 21/30 0 1.006 62 ms 190,060
grid 2 m 744,768 18 28/30 0 1.008 18 ms 71,510
grid 4 m 156,118 3.9 13/30 0 1.354 7.4 ms 24,202
simplified (0.4 m everywhere) 393,251 10.3 30/30 0 1.022 15 ms 35,082
variable (0.4 m to 4 m, this PR) ~72,000 2.1 30/30 0 1.032 3.8 ms 3,361

The holes (buildings) and the open edges are kept by simplify() as before, so a coarse far zone
keeps its streets.

Tests

  • TheErrorCanChangeFromPlaceToPlace: a bumpy terrain with an error of 0 on one half keeps every
    vertex of that half and is simplified on the other; a negative value gives the same map as no
    max_error_at.
  • TheEdgeLengthCanChangeFromPlaceToPlace: a plane with a short edge length on one half and none
    on the other keeps its NavCels small only on the first half.

colcon test --packages-select navmap_core on this branch: 328 tests (with the linters), no
failures.

SimplifyParams::max_error_at and max_edge_length_at (optional): the bound at each point,
e.g. growing with the distance to a robot, so a map is exact near it and coarse far away.
A collapse uses the smaller of the values at its two vertices; a value that is not finite or
is negative stands for max_error / max_edge_length.

This branch has not been deployed

No deployments
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.

1 participant