From 877935f47bee64bc93093c2c6ac7063f3ef16e3d Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 17 Nov 2026 00:58:59 +0100 Subject: [PATCH] navmap_core: simplify() with a bound that changes from place to place 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. --- navmap_core/include/navmap_core/Simplify.hpp | 7 +++ navmap_core/src/navmap_core/Simplify.cpp | 24 ++++++-- navmap_core/tests/test_simplify.cpp | 65 ++++++++++++++++++++ 3 files changed, 92 insertions(+), 4 deletions(-) diff --git a/navmap_core/include/navmap_core/Simplify.hpp b/navmap_core/include/navmap_core/Simplify.hpp index 5270f15..89e6353 100644 --- a/navmap_core/include/navmap_core/Simplify.hpp +++ b/navmap_core/include/navmap_core/Simplify.hpp @@ -23,6 +23,7 @@ #define NAVMAP_CORE__SIMPLIFY_HPP_ #include +#include #include #include #include @@ -49,6 +50,12 @@ struct SimplifyParams /// Layers left out of the above (e.g. per-cell geometry such as "elevation"). They are carried /// from the NavCel that survives, so recompute them if they depend on its shape. std::vector ignore_layers; + /// Optional: max_error at each point (m), e.g. growing with the distance to a robot, so the 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. + std::function max_error_at; + /// Optional: max_edge_length at each point (m), the same way. + std::function max_edge_length_at; }; /// Sizes before and after simplify(). diff --git a/navmap_core/src/navmap_core/Simplify.cpp b/navmap_core/src/navmap_core/Simplify.cpp index 48e7c3a..a594d32 100644 --- a/navmap_core/src/navmap_core/Simplify.cpp +++ b/navmap_core/src/navmap_core/Simplify.cpp @@ -218,10 +218,26 @@ class Simplifier return out; } + // The square of a bound at the collapse of u into v: the smaller of what `at` says at both + // vertices, `fallback` where it says nothing usable + double bound2( + const std::function & at, uint32_t u, uint32_t v, + double fallback) const + { + if (!at) {return fallback * fallback;} + const float a = at(P_[u].cast()), b = at(P_[v].cast()); + const bool ok_a = std::isfinite(a) && a >= 0.0f, ok_b = std::isfinite(b) && b >= 0.0f; + if (!ok_a && !ok_b) {return fallback * fallback;} + const double m = !ok_a ? b : (!ok_b ? a : std::min(a, b)); + return m * m; + } + // Whether collapsing u into v keeps layers, border, topology and quality; its quadric cost. bool valid(uint32_t u, uint32_t v, double & cost) const { if (locked_[u] || dead_[u] || dead_[v] || u == v) {return false;} + const double max_err2 = p_.max_error_at ? + bound2(p_.max_error_at, u, v, p_.max_error) : max_err2_; const auto & fan = VT_[u]; for (const uint32_t t : fan) { if (cls_[t] != cls_[fan[0]] || surf_[t] != surf_[fan[0]]) {return false;} @@ -243,7 +259,7 @@ class Simplifier if (len2 == 0.0) {return false;} const double s = (P_[u] - P_[v]).dot(d) / len2; if (s <= 0.0 || s >= 1.0) {return false;} - if ((P_[v] + s * d - P_[u]).squaredNorm() > max_err2_) {return false;} + if ((P_[v] + s * d - P_[u]).squaredNorm() > max_err2) {return false;} } else if (!open.empty() || et.size() != 2) { return false; } @@ -255,7 +271,7 @@ class Simplifier } if (common != et.size()) {return false;} // The NavCels that stay, with v instead of u - const double max_edge2 = static_cast(p_.max_edge_length) * p_.max_edge_length; + const double max_edge2 = bound2(p_.max_edge_length_at, u, v, p_.max_edge_length); bool any = false; double dev2 = std::numeric_limits::infinity(); for (const uint32_t t : fan) { @@ -279,10 +295,10 @@ class Simplifier P_[u].cast(), q[0].cast(), q[1].cast(), q[2].cast()))); any = true; } - if (!any || dev2 > max_err2_) {return false;} + if (!any || dev2 > max_err2) {return false;} const Eigen::Vector4d h(P_[v].x(), P_[v].y(), P_[v].z(), 1.0); cost = std::max(0.0, h.dot((Q_[u] + Q_[v]) * h)); - return cost <= max_err2_; + return cost <= max_err2; } void push_best(uint32_t u) diff --git a/navmap_core/tests/test_simplify.cpp b/navmap_core/tests/test_simplify.cpp index 8ae44ce..8c720ce 100644 --- a/navmap_core/tests/test_simplify.cpp +++ b/navmap_core/tests/test_simplify.cpp @@ -412,3 +412,68 @@ TEST(Simplify, DegenerateInputsAreSafe) } } } + +// max_error_at: exact where it says 0 (near a robot), simplified where it allows more +TEST(Simplify, TheErrorCanChangeFromPlaceToPlace) +{ + const Height bumps = [](float x, float y) {return 0.3f * std::sin(x) * std::cos(0.7f * y);}; + NavMap orig; + add_grid(orig, 60, 30, 0.1f, bumps); // x in [0, 6] + orig.rebuild_geometry_accels(); + NavMap nm = orig; + SimplifyParams p; + p.max_error = 0.05f; + p.max_error_at = [](const Eigen::Vector3f & q) {return q.x() < 3.0f ? 0.0f : 0.05f;}; + navmap::simplify(nm, p); + std::size_t left = 0, right = 0, left0 = 0; + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + (nm.positions.x[i] < 2.9f ? left : right) += 1; + } + for (std::size_t i = 0; i < orig.positions.size(); ++i) { + left0 += orig.positions.x[i] < 2.9f; + } + EXPECT_EQ(left, left0) << "where the error is 0 no vertex moves"; + EXPECT_LT(right, orig.positions.size() - left0) << "where it is not, the terrain is simplified"; + // what is given as not usable (NaN, negative) stands for max_error + NavMap nm2 = orig; + p.max_error_at = [](const Eigen::Vector3f &) {return -1.0f;}; + navmap::simplify(nm2, p); + NavMap nm3 = orig; + p.max_error_at = nullptr; + navmap::simplify(nm3, p); + EXPECT_EQ(nm2.navcels.size(), nm3.navcels.size()); +} + +// max_edge_length_at: small NavCels in one place, large in another +TEST(Simplify, TheEdgeLengthCanChangeFromPlaceToPlace) +{ + NavMap nm; + add_grid(nm, 60, 30, 0.1f, flat); // x in [0, 6] + nm.rebuild_geometry_accels(); + SimplifyParams p; + p.max_edge_length_at = [](const Eigen::Vector3f & q) { + return q.x() < 3.0f ? 0.3f : std::numeric_limits::infinity(); + }; + navmap::simplify(nm, p); + float left = 0.0f, right = 0.0f; + for (const auto & c : nm.navcels) { + float lo = 1e9f, longest = 0.0f; + for (int k = 0; k < 3; ++k) { + const auto a = nm.positions.at(c.v[k]), b = nm.positions.at(c.v[(k + 1) % 3]); + lo = std::min(lo, a.x()); + longest = std::max(longest, (a - b).norm()); + } + if (lo < 2.5f) { + float hi = 0.0f; + for (int k = 0; k < 3; ++k) { + hi = std::max(hi, nm.positions.at(c.v[k]).x()); + } + if (hi < 2.9f) {left = std::max(left, longest);} + } else if (lo > 3.1f) { + right = std::max(right, longest); + } + } + EXPECT_LE(left, 0.3f + 1e-4f); + EXPECT_GT(right, 1.0f); + EXPECT_NEAR(area(nm), 18.0f, 1e-3f); +}