From 21ab88fc6c178743e3dd403279b21f1a7243c7b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 09:05:09 +0200 Subject: [PATCH 01/11] Add NavCel iterators MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit navmap_core/Iterators.hpp: box (every surface, via the BVH), disc, ring, polygon, distance order and segment walk over the NavCels. Regions around a point grow from the NavCel under it through the mesh adjacency, so they never jump to another floor. Distances are Euclidean (to the centroid) or geodesic (Lazy Theta*: exact on planes, bends around borders). A reusable TraversalScratch avoids allocations. Tests against brute force and a table in navmap_bench. Signed-off-by: Francisco Martín Rico --- README.md | 29 + navmap_core/bench/navmap_bench.cpp | 76 +++ navmap_core/include/navmap_core/Iterators.hpp | 476 ++++++++++++++ navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_iterators.cpp | 597 ++++++++++++++++++ 5 files changed, 1183 insertions(+) create mode 100644 navmap_core/include/navmap_core/Iterators.hpp create mode 100644 navmap_core/tests/test_iterators.cpp diff --git a/README.md b/README.md index c7d6f39..11b066e 100644 --- a/README.md +++ b/README.md @@ -28,6 +28,7 @@ Funded by the European Union through the Horizon Europe programme under Grant Ag - **Locate API**: find the NavCel under/around a 3D position using BVH acceleration and raycasting. - **Raytracing**: Möller–Trumbore intersection with a simple BVH for efficiency. - **Multi-surface support**: naturally supports multiple disconnected surfaces (e.g., separate floors). +- **Iterators** over NavCels (`navmap_core/Iterators.hpp`): box, disc, ring, polygon, distance order and segment walk; regions grow over the surface, so they never jump to another floor. --- @@ -227,6 +228,34 @@ nm.set_area(Eigen::Vector3f(0.8f, 0.2f, 0.0f), --- +## 7b. Iterating over NavCels (`Iterators.hpp`) + +Visitors are called once per NavCel; returning `false` stops. Regions around a point grow from the NavCel under it through the mesh adjacency, so a disc on one floor never reaches the floor above. Reuse a `TraversalScratch` to avoid allocations. + +```cpp +#include + +navmap::TraversalScratch scratch; +const Eigen::Vector3f robot(2.0f, 1.0f, 0.0f); + +// NavCels within 0.5 m of the robot (3D distance to the centroid) +navmap::for_each_in_disc(nm, robot, 0.5f, [&](navmap::NavCelId c) {/* ... */}, + navmap::Metric::EUCLIDEAN, &scratch); + +// NavCels crossed by a sensor ray projected on the surface, with the entry/exit parameters +navmap::for_each_on_segment(nm, robot, Eigen::Vector3f(6.0f, 3.0f, 0.0f), + [&](navmap::NavCelId c, float t_in, float t_out) {/* clear c */}); + +// Nearest NavCel along the surface (geodesic order) with a free value +navmap::for_each_by_distance(nm, seed, robot, 3.0f, [&](navmap::NavCelId c, float d) { + return nm.layer_get("occupancy", c) != 0; // false stops at the first free one + }); +``` + +Also `for_each_in_aabb` (every surface, via the BVH), `for_each_in_ring` and `for_each_in_polygon` (XY polygon on the seed's floor). `Metric::GEODESIC` measures along the surface and bends around borders; it is about 80 times costlier than `EUCLIDEAN`, so use it for occasional queries. + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: diff --git a/navmap_core/bench/navmap_bench.cpp b/navmap_core/bench/navmap_bench.cpp index 00d3052..cf06c79 100644 --- a/navmap_core/bench/navmap_bench.cpp +++ b/navmap_core/bench/navmap_bench.cpp @@ -25,6 +25,7 @@ #include #include +#include "navmap_core/Iterators.hpp" #include "navmap_core/NavMap.hpp" using navmap::NavCelId; @@ -157,5 +158,80 @@ int main(int argc, char ** argv) nm.navcels.size(), geometry_bytes(nm) / 1e6, build_ms, loc_nohint, loc_hint, ray_down, ray_rand, closest, area, copy_same, copy_deep); } + + // Iterators (with a reused scratch), from a located seed; us per call + std::printf( + "\niterators, us/call (cells visited)\n%9s %16s %16s %16s %16s %16s %16s %16s %16s\n", + "navcels", "disc_e_r0.5", "disc_e_r2", "disc_g_r0.5", "disc_g_r2", "aabb_2x2", "poly_1x1", + "segment_5m", "bydist_r2"); + for (int side : {100, 300, 1000, 3000}) { + if (side > max_side) {break;} + NavMap nm = make_map(side, floors, res); + nm.rebuild_geometry_accels(); + const float extent = side * res; + std::mt19937 rng(2); + std::uniform_real_distribution U(2.5f, std::max(2.6f, extent - 2.5f)); + const int M = 2000; + std::vector pts(M); + std::vector seeds(M); + for (int k = 0; k < M; ++k) { + pts[k] = {U(rng), U(rng), 0.1f}; + std::size_t s; Eigen::Vector3f bary, hit; + nm.locate_navcel(pts[k], s, seeds[k], bary, &hit); + } + navmap::TraversalScratch sc; + std::size_t cells = 0; + auto count = [&](NavCelId) {++cells;}; + char col[8][32]; + int ci = 0; + auto run = [&](int n, auto && body) { + cells = 0; + const double us = median_us(R, n, body); + std::snprintf(col[ci++], sizeof(col[0]), "%.1f (%zu)", us, cells / (R * n)); + }; + run(M, [&](int k) { + navmap::for_each_in_disc(nm, seeds[k], pts[k], 0.5f, count, + navmap::Metric::EUCLIDEAN, &sc); + }); + run(M / 10, [&](int k) { + navmap::for_each_in_disc(nm, seeds[k], pts[k], 2.0f, count, + navmap::Metric::EUCLIDEAN, &sc); + }); + run(M, [&](int k) { + navmap::for_each_in_disc(nm, seeds[k], pts[k], 0.5f, count, + navmap::Metric::GEODESIC, &sc); + }); + run(M / 10, [&](int k) { + navmap::for_each_in_disc(nm, seeds[k], pts[k], 2.0f, count, + navmap::Metric::GEODESIC, &sc); + }); + run(M, [&](int k) { + navmap::AABB b; + b.expand(pts[k] - Eigen::Vector3f(1, 1, 1)); + b.expand(pts[k] + Eigen::Vector3f(1, 1, 1)); + navmap::for_each_in_aabb(nm, b, count, &sc); + }); + run(M, [&](int k) { + const float x = pts[k].x(), y = pts[k].y(); + navmap::for_each_in_polygon( + nm, seeds[k], {{x - 0.5f, y - 0.5f}, {x + 0.5f, y - 0.5f}, {x + 0.5f, y + 0.5f}, + {x - 0.5f, y + 0.5f}}, count, &sc); + }); + run(M, [&](int k) { + navmap::for_each_on_segment(nm, seeds[k], pts[k], + pts[(k + 1) % M].head<2>().isZero() ? pts[k] : + Eigen::Vector3f(pts[k] + 5.0f * (pts[(k + 1) % M] - pts[k]).normalized()), + [&](NavCelId, float, float) {++cells;}); + }); + run(M / 10, [&](int k) { + navmap::for_each_by_distance(nm, seeds[k], pts[k], 2.0f, + [&](NavCelId, float) {++cells;}, &sc); + }); + std::printf("%9zu", nm.navcels.size()); + for (auto & c : col) { + std::printf(" %16s", c); + } + std::printf("\n"); + } return 0; } diff --git a/navmap_core/include/navmap_core/Iterators.hpp b/navmap_core/include/navmap_core/Iterators.hpp new file mode 100644 index 0000000..62aafbf --- /dev/null +++ b/navmap_core/include/navmap_core/Iterators.hpp @@ -0,0 +1,476 @@ +// 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. + +/** + * @file Iterators.hpp + * @brief Traversals over the NavCels of a ::navmap::NavMap: box, disc, ring, polygon, distance + * order and segment walk. + * + * Every traversal calls a visitor per NavCel. The visitor may return void, or bool: false stops + * the traversal. Regions around a point (disc, ring, polygon, distance order) grow from a seed + * NavCel through the mesh adjacency, so they stay on the seed's surface and level: a disc on the + * ground floor never reaches the floor above, however close it is in XY. The box traversal is + * global (every surface). A NavCel belongs to a region when its centroid does. + * + * Pass a TraversalScratch to reuse buffers across calls: once warm, no traversal allocates. + */ + +#ifndef NAVMAP_CORE__ITERATORS_HPP_ +#define NAVMAP_CORE__ITERATORS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/// Distance from a centre used to grow a region. +enum class Metric +{ + EUCLIDEAN, ///< 3D distance from the centre to each NavCel centroid + GEODESIC ///< Along the surface: straight lines on it, bent only around borders +}; + +/// Reusable traversal buffers. Not thread-safe: one per thread. +class TraversalScratch +{ +public: + /// Starts a traversal over a map of n NavCels: every NavCel unmarked. + void begin(std::size_t n) + { + if (stamp_.size() < n) { + stamp_.assign(n, 0); + dist.resize(n); + epoch_ = 0; + } + if (++epoch_ == 0) { // wrapped: clear the stamps once + std::fill(stamp_.begin(), stamp_.end(), 0); + epoch_ = 1; + } + stack.clear(); + heap.clear(); + } + + /// Marks cid; false if it was already marked in this traversal. + bool mark(NavCelId cid) + { + if (stamp_[cid] == epoch_) {return false;} + stamp_[cid] = epoch_; + return true; + } + + bool marked(NavCelId cid) const {return stamp_[cid] == epoch_;} + + /// Dijkstra frontier entry: distance, NavCel, and the pivot it is measured from. + struct Front + { + float d; + NavCelId cid; + NavCelId pivot_cel; + float pivot_len; + Eigen::Vector3f pivot; + }; + + std::vector dist; ///< Per NavCel, valid once marked: its settled distance + + std::vector stack; ///< BFS frontier + std::vector heap; ///< Dijkstra frontier (min-heap) + std::vector nodes; ///< BVH node stack + +private: + std::vector stamp_; + std::uint32_t epoch_{0}; +}; + +namespace detail +{ + +// Calls the visitor; true unless it returned false. +template +inline bool visit(F & f, A &&... a) +{ + if constexpr (std::is_void_v>) { + f(std::forward(a)...); + return true; + } else { + return static_cast(f(std::forward(a)...)); + } +} + +inline constexpr NavCelId kNoCel = std::numeric_limits::max(); + +inline bool boxes_overlap(const AABB & a, const AABB & b) +{ + return a.min.x() <= b.max.x() && a.max.x() >= b.min.x() && + a.min.y() <= b.max.y() && a.max.y() >= b.min.y() && + a.min.z() <= b.max.z() && a.max.z() >= b.min.z(); +} + +inline AABB navcel_box(const NavMap & nm, NavCelId cid) +{ + AABB b; + for (const auto v : nm.navcels[cid].v) { + b.expand(nm.positions.at(v)); + } + return b; +} + +// Even-odd rule in XY. +inline bool in_polygon(const std::vector & poly, float x, float y) +{ + bool in = false; + for (std::size_t i = 0, j = poly.size() - 1; i < poly.size(); j = i++) { + const auto & a = poly[i]; + const auto & b = poly[j]; + if ((a.y() > y) != (b.y() > y) && + x < (b.x() - a.x()) * (y - a.y()) / (b.y() - a.y()) + a.x()) + { + in = !in; + } + } + return in; +} + +inline bool seed_at(const NavMap & nm, const Eigen::Vector3f & p, NavCelId & seed) +{ + std::size_t s = 0; + Eigen::Vector3f bary, hit; + return nm.locate_navcel(p, s, seed, bary, &hit); +} + +// Segment walk (see for_each_on_segment). +template +bool for_each_on_segment_impl( + const NavMap & nm, NavCelId seed, const Eigen::Vector3f & a, const Eigen::Vector3f & b, F && f) +{ + if (seed >= nm.navcels.size()) {return false;} + const Eigen::Vector2f a2(a.x(), a.y()); + const Eigen::Vector2f d2(b.x() - a.x(), b.y() - a.y()); + auto cross = [](const Eigen::Vector2f & u, const Eigen::Vector2f & v) { + return u.x() * v.y() - u.y() * v.x(); + }; + constexpr float kEps = 1e-6f; + NavCelId c = seed, prev = kNoCel; + float t_in = 0.0f; + for (std::size_t steps = 0; steps <= nm.navcels.size(); ++steps) { + const auto & cel = nm.navcels[c]; + Eigen::Vector2f p[3]; + for (int k = 0; k < 3; ++k) { + const auto v = nm.positions.at(cel.v[k]); + p[k] = Eigen::Vector2f(v.x(), v.y()); + } + const float area2 = cross(p[1] - p[0], p[2] - p[0]); + if (std::abs(area2) < 1e-12f) {return false;} // vertical in XY: cannot be walked + const float sgn = area2 > 0.0f ? 1.0f : -1.0f; + // Leaves through the edge whose inside-test reaches 0 first, beyond t_in. + float t_exit = std::numeric_limits::infinity(); + int e_exit = -1; + for (int e = 0; e < 3; ++e) { + const Eigen::Vector2f edge = p[(e + 1) % 3] - p[e]; + const float ds = sgn * cross(edge, d2); + if (ds >= 0.0f) {continue;} + const float s0 = sgn * cross(edge, a2 - p[e]); + const float te = std::max(-s0 / ds, t_in); + if (cel.neighbor[e] == prev && te <= t_in + kEps) {continue;} // back where it came from + if (te < t_exit) { + t_exit = te; + e_exit = e; + } + } + if (e_exit < 0 || t_exit >= 1.0f) { // b lies in this NavCel + return visit(f, c, t_in, 1.0f); + } + if (t_exit - t_in > kEps || c == seed) { + if (!visit(f, c, t_in, t_exit)) {return false;} + } + const NavCelId next = cel.neighbor[e_exit]; + if (next == kNoCel) {return false;} // left the surface through a border + prev = c; + c = next; + t_in = t_exit; + } + return false; +} + +// Grows the region of NavCels at distance in [0, r_out] from the centre, connected to the seed +// through NavCels of the region; visits those at distance >= r_in as f(cid, distance). The seed +// (it holds the centre) is always expanded, and visited regardless of distance if with_seed. +// GEODESIC visits by distance (non-decreasing up to one neighbour step); EUCLIDEAN breadth-first. +template +std::size_t grow( + const NavMap & nm, NavCelId seed, const Eigen::Vector3f & center, float r_in, float r_out, + Metric metric, bool with_seed, TraversalScratch * scratch, F & f) +{ + if (seed >= nm.navcels.size() || !(r_out >= 0.0f)) {return 0;} + TraversalScratch local; + TraversalScratch & sc = scratch ? *scratch : local; + sc.begin(nm.navcels.size()); + std::size_t visited = 0; + // Visits c if it passes the filter; false if the visitor asked to stop. + auto emit = [&](NavCelId c, float d) { + const bool forced = with_seed && c == seed; + if (!forced && (d < r_in || d > r_out)) {return true;} + ++visited; + return visit(f, c, d); + }; + + if (metric == Metric::EUCLIDEAN) { + sc.mark(seed); + sc.stack.push_back(seed); + std::size_t head = 0; // the stack is used as a FIFO queue + while (head < sc.stack.size()) { + const NavCelId c = sc.stack[head++]; + const float d = (nm.navcel_centroid(c) - center).norm(); + if (c != seed && d > r_out) {continue;} + if (!emit(c, d)) {return visited;} + for (const NavCelId n : nm.navcels[c].neighbor) { + if (n != kNoCel && sc.mark(n)) {sc.stack.push_back(n);} + } + } + return visited; + } + + // GEODESIC: any-angle Dijkstra (Lazy Theta*). Each NavCel is measured in a straight line from + // a pivot (first the centre). A neighbour is pushed with its parent's pivot; when it is popped, + // a walk along the line checks that the line stays on the surface, and if it does not, the + // nearest settled neighbour becomes its pivot. Exact on planes; around borders and holes it + // bends at centroids, so it can be about one NavCel longer per bend. + auto cmp = [](const TraversalScratch::Front & a, const TraversalScratch::Front & b) { + return a.d > b.d; + }; + auto push = [&](const TraversalScratch::Front & e) { + sc.heap.push_back(e); + std::push_heap(sc.heap.begin(), sc.heap.end(), cmp); + }; + auto visible = [&](NavCelId from, const Eigen::Vector3f & p, const Eigen::Vector3f & q, + NavCelId target) { + NavCelId last = kNoCel; + const bool reached = for_each_on_segment_impl( + nm, from, p, q, [&](NavCelId x, float, float) {last = x;}); + return reached && last == target; + }; + push({(nm.navcel_centroid(seed) - center).norm(), seed, seed, 0.0f, center}); + while (!sc.heap.empty()) { + std::pop_heap(sc.heap.begin(), sc.heap.end(), cmp); + TraversalScratch::Front u = sc.heap.back(); + sc.heap.pop_back(); + if (sc.marked(u.cid)) {continue;} + const Eigen::Vector3f pu = nm.navcel_centroid(u.cid); + if (u.cid != seed && !visible(u.pivot_cel, u.pivot, pu, u.cid)) { + // Not seen from the inherited pivot: through the best settled neighbour instead + u.d = std::numeric_limits::infinity(); + for (const NavCelId m : nm.navcels[u.cid].neighbor) { + if (m == kNoCel || !sc.marked(m)) {continue;} + const Eigen::Vector3f pm = nm.navcel_centroid(m); + const float dm = sc.dist[m] + (pu - pm).norm(); + if (dm < u.d) {u = {dm, u.cid, m, sc.dist[m], pm};} + } + } + sc.mark(u.cid); + sc.dist[u.cid] = u.d; + if (u.cid != seed && u.d > r_out) {continue;} // corrected beyond the radius + if (!emit(u.cid, u.d)) {return visited;} + for (const NavCelId n : nm.navcels[u.cid].neighbor) { + if (n == kNoCel || sc.marked(n)) {continue;} + // Optimistic: n seen from u's pivot (checked when popped) + const Eigen::Vector3f pn = nm.navcel_centroid(n); + const float dn = u.pivot_len + (pn - u.pivot).norm(); + if (dn <= r_out) {push({dn, n, u.pivot_cel, u.pivot_len, u.pivot});} + } + } + return visited; +} + +} // namespace detail + +/** + * @brief Visits the NavCels whose bounding box overlaps @p box, on every surface (via the BVH). + * @param f Visitor `f(NavCelId)`; returning false stops. + * @return Number of NavCels visited. + */ +template +std::size_t for_each_in_aabb( + const NavMap & nm, const AABB & box, F && f, TraversalScratch * scratch = nullptr) +{ + TraversalScratch local; + TraversalScratch & sc = scratch ? *scratch : local; + std::size_t visited = 0; + for (const auto & s : nm.surfaces) { + if (s.bvh.empty()) {continue;} + sc.nodes.clear(); + sc.nodes.push_back(0); + while (!sc.nodes.empty()) { + const BVHNode & node = s.bvh[sc.nodes.back()]; + sc.nodes.pop_back(); + if (!detail::boxes_overlap(node.box, box)) {continue;} + if (!node.is_leaf()) { + sc.nodes.push_back(node.left); + sc.nodes.push_back(node.right); + continue; + } + for (int i = 0; i < node.count; ++i) { + const auto cid = static_cast(s.prim_indices[node.start + i]); + if (!detail::boxes_overlap(detail::navcel_box(nm, cid), box)) {continue;} + ++visited; + if (!detail::visit(f, cid)) {return visited;} + } + } + } + return visited; +} + +/** + * @brief Visits the NavCels within @p radius of @p center, connected to @p seed (which should + * contain the centre). The seed is always visited. + * @param f Visitor `f(NavCelId)`; returning false stops. + * @return Number of NavCels visited. + */ +template +std::size_t for_each_in_disc( + const NavMap & nm, NavCelId seed, const Eigen::Vector3f & center, float radius, F && f, + Metric metric = Metric::EUCLIDEAN, TraversalScratch * scratch = nullptr) +{ + auto g = [&](NavCelId c, float) {return detail::visit(f, c);}; + return detail::grow(nm, seed, center, 0.0f, radius, metric, true, scratch, g); +} + +/// As above, locating the seed under @p center; visits nothing if the centre is off the map. +template +std::size_t for_each_in_disc( + const NavMap & nm, const Eigen::Vector3f & center, float radius, F && f, + Metric metric = Metric::EUCLIDEAN, TraversalScratch * scratch = nullptr) +{ + NavCelId seed; + if (!detail::seed_at(nm, center, seed)) {return 0;} + return for_each_in_disc(nm, seed, center, radius, std::forward(f), metric, scratch); +} + +/** + * @brief Visits the NavCels at distance in [@p r_in, @p r_out] from @p center, connected to + * @p seed through NavCels within @p r_out (the inner part is crossed, not visited). + * @param f Visitor `f(NavCelId)`; returning false stops. + * @return Number of NavCels visited. + */ +template +std::size_t for_each_in_ring( + const NavMap & nm, NavCelId seed, const Eigen::Vector3f & center, float r_in, float r_out, + F && f, Metric metric = Metric::EUCLIDEAN, TraversalScratch * scratch = nullptr) +{ + auto g = [&](NavCelId c, float) {return detail::visit(f, c);}; + return detail::grow(nm, seed, center, r_in, r_out, metric, false, scratch, g); +} + +/** + * @brief Visits NavCels by geodesic distance from @p center, up to @p max_distance: a spiral + * over the surface (nearest free cell, local search...). The seed comes first, always. + * The order is non-decreasing up to the distance between neighbour centroids. + * @param f Visitor `f(NavCelId, float distance)`; returning false stops. + * @return Number of NavCels visited. + */ +template +std::size_t for_each_by_distance( + const NavMap & nm, NavCelId seed, const Eigen::Vector3f & center, float max_distance, F && f, + TraversalScratch * scratch = nullptr) +{ + return detail::grow(nm, seed, center, 0.0f, max_distance, Metric::GEODESIC, true, scratch, f); +} + +/** + * @brief Visits the NavCels whose centroid lies inside the XY polygon @p poly, connected to + * @p seed through NavCels inside it (so only the seed's level). Even-odd rule. + * @param f Visitor `f(NavCelId)`; returning false stops. + * @return Number of NavCels visited. + */ +template +std::size_t for_each_in_polygon( + const NavMap & nm, NavCelId seed, const std::vector & poly, F && f, + TraversalScratch * scratch = nullptr) +{ + if (seed >= nm.navcels.size() || poly.size() < 3) {return 0;} + TraversalScratch local; + TraversalScratch & sc = scratch ? *scratch : local; + sc.begin(nm.navcels.size()); + sc.mark(seed); + sc.stack.push_back(seed); + std::size_t head = 0, visited = 0; + while (head < sc.stack.size()) { + const NavCelId c = sc.stack[head++]; + const Eigen::Vector3f p = nm.navcel_centroid(c); + const bool in = detail::in_polygon(poly, p.x(), p.y()); + if (!in && c != seed) {continue;} + if (in) { + ++visited; + if (!detail::visit(f, c)) {return visited;} + } + for (const NavCelId n : nm.navcels[c].neighbor) { + if (n != detail::kNoCel && sc.mark(n)) {sc.stack.push_back(n);} + } + } + return visited; +} + +/// As above, locating the seed under @p on_surface (a point of the polygon's level). +template +std::size_t for_each_in_polygon( + const NavMap & nm, const Eigen::Vector3f & on_surface, + const std::vector & poly, F && f, TraversalScratch * scratch = nullptr) +{ + NavCelId seed; + if (!detail::seed_at(nm, on_surface, seed)) {return 0;} + return for_each_in_polygon(nm, seed, poly, std::forward(f), scratch); +} + +/** + * @brief Walks the surface along segment a -> b (in XY), from @p seed (the NavCel under a), + * visiting each NavCel crossed with the segment parameters where it enters and leaves. + * + * The walk follows the mesh adjacency, so it stays on the seed's level, also on slopes. + * + * @param f Visitor `f(NavCelId, float t_in, float t_out)`, 0 <= t_in <= t_out <= 1; returning + * false stops. + * @return True if the walk reached the NavCel under b; false if it left the surface through a + * border first (or was stopped). + */ +template +bool for_each_on_segment( + const NavMap & nm, NavCelId seed, const Eigen::Vector3f & a, const Eigen::Vector3f & b, F && f) +{ + return detail::for_each_on_segment_impl(nm, seed, a, b, std::forward(f)); +} + +/// As above, locating the seed under @p a; false if a is off the map. +template +bool for_each_on_segment( + const NavMap & nm, const Eigen::Vector3f & a, const Eigen::Vector3f & b, F && f) +{ + NavCelId seed; + if (!detail::seed_at(nm, a, seed)) {return false;} + return for_each_on_segment(nm, seed, a, b, std::forward(f)); +} + +} // namespace navmap + +#endif // NAVMAP_CORE__ITERATORS_HPP_ diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index e87418a..1ba033c 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -52,3 +52,8 @@ ament_add_gtest(test_distance_field test_distance_field.cpp) target_link_libraries(test_distance_field ${PROJECT_NAME} ) + +ament_add_gtest(test_iterators test_iterators.cpp) +target_link_libraries(test_iterators + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_iterators.cpp b/navmap_core/tests/test_iterators.cpp new file mode 100644 index 0000000..b58b8f6 --- /dev/null +++ b/navmap_core/tests/test_iterators.cpp @@ -0,0 +1,597 @@ +// 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 +#include + +#include "navmap_core/Iterators.hpp" +#include "navmap_core/NavMap.hpp" + +using navmap::AABB; +using navmap::Metric; +using navmap::NavCelId; +using navmap::NavMap; +using navmap::TraversalScratch; + +namespace +{ + +constexpr NavCelId kNone = std::numeric_limits::max(); + +// nx x ny cells of `res` m at z = f(x, y) (+ origin), skipping cells where skip(i, j). +std::size_t add_grid( + NavMap & nm, int nx, int ny, float res, const std::function & f, + const std::function & skip = nullptr, float ox = 0.0f, float oy = 0.0f) +{ + 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) { + const float x = ox + i * res, y = oy + j * res; + nm.add_vertex({x, y, f(x, y)}); + } + } + 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; +} + +auto flat(float h) {return [h](float, float) {return h;};} + +NavMap flat_map(int n = 20, float res = 0.5f) +{ + NavMap nm; + add_grid(nm, n, n, res, flat(0.0f)); + nm.rebuild_geometry_accels(); + return nm; +} + +// Two floors over the same 10 x 10 m footprint, 3 m apart, not connected. +NavMap two_floors() +{ + NavMap nm; + add_grid(nm, 20, 20, 0.5f, flat(0.0f)); + add_grid(nm, 20, 20, 0.5f, flat(3.0f)); + nm.rebuild_geometry_accels(); + return nm; +} + +NavCelId seed_at(const NavMap & nm, const Eigen::Vector3f & p) +{ + std::size_t s; NavCelId c; Eigen::Vector3f bary, hit; + EXPECT_TRUE(nm.locate_navcel(p, s, c, bary, &hit)) << p.transpose(); + return c; +} + +template +std::set collect(Range && run) +{ + std::set out; + std::size_t calls = 0; + const std::size_t n = run([&](NavCelId c) {++calls; EXPECT_TRUE(out.insert(c).second) << c;}); + EXPECT_EQ(n, calls); + return out; +} + +// NavCels reachable from seed through NavCels satisfying `in` (the seed always expands). +std::set flood(const NavMap & nm, NavCelId seed, const std::function & in) +{ + std::set seen{seed}, out; + std::vector st{seed}; + while (!st.empty()) { + const NavCelId c = st.back(); + st.pop_back(); + if (c != seed && !in(c)) {continue;} + if (in(c)) {out.insert(c);} + for (NavCelId n : nm.navcels[c].neighbor) { + if (n != kNone && seen.insert(n).second) {st.push_back(n);} + } + } + return out; +} + +float cross2(const Eigen::Vector2f & u, const Eigen::Vector2f & v) +{ + return u.x() * v.y() - u.y() * v.x(); +} + +// Length of segment a-b (XY) inside triangle c (XY), by clipping against its three edges. +float overlap_length( + const NavMap & nm, NavCelId c, const Eigen::Vector2f & a, const Eigen::Vector2f & b) +{ + Eigen::Vector2f p[3]; + for (int k = 0; k < 3; ++k) { + const auto v = nm.positions.at(nm.navcels[c].v[k]); + p[k] = {v.x(), v.y()}; + } + const float sgn = cross2(p[1] - p[0], p[2] - p[0]) > 0 ? 1.0f : -1.0f; + float t0 = 0.0f, t1 = 1.0f; + for (int e = 0; e < 3; ++e) { + const Eigen::Vector2f edge = p[(e + 1) % 3] - p[e]; + const float s0 = sgn * cross2(edge, a - p[e]); + const float ds = sgn * cross2(edge, b - a); + if (std::abs(ds) < 1e-12f) { + if (s0 < 0) {return 0.0f;} + continue; + } + const float t = -s0 / ds; + if (ds > 0) {t0 = std::max(t0, t);} else {t1 = std::min(t1, t);} + } + return std::max(0.0f, t1 - t0) * (b - a).norm(); +} + +} // namespace + +// ------------------------------------------------------------------ AABB + +TEST(Iterators, AabbMatchesBruteForceOnBothFloors) +{ + const NavMap nm = two_floors(); + std::mt19937 rng(1); + std::uniform_real_distribution U(-1.0f, 11.0f), Z(-1.0f, 4.0f); + for (int q = 0; q < 200; ++q) { + AABB box; + box.expand({U(rng), U(rng), Z(rng)}); + box.expand({U(rng), U(rng), Z(rng)}); + const auto got = collect([&](auto f) {return navmap::for_each_in_aabb(nm, box, f);}); + std::set want; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (navmap::detail::boxes_overlap(navmap::detail::navcel_box(nm, c), box)) {want.insert(c);} + } + EXPECT_EQ(got, want) << q; + } +} + +TEST(Iterators, AabbSelectsALevelByHeightAndStops) +{ + const NavMap nm = two_floors(); + AABB upper; + upper.expand({-1, -1, 2.5f}); + upper.expand({11, 11, 3.5f}); + for (NavCelId c : collect([&](auto f) {return navmap::for_each_in_aabb(nm, upper, f);})) { + EXPECT_NEAR(nm.navcel_centroid(c).z(), 3.0f, 1e-5f); + } + EXPECT_EQ( + navmap::for_each_in_aabb(nm, upper, [](NavCelId) {return true;}), 800u); + int calls = 0; + EXPECT_EQ(navmap::for_each_in_aabb(nm, upper, [&](NavCelId) {return ++calls < 7;}), 7u); + AABB empty; // inverted (default) box overlaps nothing + EXPECT_EQ(navmap::for_each_in_aabb(nm, empty, [](NavCelId) {}), 0u); + AABB away; + away.expand({50, 50, 0}); + away.expand({60, 60, 1}); + EXPECT_EQ(navmap::for_each_in_aabb(nm, away, [](NavCelId) {}), 0u); + EXPECT_EQ(navmap::for_each_in_aabb(NavMap{}, upper, [](NavCelId) {}), 0u); +} + +// ------------------------------------------------------------------ disc and ring + +TEST(Iterators, EuclideanDiscOnAFlatMapIsTheCentroidsWithinTheRadius) +{ + const NavMap nm = flat_map(); + std::mt19937 rng(2); + std::uniform_real_distribution U(0.2f, 9.8f), R(0.0f, 4.0f); + for (int q = 0; q < 100; ++q) { + const Eigen::Vector3f c(U(rng), U(rng), 0.0f); + const float r = R(rng); + const NavCelId seed = seed_at(nm, c); + const auto got = collect( + [&](auto f) {return navmap::for_each_in_disc(nm, seed, c, r, f);}); + auto want = flood(nm, seed, [&](NavCelId x) {return (nm.navcel_centroid(x) - c).norm() <= r;}); + want.insert(seed); // always visited: it holds the centre + EXPECT_EQ(got, want) << q << " r " << r; + } +} + +TEST(Iterators, DiscNeverReachesAnotherFloor) +{ + const NavMap nm = two_floors(); + for (const float z : {0.0f, 3.0f}) { + const Eigen::Vector3f c(5.1f, 4.9f, z); + for (const Metric m : {Metric::EUCLIDEAN, Metric::GEODESIC}) { + const auto got = collect( + [&](auto f) {return navmap::for_each_in_disc(nm, c, 20.0f, f, m);}); + EXPECT_EQ(got.size(), 800u); // the whole floor, and only it + for (NavCelId x : got) { + EXPECT_NEAR(nm.navcel_centroid(x).z(), z, 1e-5f); + } + } + } +} + +TEST(Iterators, DiscEdgeValues) +{ + const NavMap nm = flat_map(); + const Eigen::Vector3f c(5.1f, 4.9f, 0.0f); + const NavCelId seed = seed_at(nm, c); + for (const Metric m : {Metric::EUCLIDEAN, Metric::GEODESIC}) { + // Radius 0, or below the seed's centroid: just the seed + EXPECT_EQ(collect([&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 0.0f, f, m);}), + std::set{seed}); + // Negative or NaN radius, bad seed: nothing + EXPECT_EQ(navmap::for_each_in_disc(nm, seed, c, -1.0f, [](NavCelId) {}, m), 0u); + EXPECT_EQ(navmap::for_each_in_disc(nm, seed, c, NAN, [](NavCelId) {}, m), 0u); + EXPECT_EQ(navmap::for_each_in_disc(nm, NavCelId{100000}, c, 1.0f, [](NavCelId) {}, m), 0u); + // Off the map: nothing + EXPECT_EQ(navmap::for_each_in_disc(nm, Eigen::Vector3f(50, 50, 0), 3.0f, [](NavCelId) {}, m), + 0u); + // Early stop + int calls = 0; + EXPECT_EQ(navmap::for_each_in_disc(nm, seed, c, 3.0f, [&](NavCelId) {return ++calls < 5;}, m), + 5u); + } + EXPECT_EQ(navmap::for_each_in_disc(NavMap{}, Eigen::Vector3f(0, 0, 0), 1.0f, [](NavCelId) {}), + 0u); +} + +TEST(Iterators, GeodesicDiscDoesNotCrossAWallThatEuclideanSees) +{ + // A wall (missing column x in [5, 5.5)) with a gap at the far end: the cells right behind the + // wall are 1 m away in a straight line but ~19 m along the surface. + NavMap nm; + add_grid(nm, 20, 20, 0.5f, flat(0.0f), [](int i, int j) {return i == 10 && j < 19;}); + nm.rebuild_geometry_accels(); + const Eigen::Vector3f c(4.75f, 2.0f, 0.0f), behind(5.75f, 2.0f, 0.0f); + const NavCelId seed = seed_at(nm, c), b = seed_at(nm, behind); + const auto euclid = collect( + [&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 3.0f, f, Metric::EUCLIDEAN);}); + const auto geo = collect( + [&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 3.0f, f, Metric::GEODESIC);}); + // Both grow through the surface, so neither crosses the wall within 3 m... + EXPECT_FALSE(euclid.count(b)); + EXPECT_FALSE(geo.count(b)); + // ...but with a radius that reaches the gap, Euclidean wraps around and geodesic does not. + const auto e_far = collect( + [&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 9.5f, f, Metric::EUCLIDEAN);}); + const auto g_far = collect( + [&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 9.5f, f, Metric::GEODESIC);}); + EXPECT_TRUE(e_far.count(b)); + EXPECT_FALSE(g_far.count(b)); + for (NavCelId x : g_far) { + EXPECT_TRUE(e_far.count(x)) << x; + } // geodesic >= Euclidean +} + +TEST(Iterators, GeodesicDistanceAroundAWallIsTheShortestDetour) +{ + // Wall x in [5, 5.5), y in [0, 9.5), gap above it. From (4.75, 2) to (5.75, 2) the shortest + // path on the surface goes over the wall's end: about 7.5 + 0.5 + 7.5 = 15.5 m. + NavMap nm; + add_grid(nm, 20, 20, 0.5f, flat(0.0f), [](int i, int j) {return i == 10 && j < 19;}); + nm.rebuild_geometry_accels(); + const Eigen::Vector3f c(4.75f, 2.0f, 0.0f); + const NavCelId seed = seed_at(nm, c), b = seed_at(nm, {5.75f, 2.0f, 0.0f}); + float db = -1.0f; + navmap::for_each_by_distance(nm, seed, c, 30.0f, [&](NavCelId x, float d) { + if (x == b) {db = d;} + }); + const Eigen::Vector3f pb = nm.navcel_centroid(b); + const float detour = (Eigen::Vector3f(5.0f, 9.5f, 0) - c).norm() + 0.5f + + (pb - Eigen::Vector3f(5.5f, 9.5f, 0)).norm(); + EXPECT_GE(db, detour - 1e-3f); // never through the wall + EXPECT_LE(db, detour + 2 * 0.5f); // bends at centroids: about a NavCel per bend +} + +TEST(Iterators, RingIsTheDiscMinusItsInnerPartAndCrossesIt) +{ + const NavMap nm = flat_map(); + const Eigen::Vector3f c(5.1f, 4.9f, 0.0f); + const NavCelId seed = seed_at(nm, c); + for (const Metric m : {Metric::EUCLIDEAN, Metric::GEODESIC}) { + const auto ring = collect( + [&](auto f) {return navmap::for_each_in_ring(nm, seed, c, 2.0f, 3.0f, f, m);}); + std::set want; + navmap::for_each_by_distance(nm, seed, c, 3.0f, [&](NavCelId x, float d) { + if (m == Metric::GEODESIC ? d >= 2.0f : (nm.navcel_centroid(x) - c).norm() >= 2.0f) { + want.insert(x); + } + }); + if (m == Metric::EUCLIDEAN) { + want = flood(nm, seed, [&](NavCelId x) {return (nm.navcel_centroid(x) - c).norm() <= 3.0f;}); + std::erase_if(want, [&](NavCelId x) {return (nm.navcel_centroid(x) - c).norm() < 2.0f;}); + } + EXPECT_EQ(ring, want); + EXPECT_FALSE(ring.count(seed)); + EXPECT_GT(ring.size(), 50u); // the inner 2 m were crossed to get there + } + // Empty ring (r_in > r_out) and r_in = 0 (= disc without forcing the seed) + EXPECT_EQ(navmap::for_each_in_ring(nm, seed, c, 3.0f, 2.0f, [](NavCelId) {}), 0u); + EXPECT_EQ( + navmap::for_each_in_ring(nm, seed, c, 0.0f, 3.0f, [](NavCelId) {}), + navmap::for_each_in_disc(nm, seed, c, 3.0f, [](NavCelId) {})); +} + +// ------------------------------------------------------------------ distance order + +TEST(Iterators, ByDistanceIsOrderedCompleteAndNeverShorterThanAStraightLine) +{ + const NavMap nm = flat_map(); + const Eigen::Vector3f c(3.3f, 6.1f, 0.0f); + const NavCelId seed = seed_at(nm, c); + std::vector> seq; + const std::size_t n = navmap::for_each_by_distance(nm, seed, c, 4.0f, [&](NavCelId x, float d) { + seq.emplace_back(x, d); + }); + ASSERT_EQ(n, seq.size()); + ASSERT_FALSE(seq.empty()); + EXPECT_EQ(seq.front().first, seed); + std::set ids; + for (std::size_t k = 0; k < seq.size(); ++k) { + EXPECT_TRUE(ids.insert(seq[k].first).second); + if (k > 0) {EXPECT_GE(seq[k].second, seq[k - 1].second - 0.5f);} // up to a neighbour step + EXPECT_GE(seq[k].second, (nm.navcel_centroid(seq[k].first) - c).norm() - 1e-4f); + if (k > 0) {EXPECT_LE(seq[k].second, 4.0f);} + } + // On a flat open grid the geodesic is the straight line + float worst = 0.0f; + for (const auto & [x, d] : seq) { + const float straight = (nm.navcel_centroid(x) - c).norm(); + if (straight > 1.0f) {worst = std::max(worst, d / straight);} + } + std::printf("worst geodesic / straight on a flat grid: %.3f\n", worst); + EXPECT_LE(worst, 1.001f); + // Every NavCel with a short straight line to the centre is there + for (NavCelId x = 0; x < nm.navcels.size(); ++x) { + if ((nm.navcel_centroid(x) - c).norm() <= 3.0f) {EXPECT_TRUE(ids.count(x)) << x;} + } + int calls = 0; + EXPECT_EQ(navmap::for_each_by_distance(nm, seed, c, 4.0f, [&](NavCelId, float) { + return ++calls < 3; + }), 3u); +} + +TEST(Iterators, ByDistanceOnASlopeMeasuresAlongTheSurface) +{ + // 30 degree slope along x: a cell 4 m away in XY is 4 / cos(30) = 4.62 m away on the surface. + NavMap nm; + const float k = std::tan(30.0f * static_cast(M_PI) / 180.0f); + add_grid(nm, 40, 4, 0.25f, [k](float x, float) {return k * x;}); + nm.rebuild_geometry_accels(); + const Eigen::Vector3f c(1.0f, 0.5f, k * 1.0f), far(5.0f, 0.5f, k * 5.0f); + const NavCelId seed = seed_at(nm, c), target = seed_at(nm, far); + float d_target = -1.0f; + navmap::for_each_by_distance(nm, seed, c, 10.0f, [&](NavCelId x, float d) { + if (x == target) {d_target = d;} + }); + const float straight = (nm.navcel_centroid(target) - c).norm(); + EXPECT_NEAR(d_target, straight, 1e-3f); // a plane: the straight line is the geodesic + EXPECT_GT(d_target, 4.4f); // the XY distance (4 m) would be wrong +} + +// ------------------------------------------------------------------ polygon + +TEST(Iterators, PolygonMatchesBruteForceIncludingConcaveOnes) +{ + const NavMap nm = flat_map(); + const std::vector> polys = { + {{1, 1}, {6, 1}, {6, 4}, {1, 4}}, // rectangle + {{1, 1}, {8, 1}, {8, 3}, {3, 3}, {3, 8}, {1, 8}}, // L (concave) + {{5, 0.5f}, {9.5f, 5}, {5, 9.5f}, {0.5f, 5}}, // rotated square + }; + for (const auto & poly : polys) { + const Eigen::Vector3f inside(poly[0].x() + 0.3f, poly[0].y() + 0.4f, 0.0f); + const NavCelId seed = seed_at(nm, inside); + const auto got = collect( + [&](auto f) {return navmap::for_each_in_polygon(nm, seed, poly, f);}); + std::set want; + for (NavCelId x = 0; x < nm.navcels.size(); ++x) { + const auto p = nm.navcel_centroid(x); + if (navmap::detail::in_polygon(poly, p.x(), p.y())) {want.insert(x);} + } + EXPECT_EQ(got, want); + } +} + +TEST(Iterators, PolygonStaysOnTheSeedLevelAndHandlesDegenerateInput) +{ + const NavMap nm = two_floors(); + const std::vector sq{{2, 2}, {6, 2}, {6, 6}, {2, 6}}; + for (const float z : {0.0f, 3.0f}) { + const auto got = collect([&](auto f) { + return navmap::for_each_in_polygon(nm, Eigen::Vector3f(4, 4, z), sq, f); + }); + EXPECT_EQ(got.size(), 2u * 8u * 8u); + for (NavCelId x : got) { + EXPECT_NEAR(nm.navcel_centroid(x).z(), z, 1e-5f); + } + } + const NavCelId seed = seed_at(nm, {4, 4, 0}); + EXPECT_EQ(navmap::for_each_in_polygon(nm, seed, {{0, 0}, {1, 1}}, [](NavCelId) {}), 0u); + EXPECT_EQ(navmap::for_each_in_polygon(nm, seed, {}, [](NavCelId) {}), 0u); + // A polygon away from the seed is not reached (regions grow from the seed) + EXPECT_EQ(navmap::for_each_in_polygon( + nm, seed, {{8, 8}, {9, 8}, {9, 9}, {8, 9}}, [](NavCelId) {}), 0u); + EXPECT_EQ(navmap::for_each_in_polygon( + nm, Eigen::Vector3f(50, 50, 0), sq, [](NavCelId) {}), 0u); + int calls = 0; + EXPECT_EQ(navmap::for_each_in_polygon(nm, seed, sq, [&](NavCelId) {return ++calls < 4;}), 4u); +} + +// ------------------------------------------------------------------ segment walk + +namespace +{ + +struct Walk +{ + bool reached; + std::vector> cells; +}; + +Walk walk(const NavMap & nm, const Eigen::Vector3f & a, const Eigen::Vector3f & b) +{ + Walk w; + w.reached = navmap::for_each_on_segment(nm, a, b, [&](NavCelId c, float t0, float t1) { + w.cells.emplace_back(c, t0, t1); + }); + return w; +} + +} // namespace + +TEST(Iterators, SegmentWalkCoversTheSegmentAndMatchesTheCrossedCells) +{ + const NavMap nm = flat_map(); + std::mt19937 rng(3); + std::uniform_real_distribution U(0.05f, 9.95f); + for (int q = 0; q < 300; ++q) { + const Eigen::Vector3f a(U(rng), U(rng), 0.0f), b(U(rng), U(rng), 0.0f); + const Walk w = walk(nm, a, b); + ASSERT_TRUE(w.reached) << q; + ASSERT_FALSE(w.cells.empty()); + EXPECT_FLOAT_EQ(std::get<1>(w.cells.front()), 0.0f); + EXPECT_FLOAT_EQ(std::get<2>(w.cells.back()), 1.0f); + std::set seen; + for (std::size_t k = 0; k < w.cells.size(); ++k) { + const auto [c, t0, t1] = w.cells[k]; + EXPECT_TRUE(seen.insert(c).second) << "cell visited twice"; + EXPECT_LE(t0, t1); + if (k > 0) {EXPECT_NEAR(t0, std::get<2>(w.cells[k - 1]), 1e-5f);} // contiguous + } + // Same set as every triangle the segment crosses with positive length + std::set want; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (overlap_length(nm, c, a.head<2>(), b.head<2>()) > 1e-4f) {want.insert(c);} + } + for (NavCelId c : want) { + EXPECT_TRUE(seen.count(c)) << q << " missing " << c; + } + // Every visited cell holds the middle of its own interval (crossed, or touched at a vertex) + for (const auto & [c, t0, t1] : w.cells) { + const Eigen::Vector3f m = a + 0.5f * (t0 + t1) * (b - a); + const Eigen::Vector2f e(1e-3f, 0.0f); + EXPECT_TRUE( + want.count(c) || + overlap_length(nm, c, m.head<2>() - e, m.head<2>() + e) > 0.0f || + overlap_length(nm, c, m.head<2>() - e.reverse(), m.head<2>() + e.reverse()) > 0.0f) + << q << " cell " << c << " not on the segment"; + } + } +} + +TEST(Iterators, SegmentWalkAlongMeshEdgesAndThroughVertices) +{ + const NavMap nm = flat_map(); + // Along the diagonals of the grid (exactly on triangle edges) and along grid lines + for (const auto & [a, b] : std::vector>{ + {{0.5f, 0.5f, 0}, {9.5f, 9.5f, 0}}, {{9.5f, 9.5f, 0}, {0.5f, 0.5f, 0}}, + {{1.0f, 0.25f, 0}, {1.0f, 9.75f, 0}}, {{0.25f, 3.0f, 0}, {9.75f, 3.0f, 0}}, + {{2.0f, 2.0f, 0}, {2.0f, 2.0f, 0}}}) + { + const Walk w = walk(nm, a, b); + EXPECT_TRUE(w.reached) << a.transpose() << " -> " << b.transpose(); + EXPECT_LE(w.cells.size(), 4u * 20u); // no loops + EXPECT_FLOAT_EQ(std::get<2>(w.cells.back()), 1.0f); + } + const Walk point = walk(nm, {2.2f, 2.3f, 0}, {2.2f, 2.3f, 0}); // a == b: one cell + ASSERT_EQ(point.cells.size(), 1u); + EXPECT_FLOAT_EQ(std::get<1>(point.cells[0]), 0.0f); + EXPECT_FLOAT_EQ(std::get<2>(point.cells[0]), 1.0f); +} + +TEST(Iterators, SegmentWalkStopsAtBordersAndHoles) +{ + NavMap nm; + add_grid(nm, 20, 20, 0.5f, flat(0.0f), [](int i, int j) {return i == 10 && j >= 5 && j < 15;}); + nm.rebuild_geometry_accels(); + // Across the hole + Walk w = walk(nm, {2, 5, 0}, {8, 5, 0}); + EXPECT_FALSE(w.reached); + ASSERT_FALSE(w.cells.empty()); + EXPECT_LT(std::get<2>(w.cells.back()), 0.55f); + EXPECT_GT(std::get<2>(w.cells.back()), 0.45f); + // Leaving the map + w = walk(nm, {2, 1, 0}, {15, 1, 0}); + EXPECT_FALSE(w.reached); + EXPECT_NEAR(std::get<2>(w.cells.back()) * 13.0f + 2.0f, 10.0f, 1e-3f); + // Around the hole, below it: reaches + EXPECT_TRUE(walk(nm, {2, 1, 0}, {8, 1, 0}).reached); + // Off the map start + EXPECT_FALSE(walk(nm, {-3, 1, 0}, {2, 1, 0}).reached); + // Early stop + int calls = 0; + EXPECT_FALSE(navmap::for_each_on_segment(nm, Eigen::Vector3f(1, 1, 0), Eigen::Vector3f(9, 1, 0), + [&](NavCelId, float, float) {return ++calls < 3;})); + EXPECT_EQ(calls, 3); +} + +TEST(Iterators, SegmentWalkStaysOnItsFloorAndFollowsSlopes) +{ + const NavMap nm = two_floors(); + for (const float z : {0.0f, 3.0f}) { + const Walk w = walk(nm, {1, 1, z}, {9, 8, z}); + EXPECT_TRUE(w.reached); + for (const auto & [c, t0, t1] : w.cells) { + EXPECT_NEAR(nm.navcel_centroid(c).z(), z, 1e-5f); + } + } + NavMap ramp; + add_grid(ramp, 40, 4, 0.25f, [](float x, float) {return 0.3f * x;}); + ramp.rebuild_geometry_accels(); + const Walk up = walk(ramp, {0.5f, 0.5f, 0.15f}, {9.5f, 0.6f, 2.85f}); + EXPECT_TRUE(up.reached); + float zprev = -1.0f; + for (const auto & [c, t0, t1] : up.cells) { + const float zc = ramp.navcel_centroid(c).z(); + EXPECT_GE(zc, zprev - 0.1f); // climbs + zprev = zc; + } +} + +// ------------------------------------------------------------------ scratch reuse + +TEST(Iterators, ScratchReuseGivesTheSameResultsAcrossMapsAndCalls) +{ + const NavMap big = flat_map(30), small = flat_map(6); + TraversalScratch sc; + for (int round = 0; round < 3; ++round) { + for (const NavMap * nm : {&big, &small}) { + const Eigen::Vector3f c(1.3f, 1.6f, 0.0f); + const NavCelId seed = seed_at(*nm, c); + for (const Metric m : {Metric::EUCLIDEAN, Metric::GEODESIC}) { + const auto with = collect( + [&](auto f) {return navmap::for_each_in_disc(*nm, seed, c, 1.5f, f, m, &sc);}); + const auto without = collect( + [&](auto f) {return navmap::for_each_in_disc(*nm, seed, c, 1.5f, f, m);}); + EXPECT_EQ(with, without); + } + const std::vector sq{{0.6f, 0.6f}, {2, 0.6f}, {2, 2}, {0.6f, 2}}; + EXPECT_EQ( + navmap::for_each_in_polygon(*nm, seed, sq, [](NavCelId) {}, &sc), + navmap::for_each_in_polygon(*nm, seed, sq, [](NavCelId) {})); + AABB box; + box.expand({0, 0, -1}); + box.expand({2, 2, 1}); + EXPECT_EQ( + navmap::for_each_in_aabb(*nm, box, [](NavCelId) {}, &sc), + navmap::for_each_in_aabb(*nm, box, [](NavCelId) {})); + } + } +} From 2c77644d6c2b091f832d4e22d643492f68bfb085 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 09:23:23 +0200 Subject: [PATCH 02/11] Add unknown values, int16 layers and log-odds occupancy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Each layer can declare the value meaning "not observed" (NaN allowed): NavMap::set_layer_unknown() / is_unknown(). The unknown value, description and unit now travel in NavMapLayer and in the file; from_occupancy_grid declares NO_INFORMATION as the occupancy layer's unknown value. - int16 layers (LayerType::I16, NavMapLayer::I16 / data_i16). - navmap_core/Occupancy.hpp: log-odds occupancy with OctoMap's clamped update, in fixed point over an int16 layer, and a free / occupied / unknown state. - .navmap file format version 2. Version 1 files (and files without header) load through frozen NavMapV1 / NavMapLayerV1 messages, with a deprecation warning, and are rewritten in place when writable. resample.py reads them too. - RViz2: int16 layers (log-odds as occupancy greys), and cells holding a layer's unknown value in translucent grey; U8 layers unchanged. - Fixes: add_layer() ignored the default value of a new layer, and integer conversions clamped negative values to 0. - Example 04_unknown_and_occupancy with its launch file and RViz2 config. Signed-off-by: Francisco Martín Rico --- README.md | 22 ++ navmap_core/include/navmap_core/NavMap.hpp | 43 ++- navmap_core/include/navmap_core/Occupancy.hpp | 137 +++++++++ navmap_core/src/navmap_core/NavMap.cpp | 24 ++ navmap_core/tests/CMakeLists.txt | 5 + .../tests/test_layers_unknown_occupancy.cpp | 242 +++++++++++++++ navmap_examples/CMakeLists.txt | 2 +- .../launch/unknown_occupancy.launch.py | 30 ++ navmap_examples/package.xml | 2 + navmap_examples/rviz/unknown_occupancy.rviz | 53 ++++ .../src_ros2/04_unknown_and_occupancy.cpp | 169 +++++++++++ navmap_ros/src/navmap_ros/conversions.cpp | 159 +++++----- navmap_ros/src/navmap_ros/navmap_io.cpp | 92 +++++- navmap_ros/tests/CMakeLists.txt | 5 + .../test_layer_metadata_and_migration.cpp | 286 ++++++++++++++++++ navmap_ros_interfaces/CMakeLists.txt | 2 + navmap_ros_interfaces/msg/NavMapLayer.msg | 10 +- navmap_ros_interfaces/msg/NavMapLayerV1.msg | 16 + navmap_ros_interfaces/msg/NavMapV1.msg | 26 ++ .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 65 +++- navmap_tools/navmap_tools/resample.py | 60 +++- navmap_tools/test/test_resample.py | 63 +++- 22 files changed, 1381 insertions(+), 132 deletions(-) create mode 100644 navmap_core/include/navmap_core/Occupancy.hpp create mode 100644 navmap_core/tests/test_layers_unknown_occupancy.cpp create mode 100644 navmap_examples/launch/unknown_occupancy.launch.py create mode 100644 navmap_examples/rviz/unknown_occupancy.rviz create mode 100644 navmap_examples/src_ros2/04_unknown_and_occupancy.cpp create mode 100644 navmap_ros/tests/test_layer_metadata_and_migration.cpp create mode 100644 navmap_ros_interfaces/msg/NavMapLayerV1.msg create mode 100644 navmap_ros_interfaces/msg/NavMapV1.msg diff --git a/README.md b/README.md index 11b066e..c92c53d 100644 --- a/README.md +++ b/README.md @@ -28,6 +28,7 @@ Funded by the European Union through the Horizon Europe programme under Grant Ag - **Locate API**: find the NavCel under/around a 3D position using BVH acceleration and raycasting. - **Raytracing**: Möller–Trumbore intersection with a simple BVH for efficiency. - **Multi-surface support**: naturally supports multiple disconnected surfaces (e.g., separate floors). +- **Unknown values and probabilistic occupancy**: each layer can declare the value that means "not observed"; `Occupancy.hpp` keeps log-odds occupancy (OctoMap's clamped update) in an int16 layer, with free / occupied / unknown states. - **Iterators** over NavCels (`navmap_core/Iterators.hpp`): box, disc, ring, polygon, distance order and segment walk; regions grow over the surface, so they never jump to another floor. --- @@ -256,6 +257,25 @@ Also `for_each_in_aabb` (every surface, via the BVH), `for_each_in_ring` and `fo --- +## 7c. Unknown values and log-odds occupancy (`Occupancy.hpp`) + +```cpp +#include + +nm.add_layer("elevation", "terrain height", "m", 0.0f); +nm.set_layer_unknown("elevation", std::numeric_limits::quiet_NaN()); // NaN = not surveyed +bool unseen = nm.is_unknown("elevation", cid); + +navmap::OccupancyLayer occ(nm); // int16 layer "occupancy_logodds", all unknown +occ.miss(crossed_cid); // a ray crossed it +occ.hit(end_cid); // a ray ended on it +if (occ.state(cid) == navmap::OccupancyState::OCCUPIED) {/* ... */} +``` + +The unknown value, description and unit of each layer travel in the message and in the file. Which NavCels a ray hits or crosses is up to the algorithm using it (see `for_each_on_segment`). `ros2 launch navmap_examples unknown_occupancy.launch.py` shows both in RViz2 (build with `-DBUILD_ROS_EXAMPLES=ON`). + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: @@ -289,6 +309,8 @@ navmap::NavMap nm3 = navmap_ros::loadMapFromYaml("/tmp/navmap.yaml"); --- +> **File format.** `.navmap` files are version 2 (layer metadata and int16 layers). Version 1 files, and files without header, still load: they are migrated with a deprecation warning and rewritten in place when the location is writable. + ## 10. Classic low-level API For advanced control you can still access internal data directly: diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 5c611c6..13ac76f 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -114,7 +114,7 @@ struct Colors * * The registry currently supports: 8-bit unsigned, 32-bit float, 64-bit double. */ -enum class LayerType : uint8_t { U8 = 0, F32 = 1, F64 = 2 }; +enum class LayerType : uint8_t { U8 = 0, F32 = 1, F64 = 2, I16 = 3 }; /** * \brief Non-templated base for runtime layer handling. @@ -340,6 +340,7 @@ struct LayerMeta std::string description; ///< Human-readable description std::string unit; ///< Physical unit (e.g., "m", "deg", "%") bool per_cell{true}; ///< true for per-NavCel layers (current behavior) + std::optional unknown; ///< Value meaning "not observed" (NaN allowed), if any }; // ----------------------------------------------------------------------------- @@ -436,6 +437,7 @@ template inline constexpr LayerType layer_type_tag(); template<> inline constexpr LayerType layer_type_tag() {return LayerType::U8;} template<> inline constexpr LayerType layer_type_tag() {return LayerType::F32;} template<> inline constexpr LayerType layer_type_tag() {return LayerType::F64;} +template<> inline constexpr LayerType layer_type_tag() {return LayerType::I16;} /** * \brief Shape selector for area-writing APIs. @@ -746,11 +748,16 @@ class NavMap const std::string & unit = {}, T default_value = T{}) { + const bool existed = has_layer(name); auto v = layers.add_or_get(name, navcels.size(), layer_type_tag()); - if (v->data().size() != navcels.size()) { + if (!existed || v->data().size() != navcels.size()) { // new layers start at default_value v->data().assign(navcels.size(), default_value); + v->mark_dirty(); } - layer_meta[name] = LayerMeta{description, unit, true}; + auto & meta = layer_meta[name]; + meta.description = description; + meta.unit = unit; + meta.per_cell = true; return v; } @@ -789,7 +796,7 @@ class NavMap * * Si la capa no existe o \p cid está fuera de rango, devuelve \p def. * - * \tparam T uint8_t, float, or double + * \tparam T uint8_t, int16_t, float, or double * \param name Layer name * \param cid Triangle id * \param def Default value on failure @@ -822,6 +829,10 @@ class NavMap const auto & buf = vd->data(); if (static_cast(cid) >= buf.size()) {return def;} v = buf[cid]; + } else if (auto vi = std::dynamic_pointer_cast>(base)) { + const auto & buf = vi->data(); + if (static_cast(cid) >= buf.size()) {return def;} + v = static_cast(buf[cid]); } else { return def; } @@ -832,8 +843,8 @@ class NavMap if constexpr (std::is_floating_point_v) { return static_cast(v); } else if constexpr (std::is_integral_v) { - double lo = 0.0; - double hi = static_cast(std::numeric_limits::max()); + const double lo = static_cast(std::numeric_limits::lowest()); + const double hi = static_cast(std::numeric_limits::max()); double vv = v < lo ? lo : (v > hi ? hi : v); return static_cast(std::llround(vv)); } else { @@ -848,6 +859,15 @@ class NavMap */ std::optional get_layer_meta(const std::string & name) const; + /** + * \brief Declare the value of layer \p name that means "not observed" (NaN allowed). + * Travels with the map (message and file) and is kept by add_layer(). + */ + void set_layer_unknown(const std::string & name, double value); + + /// \brief Whether layer \p name holds its unknown value at \p cid (false if it has none). + bool is_unknown(const std::string & name, NavCelId cid) const; + /** * \brief List all layer names currently stored. * \return Vector of layer names. @@ -965,7 +985,10 @@ class NavMap if (dst->data().size() != navcels.size()) { dst->data().assign(navcels.size(), T{}); } - layer_meta[name] = LayerMeta{description, unit, true}; + auto & meta = layer_meta[name]; + meta.description = description; + meta.unit = unit; + meta.per_cell = true; if (!has_layer(src_layer)) { return dst; @@ -983,8 +1006,8 @@ class NavMap if constexpr (std::is_floating_point_v) { dst->data()[cid] = static_cast(v); } else if constexpr (std::is_integral_v) { - double lo = 0.0; - double hi = static_cast(std::numeric_limits::max()); + const double lo = static_cast(std::numeric_limits::lowest()); + const double hi = static_cast(std::numeric_limits::max()); if (v < lo) {v = lo;} if (v > hi) {v = hi;} dst->data()[cid] = static_cast(std::llround(v)); @@ -1019,7 +1042,7 @@ class NavMap * Efficiency: BFS from the seed NavCel with XY AABB pruning against the * circumscribed square of the area. Inclusion test by triangle centroid. * - * \tparam T uint8_t, float, or double + * \tparam T uint8_t, int16_t, float, or double * \param p_world 3D point in world coordinates (projected to ground) * \param value Value to set in the layer * \param layer_name Target layer name diff --git a/navmap_core/include/navmap_core/Occupancy.hpp b/navmap_core/include/navmap_core/Occupancy.hpp new file mode 100644 index 0000000..6320324 --- /dev/null +++ b/navmap_core/include/navmap_core/Occupancy.hpp @@ -0,0 +1,137 @@ +// 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. + +/** + * @file Occupancy.hpp + * @brief Probabilistic occupancy per NavCel: log-odds with clamping (as OctoMap), stored in an + * int16 layer in fixed point, and a three-state classification (as UFOMap). + * + * This is the update rule only. Which NavCels a measurement hits or traverses (the sensor + * model) is decided by the algorithm that uses it. + */ + +#ifndef NAVMAP_CORE__OCCUPANCY_HPP_ +#define NAVMAP_CORE__OCCUPANCY_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/// Probabilities of the update rule and of the classification (OctoMap defaults). +struct OccupancyParams +{ + float p_hit{0.7f}; ///< A measurement ends in the NavCel + float p_miss{0.4f}; ///< A measurement crosses the NavCel + float p_min{0.12f}; ///< Clamping: never more certain of free than this + float p_max{0.97f}; ///< Clamping: never more certain of occupied than this + float p_occupied{0.6f}; ///< At or above: OCCUPIED + float p_free{0.4f}; ///< At or below: FREE (in between: UNKNOWN) +}; + +/// Three-state classification of a NavCel. +enum class OccupancyState : uint8_t { UNKNOWN = 0, FREE = 1, OCCUPIED = 2 }; + +/** + * @brief Log-odds occupancy over an int16 layer of a NavMap. + * + * Values are log-odds in units of 1/kScale; 0 (probability 0.5) is "never observed" and is + * declared as the layer's unknown value. Updates add the hit/miss log-odds and clamp. + */ +class OccupancyLayer +{ +public: + static constexpr float kScale = 1000.0f; ///< int16 units per log-odds unit + + static int16_t to_fixed(float logodds) + { + const float v = std::round(logodds * kScale); + return static_cast(std::clamp(v, -32767.0f, 32767.0f)); + } + static float logodds_of(float p) {return std::log(p / (1.0f - p));} + static float probability_of(float logodds) {return 1.0f / (1.0f + std::exp(-logodds));} + + /// Uses (or creates, all unknown) the int16 layer @p name of @p nm. + explicit OccupancyLayer( + NavMap & nm, const std::string & name = "occupancy_logodds", OccupancyParams p = {}) + : name_(name), + hit_(to_fixed(logodds_of(p.p_hit))), miss_(to_fixed(logodds_of(p.p_miss))), + min_(to_fixed(logodds_of(p.p_min))), max_(to_fixed(logodds_of(p.p_max))), + occ_(to_fixed(logodds_of(p.p_occupied))), free_(to_fixed(logodds_of(p.p_free))) + { + const bool existed = nm.has_layer(name); + view_ = nm.add_layer(name, "occupancy log-odds", "1/1000 log-odds", 0); + if (!existed) {nm.set_layer_unknown(name, 0.0);} + } + + /// A measurement ended in @p cid (occupied evidence). + void hit(NavCelId cid) {add(cid, hit_);} + /// A measurement crossed @p cid (free evidence). + void miss(NavCelId cid) {add(cid, miss_);} + void update(NavCelId cid, bool occupied) {add(cid, occupied ? hit_ : miss_);} + + /// Adds @p delta (fixed point) to @p cid and clamps to [p_min, p_max]. + void add(NavCelId cid, int16_t delta) + { + auto & d = view_->data(); + if (cid >= d.size()) {return;} + const int v = static_cast(d[cid]) + delta; + d[cid] = static_cast(std::clamp(v, static_cast(min_), static_cast(max_))); + view_->mark_dirty(); + } + + /// Forgets @p cid (back to unknown). + void reset(NavCelId cid) + { + auto & d = view_->data(); + if (cid < d.size()) {d[cid] = 0; view_->mark_dirty();} + } + + float logodds(NavCelId cid) const {return value(cid) / kScale;} + float probability(NavCelId cid) const {return probability_of(logodds(cid));} + + OccupancyState state(NavCelId cid) const + { + const int16_t v = value(cid); + if (v >= occ_) {return OccupancyState::OCCUPIED;} + if (v <= free_) {return OccupancyState::FREE;} + return OccupancyState::UNKNOWN; + } + + const std::string & name() const {return name_;} + const std::shared_ptr> & view() const {return view_;} + +private: + int16_t value(NavCelId cid) const + { + const auto & d = view_->data(); + return cid < d.size() ? d[cid] : int16_t{0}; + } + + std::string name_; + std::shared_ptr> view_; + int16_t hit_, miss_, min_, max_, occ_, free_; +}; + +} // namespace navmap + +#endif // NAVMAP_CORE__OCCUPANCY_HPP_ diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 85efaac..d314578 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -168,6 +168,14 @@ NavMap & NavMap::operator=(const NavMap & other) if (!same_hash) {dst->set_data(src->data());} break; } + case LayerType::I16: { + auto src = std::dynamic_pointer_cast>(src_base); + auto dst = layers.add_or_get(name, navcels.size(), layer_type_tag()); + const bool same_size = dst->data().size() == src->data().size(); + const bool same_hash = same_size && (dst->content_hash() == src->content_hash()); + if (!same_hash) {dst->set_data(src->data());} + break; + } default: break; } } @@ -1192,6 +1200,7 @@ std::string NavMap::layer_type_name(const std::string & name) const case LayerType::U8: return "uint8"; case LayerType::F32: return "float"; case LayerType::F64: return "double"; + case LayerType::I16: return "int16"; default: break; } } @@ -1205,6 +1214,21 @@ std::optional NavMap::get_layer_meta(const std::string & name) const return it->second; } +void NavMap::set_layer_unknown(const std::string & name, double value) +{ + layer_meta[name].unknown = value; +} + +bool NavMap::is_unknown(const std::string & name, NavCelId cid) const +{ + const auto it = layer_meta.find(name); + if (it == layer_meta.end() || !it->second.unknown) {return false;} + const double u = *it->second.unknown; + const double v = layer_get(name, cid, std::numeric_limits::quiet_NaN()); + if (std::isnan(u)) {return std::isnan(v) && has_layer(name) && cid < layer_size(name);} + return v == u; +} + std::vector NavMap::list_layers() const { return layers.list(); diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index 1ba033c..2b61c06 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -57,3 +57,8 @@ ament_add_gtest(test_iterators test_iterators.cpp) target_link_libraries(test_iterators ${PROJECT_NAME} ) + +ament_add_gtest(test_layers_unknown_occupancy test_layers_unknown_occupancy.cpp) +target_link_libraries(test_layers_unknown_occupancy + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_layers_unknown_occupancy.cpp b/navmap_core/tests/test_layers_unknown_occupancy.cpp new file mode 100644 index 0000000..8e2333d --- /dev/null +++ b/navmap_core/tests/test_layers_unknown_occupancy.cpp @@ -0,0 +1,242 @@ +// 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 "navmap_core/NavMap.hpp" +#include "navmap_core/Occupancy.hpp" + +using navmap::NavCelId; +using navmap::NavMap; +using navmap::OccupancyLayer; +using navmap::OccupancyParams; +using navmap::OccupancyState; + +namespace +{ + +// n x n cells of 1 m, two triangles each. +NavMap grid(int n = 4) +{ + NavMap nm; + const std::size_t s = nm.create_surface("map"); + for (int j = 0; j <= n; ++j) { + for (int i = 0; i <= n; ++i) { + nm.add_vertex({static_cast(i), static_cast(j), 0.0f}); + } + } + auto v = [&](int i, int j) {return static_cast(j * (n + 1) + i);}; + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + 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))); + } + } + nm.rebuild_geometry_accels(); + return nm; +} + +} // namespace + +// ------------------------------------------------------------------ int16 layers + +TEST(LayersI16, StoresSignedValuesAndConvertsBothWays) +{ + NavMap nm = grid(); + auto v = nm.add_layer("h", "height", "mm", int16_t{-5}); + EXPECT_EQ(nm.layer_type_name("h"), "int16"); + EXPECT_EQ(v->data().size(), nm.navcels.size()); + EXPECT_EQ(nm.layer_get("h", 0), -5); + nm.layer_set("h", 3, -32768); + nm.layer_set("h", 4, 32767); + EXPECT_EQ(nm.layer_get("h", 3), -32768); + EXPECT_DOUBLE_EQ(nm.layer_get("h", 3), -32768.0); // read through the double fallback + EXPECT_EQ(nm.layer_get("h", 3), 0); // clamped to the target range + EXPECT_EQ(nm.layer_get("h", 4), 255); + EXPECT_EQ(nm.layer_get("h", 9999, int16_t{7}), 7); // out of range: default + // Reading another layer type as int16 keeps negative values (they used to clamp to 0) + nm.add_layer("f", "", "", -12.6f); + EXPECT_EQ(nm.layer_get("f", 0), -13); + nm.layer_set("f", 1, 1e9f); + EXPECT_EQ(nm.layer_get("f", 1), 32767); + // And so does creating an int16 layer from a float one + auto conv = nm.add_layer("f16", "", "", std::string("f")); + EXPECT_EQ(conv->data()[0], -13); + EXPECT_EQ(conv->data()[1], 32767); +} + +TEST(LayersI16, CopiesAssignmentsAndHashes) +{ + NavMap nm = grid(); + auto v = nm.add_layer("h", "", "", int16_t{1}); + const auto h0 = v->content_hash(); + nm.layer_set("h", 2, -300); + EXPECT_NE(v->content_hash(), h0); + + NavMap copy(nm); + EXPECT_EQ(copy.layer_get("h", 2), -300); + NavMap assigned = grid(); + assigned = nm; + EXPECT_EQ(assigned.layer_get("h", 2), -300); + EXPECT_EQ(assigned.layer_type_name("h"), "int16"); + // Re-assigning after a change in the source updates the destination + nm.layer_set("h", 2, 42); + assigned = nm; + EXPECT_EQ(assigned.layer_get("h", 2), 42); +} + +// ------------------------------------------------------------------ unknown value + +TEST(LayerUnknown, DeclaredPerLayerForIntegersAndNaN) +{ + NavMap nm = grid(); + nm.add_layer("occ", "", "", uint8_t{0}); + nm.add_layer("elev", "", "m", 0.0f); + // Without a declared unknown value nothing is unknown + nm.layer_set("occ", 1, 255); + EXPECT_FALSE(nm.is_unknown("occ", 1)); + nm.set_layer_unknown("occ", 255.0); + EXPECT_TRUE(nm.is_unknown("occ", 1)); + EXPECT_FALSE(nm.is_unknown("occ", 0)); + // NaN as the unknown value of a float layer + nm.set_layer_unknown("elev", std::numeric_limits::quiet_NaN()); + nm.layer_set("elev", 2, std::numeric_limits::quiet_NaN()); + EXPECT_TRUE(nm.is_unknown("elev", 2)); + EXPECT_FALSE(nm.is_unknown("elev", 3)); + // Missing layer or cell: not unknown (nothing to say) + EXPECT_FALSE(nm.is_unknown("nope", 0)); + EXPECT_FALSE(nm.is_unknown("elev", 9999)); + EXPECT_FALSE(nm.is_unknown("occ", 9999)); + // Kept by add_layer (which used to reset the metadata) and by copies + nm.add_layer("occ", "occupancy", "%", uint8_t{0}); + ASSERT_TRUE(nm.get_layer_meta("occ")->unknown.has_value()); + EXPECT_EQ(*nm.get_layer_meta("occ")->unknown, 255.0); + EXPECT_EQ(nm.get_layer_meta("occ")->description, "occupancy"); + NavMap copy(nm); + EXPECT_TRUE(copy.is_unknown("occ", 1)); + EXPECT_TRUE(copy.is_unknown("elev", 2)); +} + +// ------------------------------------------------------------------ log-odds occupancy + +TEST(Occupancy, FixedPointValuesOfTheOctoMapDefaults) +{ + EXPECT_EQ(OccupancyLayer::to_fixed(OccupancyLayer::logodds_of(0.7f)), 847); + EXPECT_EQ(OccupancyLayer::to_fixed(OccupancyLayer::logodds_of(0.4f)), -405); + EXPECT_EQ(OccupancyLayer::to_fixed(OccupancyLayer::logodds_of(0.12f)), -1992); + EXPECT_EQ(OccupancyLayer::to_fixed(OccupancyLayer::logodds_of(0.97f)), 3476); + EXPECT_EQ(OccupancyLayer::to_fixed(1e9f), 32767); + EXPECT_EQ(OccupancyLayer::to_fixed(-1e9f), -32767); + EXPECT_NEAR(OccupancyLayer::probability_of(OccupancyLayer::logodds_of(0.3f)), 0.3f, 1e-6f); +} + +TEST(Occupancy, NewCellsAreUnknownAndTheLayerTravelsWithTheMap) +{ + NavMap nm = grid(); + OccupancyLayer occ(nm); + EXPECT_EQ(nm.layer_type_name("occupancy_logodds"), "int16"); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_EQ(occ.state(c), OccupancyState::UNKNOWN); + EXPECT_FLOAT_EQ(occ.probability(c), 0.5f); + EXPECT_TRUE(nm.is_unknown("occupancy_logodds", c)); + } + occ.hit(3); + EXPECT_FALSE(nm.is_unknown("occupancy_logodds", 3)); + // A second OccupancyLayer over the same layer sees (and keeps) its values + OccupancyLayer again(nm); + EXPECT_EQ(again.state(3), OccupancyState::OCCUPIED); + NavMap copy(nm); + EXPECT_EQ(copy.layer_get("occupancy_logodds", 3), 847); +} + +TEST(Occupancy, SequencesClampAndClassify) +{ + NavMap nm = grid(); + OccupancyLayer occ(nm); + // One hit: occupied; one miss: free (exactly at the threshold) + occ.hit(0); + EXPECT_EQ(occ.state(0), OccupancyState::OCCUPIED); + occ.miss(1); + EXPECT_EQ(occ.state(1), OccupancyState::FREE); + // Hit then miss: 442 (still occupied); one more miss: 37 (unknown) + occ.miss(0); + EXPECT_EQ(nm.layer_get(occ.name(), 0), 442); + EXPECT_EQ(occ.state(0), OccupancyState::OCCUPIED); + occ.miss(0); + EXPECT_EQ(nm.layer_get(occ.name(), 0), 37); + EXPECT_EQ(occ.state(0), OccupancyState::UNKNOWN); + // Saturation both ways, and recovery from saturation takes a bounded number of updates + for (int k = 0; k < 100; ++k) { + occ.hit(2); + } + EXPECT_EQ(nm.layer_get(occ.name(), 2), 3476); + EXPECT_NEAR(occ.probability(2), 0.97f, 1e-3f); + int misses = 0; + while (occ.state(2) != OccupancyState::FREE) {occ.miss(2); ++misses;} + EXPECT_EQ(misses, (3476 + 405 + 404) / 405); // from the top to <= -405 + for (int k = 0; k < 100; ++k) { + occ.miss(4); + } + EXPECT_EQ(nm.layer_get(occ.name(), 4), -1992); + // Reset and out-of-range cells + occ.reset(4); + EXPECT_EQ(occ.state(4), OccupancyState::UNKNOWN); + occ.hit(99999); + occ.reset(99999); + EXPECT_EQ(occ.state(99999), OccupancyState::UNKNOWN); +} + +TEST(Occupancy, MatchesTheFloatRuleWithinRounding) +{ + // The fixed-point layer must follow OctoMap's float update with clamping. + NavMap nm = grid(); + OccupancyLayer occ(nm); + std::mt19937 rng(7); + std::bernoulli_distribution B(0.6); + const float lh = OccupancyLayer::logodds_of(0.7f), lm = OccupancyLayer::logodds_of(0.4f); + const float lo = OccupancyLayer::logodds_of(0.12f), hi = OccupancyLayer::logodds_of(0.97f); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + float ref = 0.0f; + for (int k = 0; k < 200; ++k) { + const bool h = B(rng); + occ.update(c, h); + ref = std::clamp(ref + (h ? lh : lm), lo, hi); + ASSERT_NEAR(occ.logodds(c), ref, 0.0006f * (k + 1) + 1e-3f) << c << " step " << k; + } + } +} + +TEST(Occupancy, CustomParametersAndDirtyHash) +{ + NavMap nm = grid(); + OccupancyParams p; + p.p_hit = 0.9f; + p.p_occupied = 0.95f; + OccupancyLayer occ(nm, "occ2", p); + occ.hit(0); + EXPECT_EQ(occ.state(0), OccupancyState::UNKNOWN); // 0.9 < 0.95 + occ.hit(0); + EXPECT_EQ(occ.state(0), OccupancyState::OCCUPIED); + const auto h = occ.view()->content_hash(); + occ.miss(5); + EXPECT_NE(occ.view()->content_hash(), h); // updates invalidate the cached hash +} diff --git a/navmap_examples/CMakeLists.txt b/navmap_examples/CMakeLists.txt index ea94257..9464db0 100644 --- a/navmap_examples/CMakeLists.txt +++ b/navmap_examples/CMakeLists.txt @@ -57,6 +57,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/navmap_examples/launch/unknown_occupancy.launch.py b/navmap_examples/launch/unknown_occupancy.launch.py new file mode 100644 index 0000000..7477680 --- /dev/null +++ b/navmap_examples/launch/unknown_occupancy.launch.py @@ -0,0 +1,30 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# 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. + +"""Log-odds occupancy and unknown values: the example node and RViz2.""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + rviz = os.path.join(get_package_share_directory('navmap_examples'), 'rviz', + 'unknown_occupancy.rviz') + return LaunchDescription([ + Node(package='navmap_examples', executable='04_unknown_and_occupancy', output='screen'), + Node(package='rviz2', executable='rviz2', arguments=['-d', rviz]), + ]) diff --git a/navmap_examples/package.xml b/navmap_examples/package.xml index fbe202b..fbee6c5 100644 --- a/navmap_examples/package.xml +++ b/navmap_examples/package.xml @@ -19,6 +19,8 @@ rclcpp launch_ros + navmap_rviz_plugin + rviz2 ament_cmake_gtest ament_lint_auto diff --git a/navmap_examples/rviz/unknown_occupancy.rviz b/navmap_examples/rviz/unknown_occupancy.rviz new file mode 100644 index 0000000..e266e02 --- /dev/null +++ b/navmap_examples/rviz/unknown_occupancy.rviz @@ -0,0 +1,53 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Grid + Enabled: true + Name: Grid + Plane Cell Count: 25 + Cell Size: 1 + Offset: {X: 10, Y: 5, Z: 0} + Value: true + - Alpha: 1 + Class: navmap_rviz_plugin/NavMapDisplay + Color Scheme: Occupancy + Draw Normals: false + Enabled: true + Layer: occupancy_logodds + Name: NavMap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /navmap + Update Layer Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /navmap_updates + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + Value: true + Views: + Current: + Class: rviz_default_plugins/TopDownOrtho + Name: Current View + Scale: 40 + X: 10 + Y: 5 + Value: TopDownOrtho (rviz_default_plugins) +Window Geometry: + Height: 800 + Width: 1200 diff --git a/navmap_examples/src_ros2/04_unknown_and_occupancy.cpp b/navmap_examples/src_ros2/04_unknown_and_occupancy.cpp new file mode 100644 index 0000000..fb3a86b --- /dev/null +++ b/navmap_examples/src_ros2/04_unknown_and_occupancy.cpp @@ -0,0 +1,169 @@ +// 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. + +// Unknown values and log-odds occupancy: a simulated 2D lidar circles a 20 x 10 m floor with a few +// boxes. Each ray clears the NavCels it crosses (miss) and marks the one it ends on (hit), so the +// unknown floor (grey) turns free (white) or occupied (black). An "elevation" layer has a patch +// never surveyed, NaN, declared as its unknown value (translucent grey in RViz). +// +// Publishes the NavMap once on "navmap" and the layers on "navmap_updates". + +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/Iterators.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_core/Occupancy.hpp" +#include "navmap_ros/conversions.hpp" +#include "navmap_ros_interfaces/msg/nav_map.hpp" +#include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "rclcpp/rclcpp.hpp" + +using navmap::NavCelId; + +namespace +{ + +struct Box {float x0, y0, x1, y1;}; + +// Distance along (dx, dy) from (x, y) to the first box, or max_range. +float cast(const std::vector & boxes, float x, float y, float dx, float dy, float max_range) +{ + float best = max_range; + for (const auto & b : boxes) { + float t0 = 0.0f, t1 = max_range; + for (const auto & [o, d, lo, hi] : {std::array{x, dx, b.x0, b.x1}, + std::array{y, dy, b.y0, b.y1}}) + { + if (std::abs(d) < 1e-9f) { + if (o < lo || o > hi) {t0 = max_range + 1;} + continue; + } + float ta = (lo - o) / d, tb = (hi - o) / d; + if (ta > tb) {std::swap(ta, tb);} + t0 = std::max(t0, ta); + t1 = std::min(t1, tb); + } + if (t0 <= t1 && t0 < best) {best = t0;} + } + return best; +} + +} // namespace + +class UnknownOccupancyNode : public rclcpp::Node +{ +public: + UnknownOccupancyNode() + : Node("navmap_unknown_occupancy") + { + // 20 x 10 m floor, 0.2 m cells (two triangles each) + const int nx = 100, ny = 50; + const float res = 0.2f; + const std::size_t s = nm_.create_surface("map"); + for (int j = 0; j <= ny; ++j) { + for (int i = 0; i <= nx; ++i) { + nm_.add_vertex({i * res, j * res, 0.0f}); + } + } + auto v = [&](int i, int j) {return static_cast(j * (nx + 1) + i);}; + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + 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))); + } + } + nm_.rebuild_geometry_accels(); + + // Elevation: a gentle wave, except a patch never surveyed (NaN = unknown) + nm_.add_layer("elevation", "terrain height", "m", 0.0f); + nm_.set_layer_unknown("elevation", std::numeric_limits::quiet_NaN()); + for (NavCelId c = 0; c < nm_.navcels.size(); ++c) { + const auto p = nm_.navcel_centroid(c); + const bool surveyed = !(p.x() > 14.0f && p.y() > 6.0f); + nm_.layer_set( + "elevation", c, surveyed ? 0.5f + 0.5f * std::sin(p.x() * 0.6f) * std::cos(p.y() * 0.5f) : + std::numeric_limits::quiet_NaN()); + } + + occ_ = std::make_unique(nm_); // all unknown + + boxes_ = {{3, 2, 4, 3}, {8, 6, 9.5f, 7}, {12, 1.5f, 12.5f, 4}, {16, 3, 17, 4}}; + + rclcpp::QoS latched(1); + latched.transient_local().reliable(); + map_pub_ = create_publisher("navmap", latched); + layer_pub_ = create_publisher("navmap_updates", 10); + + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp = now(); + map_pub_->publish(navmap_ros::to_msg(nm_, h)); + timer_ = create_wall_timer(std::chrono::milliseconds(200), [this] {step();}); + } + +private: + void step() + { + // Robot on a circle around the map centre; 360 rays, 6 m range + const float a = 0.05f * static_cast(tick_++); + const Eigen::Vector3f robot(10.0f + 6.0f * std::cos(a), 5.0f + 3.0f * std::sin(a), 0.0f); + for (int k = 0; k < 360; ++k) { + const float th = static_cast(k) * static_cast(M_PI) / 180.0f; + const float dx = std::cos(th), dy = std::sin(th); + const float range = 6.0f; + const float d = cast(boxes_, robot.x(), robot.y(), dx, dy, range); + const Eigen::Vector3f end(robot.x() + d * dx, robot.y() + d * dy, 0.0f); + // Every NavCel the ray crosses is free, but the one it ends on + NavCelId last = std::numeric_limits::max(); + navmap::for_each_on_segment( + nm_, robot, end, [&](NavCelId c, float, float t_out) { + if (t_out < 1.0f) {occ_->miss(c);} else {last = c;} + }); + if (last != std::numeric_limits::max()) { + if (d < range) {occ_->hit(last);} else {occ_->miss(last);} + } + } + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp = now(); + layer_pub_->publish(navmap_ros::to_msg(nm_, occ_->name(), h)); + if (tick_ % 10 == 1) { + layer_pub_->publish(navmap_ros::to_msg(nm_, "elevation", h)); + map_pub_->publish(navmap_ros::to_msg(nm_, h)); // late joiners see the current state + } + } + + navmap::NavMap nm_; + std::unique_ptr occ_; + std::vector boxes_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr layer_pub_; + rclcpp::TimerBase::SharedPtr timer_; + int tick_{0}; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 0ae1cf4..055c6de 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -71,6 +71,74 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 // ----------------- NavMap <-> ROS message ----------------- +namespace +{ + +// Layer data and metadata -> message. +void fill_layer_msg( + const navmap::NavMap & nm, const std::string & name, const navmap::LayerViewBase & base, + NavMapLayer & lmsg) +{ + lmsg.name = name; + lmsg.type = static_cast(base.type()); + switch (base.type()) { + case navmap::LayerType::U8: + lmsg.data_u8 = static_cast &>(base).data(); + break; + case navmap::LayerType::F32: + lmsg.data_f32 = static_cast &>(base).data(); + break; + case navmap::LayerType::F64: + lmsg.data_f64 = static_cast &>(base).data(); + break; + case navmap::LayerType::I16: + lmsg.data_i16 = static_cast &>(base).data(); + break; + default: + throw std::runtime_error("to_msg(NavMapLayer): unsupported layer type"); + } + if (const auto meta = nm.get_layer_meta(name)) { + lmsg.description = meta->description; + lmsg.unit = meta->unit; + lmsg.has_unknown = meta->unknown.has_value(); + lmsg.unknown_value = meta->unknown.value_or(0.0); + } +} + +template +void set_layer(navmap::NavMap & nm, const NavMapLayer & msg, const V & data) +{ + using T = typename V::value_type; + auto dst = nm.add_layer(msg.name, msg.description, msg.unit, T{}); + dst->data().assign(data.begin(), data.end()); + dst->mark_dirty(); +} + +// Message -> layer data and metadata (the layer is created or retyped as needed). +void apply_layer_msg(const NavMapLayer & msg, navmap::NavMap & nm) +{ + if (auto existing = nm.layers.get(msg.name); + existing && static_cast(existing->type()) != msg.type) + { + nm.layers.remove(msg.name); + } + switch (msg.type) { + case NavMapLayer::U8: set_layer(nm, msg, msg.data_u8); break; + case NavMapLayer::F32: set_layer(nm, msg, msg.data_f32); break; + case NavMapLayer::F64: set_layer(nm, msg, msg.data_f64); break; + case NavMapLayer::I16: set_layer(nm, msg, msg.data_i16); break; + default: + throw std::runtime_error("from_msg(NavMapLayer): unsupported type value " + + std::to_string(msg.type)); + } + auto & meta = nm.layer_meta[msg.name]; + meta.description = msg.description; + meta.unit = msg.unit; + if (msg.has_unknown) {meta.unknown = msg.unknown_value;} else {meta.unknown.reset();} +} + +} // namespace + NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) { NavMap out; @@ -117,23 +185,7 @@ NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) if (!base) {continue;} NavMapLayer lmsg; - lmsg.name = lname; - lmsg.type = static_cast(base->type()); - - switch (base->type()) { - case navmap::LayerType::U8: { - auto v = std::dynamic_pointer_cast>(base); - lmsg.data_u8 = v->data(); - } break; - case navmap::LayerType::F32: { - auto v = std::dynamic_pointer_cast>(base); - lmsg.data_f32 = v->data(); - } break; - case navmap::LayerType::F64: { - auto v = std::dynamic_pointer_cast>(base); - lmsg.data_f64 = v->data(); - } break; - } + fill_layer_msg(nm, lname, *base, lmsg); out.layers.push_back(std::move(lmsg)); } @@ -194,20 +246,7 @@ navmap::NavMap from_msg(const NavMap & msg, std_msgs::msg::Header & header) // layers for (const auto & l : msg.layers) { - switch (l.type) { - case 0: { - auto v = nm.layers.add_or_get(l.name, ntris, navmap::LayerType::U8); - v->data() = l.data_u8; - } break; - case 1: { - auto v = nm.layers.add_or_get(l.name, ntris, navmap::LayerType::F32); - v->data() = l.data_f32; - } break; - case 2: { - auto v = nm.layers.add_or_get(l.name, ntris, navmap::LayerType::F64); - v->data() = l.data_f64; - } break; - } + apply_layer_msg(l, nm); } nm.rebuild_geometry_accels(); @@ -233,30 +272,7 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( if (!base) { throw std::runtime_error("to_msg(NavMapLayer): layer '" + layer_name + "' not found"); } - - switch (base->type()) { - case navmap::LayerType::U8: { - msg.type = navmap_ros_interfaces::msg::NavMapLayer::U8; - auto v = std::dynamic_pointer_cast>(base); - msg.data_u8 = v->data(); - break; - } - case navmap::LayerType::F32: { - msg.type = navmap_ros_interfaces::msg::NavMapLayer::F32; - auto v = std::dynamic_pointer_cast>(base); - msg.data_f32 = v->data(); - break; - } - case navmap::LayerType::F64: { - msg.type = navmap_ros_interfaces::msg::NavMapLayer::F64; - auto v = std::dynamic_pointer_cast>(base); - msg.data_f64 = v->data(); - break; - } - default: - throw std::runtime_error("to_msg(NavMapLayer): unsupported layer type"); - } - + fill_layer_msg(nm, layer_name, *base, msg); return msg; } @@ -273,35 +289,7 @@ void from_msg( std_msgs::msg::Header & header) { header = msg.header; - switch (msg.type) { - case navmap_ros_interfaces::msg::NavMapLayer::U8: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", uint8_t{}); - if (dst->data().size() != msg.data_u8.size()) { - dst->data().resize(msg.data_u8.size()); - } - std::copy(msg.data_u8.begin(), msg.data_u8.end(), dst->data().begin()); - break; - } - case navmap_ros_interfaces::msg::NavMapLayer::F32: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", 0.0f); - if (dst->data().size() != msg.data_f32.size()) { - dst->data().resize(msg.data_f32.size()); - } - std::copy(msg.data_f32.begin(), msg.data_f32.end(), dst->data().begin()); - break; - } - case navmap_ros_interfaces::msg::NavMapLayer::F64: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", 0.0); - if (dst->data().size() != msg.data_f64.size()) { - dst->data().resize(msg.data_f64.size()); - } - std::copy(msg.data_f64.begin(), msg.data_f64.end(), dst->data().begin()); - break; - } - default: - throw std::runtime_error("from_msg(NavMapLayer): unsupported type value " + - std::to_string(msg.type)); - } + apply_layer_msg(msg, nm); } void from_msg( @@ -371,8 +359,9 @@ navmap::NavMap from_occupancy_grid( nm.rebuild_geometry_accels(); - // Per-NavCel occupancy layer (U8) + // Per-NavCel occupancy layer (U8); NO_INFORMATION (from -1) is its unknown value auto occ = nm.layers.add_or_get("occupancy", nm.navcels.size(), navmap::LayerType::U8); + nm.set_layer_unknown("occupancy", NO_INFORMATION); tidx = 0; auto cell_id = [W](uint32_t i, uint32_t j) {return j * W + i;}; for (uint32_t j = 0; j < H; ++j) { diff --git a/navmap_ros/src/navmap_ros/navmap_io.cpp b/navmap_ros/src/navmap_ros/navmap_io.cpp index b538140..dd8f86a 100644 --- a/navmap_ros/src/navmap_ros/navmap_io.cpp +++ b/navmap_ros/src/navmap_ros/navmap_io.cpp @@ -16,6 +16,7 @@ #include +#include #include #include #include @@ -26,15 +27,18 @@ #include "navmap_ros/conversions.hpp" #include "rclcpp/serialized_message.hpp" #include "rclcpp/serialization.hpp" +#include "rclcpp/logging.hpp" +#include "navmap_ros_interfaces/msg/nav_map_v1.hpp" namespace navmap_ros::io { namespace { -// Magic header "NAVMAP\0" (7 chars + NUL), version uint32_t = 1, payload_size uint64_t (LE) +// Magic header "NAVMAP\0" (7 chars + NUL), version uint32_t, payload_size uint64_t (LE). +// Version 1 (also files without header) is migrated on load. static constexpr char kMagic[8] = {'N', 'A', 'V', 'M', 'A', 'P', '\0', '\0'}; -static constexpr uint32_t kVersion = 1; +static constexpr uint32_t kVersion = 2; // 2: layer metadata (unknown, description, unit) and int16 layers inline void set_error(std::error_code * ec, int ev = EIO) { @@ -82,12 +86,57 @@ bool save_msg_to_file( } } +namespace +{ + +// Version 1 (no layer metadata, no int16 layers) -> current message. +navmap_ros_interfaces::msg::NavMap migrate_v1(const navmap_ros_interfaces::msg::NavMapV1 & old) +{ + navmap_ros_interfaces::msg::NavMap m; + m.header = old.header; + m.positions_x = old.positions_x; + m.positions_y = old.positions_y; + m.positions_z = old.positions_z; + m.has_vertex_rgba = old.has_vertex_rgba; + m.colors_r = old.colors_r; + m.colors_g = old.colors_g; + m.colors_b = old.colors_b; + m.colors_a = old.colors_a; + m.navcels_v0 = old.navcels_v0; + m.navcels_v1 = old.navcels_v1; + m.navcels_v2 = old.navcels_v2; + m.surfaces = old.surfaces; + for (const auto & l : old.layers) { + navmap_ros_interfaces::msg::NavMapLayer n; + n.header = l.header; + n.name = l.name; + n.type = l.type; + n.data_u8 = l.data_u8; + n.data_f32 = l.data_f32; + n.data_f64 = l.data_f64; + m.layers.push_back(std::move(n)); + } + return m; +} + +template +void deserialize(const std::vector & buffer, MsgT & out) +{ + rclcpp::SerializedMessage serialized(buffer.size()); + auto & rcl_ser = serialized.get_rcl_serialized_message(); + std::memcpy(rcl_ser.buffer, buffer.data(), buffer.size()); + rcl_ser.buffer_length = buffer.size(); + rclcpp::Serialization ser; + ser.deserialize_message(&serialized, &out); +} + +} // namespace + bool load_msg_from_file( const std::string & path, navmap_ros_interfaces::msg::NavMap & out_msg, std::error_code * ec) { - using MsgT = navmap_ros_interfaces::msg::NavMap; try { std::ifstream ifs(path, std::ios::binary); if (!ifs) {set_error(ec, ENOENT); return false;} @@ -97,13 +146,14 @@ bool load_msg_from_file( ifs.read(magic, sizeof(magic)); if (!ifs) {set_error(ec); return false;} - uint32_t version = 0; + // Files without header are the oldest format: version 1 + uint32_t version = 1; uint64_t payload_size = 0; bool has_header = std::memcmp(magic, kMagic, sizeof(kMagic)) == 0; if (has_header) { ifs.read(reinterpret_cast(&version), sizeof(version)); ifs.read(reinterpret_cast(&payload_size), sizeof(payload_size)); - if (!ifs || version != kVersion) {set_error(ec); return false;} + if (!ifs || version < 1 || version > kVersion) {set_error(ec); return false;} } else { // No header: treat whole file as payload ifs.clear(); @@ -119,15 +169,31 @@ bool load_msg_from_file( buffer.resize(static_cast(payload_size)); ifs.read(reinterpret_cast(buffer.data()), static_cast(payload_size)); if (!ifs) {set_error(ec); return false;} + ifs.close(); - // Deserialize ROS 2 CDR - rclcpp::SerializedMessage serialized(buffer.size()); - auto & rcl_ser = serialized.get_rcl_serialized_message(); - std::memcpy(rcl_ser.buffer, buffer.data(), buffer.size()); - rcl_ser.buffer_length = buffer.size(); - - rclcpp::Serialization ser; - ser.deserialize_message(&serialized, &out_msg); + if (version == kVersion) { + deserialize(buffer, out_msg); + } else { + // Deprecated format: migrate, and replace the file with the current format + navmap_ros_interfaces::msg::NavMapV1 old; + deserialize(buffer, old); + out_msg = migrate_v1(old); + auto logger = rclcpp::get_logger("navmap_io"); + std::error_code wec; + if (save_msg_to_file(out_msg, path + ".migrating", {}, &wec) && + std::rename((path + ".migrating").c_str(), path.c_str()) == 0) + { + RCLCPP_WARN( + logger, "%s: .navmap format version %u is deprecated; migrated to version %u and " + "rewritten in place", path.c_str(), version, kVersion); + } else { + std::remove((path + ".migrating").c_str()); + RCLCPP_WARN( + logger, "%s: .navmap format version %u is deprecated; loaded through a migration to " + "version %u, but the file could not be rewritten (save it again to update it)", + path.c_str(), version, kVersion); + } + } if (ec) {*ec = {};} return true; diff --git a/navmap_ros/tests/CMakeLists.txt b/navmap_ros/tests/CMakeLists.txt index 6ca04b2..a73b1b1 100644 --- a/navmap_ros/tests/CMakeLists.txt +++ b/navmap_ros/tests/CMakeLists.txt @@ -15,3 +15,8 @@ target_link_libraries(test_from_points ${pcl_conversions_TARGETS} ${pcl_conversions_LIBRARIES} ) + +ament_add_gtest(test_layer_metadata_and_migration test_layer_metadata_and_migration.cpp) +target_link_libraries(test_layer_metadata_and_migration + ${PROJECT_NAME} +) diff --git a/navmap_ros/tests/test_layer_metadata_and_migration.cpp b/navmap_ros/tests/test_layer_metadata_and_migration.cpp new file mode 100644 index 0000000..375210f --- /dev/null +++ b/navmap_ros/tests/test_layer_metadata_and_migration.cpp @@ -0,0 +1,286 @@ +// 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 + +#include "navmap_core/NavMap.hpp" +#include "navmap_core/Occupancy.hpp" +#include "navmap_ros/conversions.hpp" +#include "navmap_ros/navmap_io.hpp" +#include "navmap_ros_interfaces/msg/nav_map.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "navmap_ros_interfaces/msg/nav_map_v1.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" + +namespace fs = std::filesystem; +using navmap::NavMap; + +namespace +{ + +NavMap small_map() +{ + NavMap nm; + const std::size_t s = nm.create_surface("map"); + for (int j = 0; j <= 2; ++j) { + for (int i = 0; i <= 2; ++i) { + nm.add_vertex({static_cast(i), static_cast(j), 0.0f}); + } + } + auto v = [](int i, int j) {return static_cast(j * 3 + i);}; + for (int j = 0; j < 2; ++j) { + for (int i = 0; i < 2; ++i) { + 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))); + } + } + nm.rebuild_geometry_accels(); + return nm; +} + +// A map with one layer of each type and metadata. +NavMap rich_map() +{ + NavMap nm = small_map(); + nm.add_layer("occupancy", "occupancy", "", uint8_t{0}); + nm.set_layer_unknown("occupancy", 255.0); + nm.layer_set("occupancy", 1, 255); + nm.add_layer("elevation", "terrain height", "m", 1.5f); + nm.set_layer_unknown("elevation", std::numeric_limits::quiet_NaN()); + nm.layer_set("elevation", 2, std::numeric_limits::quiet_NaN()); + nm.add_layer("cost", "", "", 0.25); + navmap::OccupancyLayer occ(nm); + occ.hit(3); + occ.miss(4); + return nm; +} + +void expect_rich(const NavMap & nm) +{ + EXPECT_EQ(nm.layer_type_name("occupancy_logodds"), "int16"); + EXPECT_EQ(nm.layer_get("occupancy_logodds", 3), 847); + EXPECT_EQ(nm.layer_get("occupancy_logodds", 4), -405); + EXPECT_TRUE(nm.is_unknown("occupancy_logodds", 0)); + EXPECT_TRUE(nm.is_unknown("occupancy", 1)); + EXPECT_FALSE(nm.is_unknown("occupancy", 0)); + EXPECT_TRUE(nm.is_unknown("elevation", 2)); + EXPECT_FLOAT_EQ(nm.layer_get("elevation", 0), 1.5f); + EXPECT_EQ(nm.get_layer_meta("elevation")->description, "terrain height"); + EXPECT_EQ(nm.get_layer_meta("elevation")->unit, "m"); + EXPECT_FALSE(nm.get_layer_meta("cost")->unknown.has_value()); + EXPECT_DOUBLE_EQ(nm.layer_get("cost", 5), 0.25); +} + +std::string tmp_dir(const std::string & name) +{ + const fs::path d = fs::temp_directory_path() / (name + std::to_string(::getpid())); + fs::remove_all(d); + fs::create_directories(d); + return d.string(); +} + +// Writes `msg` as a version-1 file (the v1 message, with or without the header). +void write_v1(const NavMap & nm, const std::string & path, bool header) +{ + const auto cur = navmap_ros::to_msg(nm); + navmap_ros_interfaces::msg::NavMapV1 old; + old.header = cur.header; + old.positions_x = cur.positions_x; + old.positions_y = cur.positions_y; + old.positions_z = cur.positions_z; + old.navcels_v0 = cur.navcels_v0; + old.navcels_v1 = cur.navcels_v1; + old.navcels_v2 = cur.navcels_v2; + old.surfaces = cur.surfaces; + for (const auto & l : cur.layers) { + if (l.type == navmap_ros_interfaces::msg::NavMapLayer::I16) {continue;} // not in v1 + navmap_ros_interfaces::msg::NavMapLayerV1 o; + o.name = l.name; + o.type = l.type; + o.data_u8 = l.data_u8; + o.data_f32 = l.data_f32; + o.data_f64 = l.data_f64; + old.layers.push_back(o); + } + rclcpp::Serialization ser; + rclcpp::SerializedMessage sm; + ser.serialize_message(&old, &sm); + const auto & r = sm.get_rcl_serialized_message(); + std::ofstream f(path, std::ios::binary); + if (header) { + const char magic[8] = {'N', 'A', 'V', 'M', 'A', 'P', '\0', '\0'}; + const uint32_t ver = 1; + const uint64_t size = r.buffer_length; + f.write(magic, 8); + f.write(reinterpret_cast(&ver), 4); + f.write(reinterpret_cast(&size), 8); + } + f.write(reinterpret_cast(r.buffer), static_cast(r.buffer_length)); +} + +uint32_t file_version(const std::string & path) +{ + std::ifstream f(path, std::ios::binary); + char magic[8]; + uint32_t ver = 0; + f.read(magic, 8); + f.read(reinterpret_cast(&ver), 4); + return std::string(magic, 6) == "NAVMAP" ? ver : 0; +} + +} // namespace + +// ------------------------------------------------------------------ conversions + +TEST(LayerMetadata, FullMapRoundTripKeepsInt16AndMetadata) +{ + const NavMap nm = rich_map(); + const auto msg = navmap_ros::to_msg(nm); + for (const auto & l : msg.layers) { + if (l.name == "occupancy_logodds") { + EXPECT_EQ(l.type, navmap_ros_interfaces::msg::NavMapLayer::I16); + EXPECT_EQ(l.unit, "1/1000 log-odds"); + EXPECT_TRUE(l.has_unknown); + EXPECT_EQ(l.data_i16.size(), nm.navcels.size()); + } + if (l.name == "elevation") { + EXPECT_TRUE(l.has_unknown); + EXPECT_TRUE(std::isnan(l.unknown_value)); + } + if (l.name == "cost") {EXPECT_FALSE(l.has_unknown);} + } + expect_rich(navmap_ros::from_msg(msg)); +} + +TEST(LayerMetadata, SingleLayerMessagesCarryMetadataAndRetype) +{ + const NavMap src = rich_map(); + NavMap dst = small_map(); + for (const auto & name : {"occupancy_logodds", "occupancy", "elevation", "cost"}) { + navmap_ros::from_msg(navmap_ros::to_msg(src, name), dst); + } + expect_rich(dst); + // A layer message of another type replaces the existing layer and its metadata + NavMap other = small_map(); + other.add_layer("occupancy", "now a float", "%", 0.5f); + navmap_ros::from_msg(navmap_ros::to_msg(other, "occupancy"), dst); + EXPECT_EQ(dst.layer_type_name("occupancy"), "float"); + EXPECT_FLOAT_EQ(dst.layer_get("occupancy", 0), 0.5f); + EXPECT_FALSE(dst.get_layer_meta("occupancy")->unknown.has_value()); + EXPECT_EQ(dst.get_layer_meta("occupancy")->description, "now a float"); +} + +TEST(LayerMetadata, FileRoundTripIsVersion2) +{ + const std::string dir = tmp_dir("navmap_v2_"); + const std::string path = dir + "/m.navmap"; + ASSERT_TRUE(navmap_ros::io::save_to_file(rich_map(), path)); + EXPECT_EQ(file_version(path), 2u); + NavMap back; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, back)); + expect_rich(back); + fs::remove_all(dir); +} + +// ------------------------------------------------------------------ migration of version 1 + +TEST(FormatMigration, Version1FilesLoadAndAreRewrittenAsVersion2) +{ + for (const bool header : {true, false}) { + const std::string dir = tmp_dir(header ? "navmap_v1h_" : "navmap_v1n_"); + const std::string path = dir + "/old.navmap"; + write_v1(rich_map(), path, header); + EXPECT_NE(file_version(path), 2u); + NavMap nm; + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, nm, &ec)) << ec.message(); + // Geometry and old layers kept; no metadata existed in version 1 + EXPECT_EQ(nm.navcels.size(), 8u); + EXPECT_EQ(nm.layer_get("occupancy", 1), 255); + EXPECT_FLOAT_EQ(nm.layer_get("elevation", 0), 1.5f); + EXPECT_FALSE(nm.has_layer("occupancy_logodds")); + EXPECT_FALSE(nm.is_unknown("occupancy", 1)); + // Rewritten in place, and the next load is a plain version-2 load + EXPECT_EQ(file_version(path), 2u) << "header " << header; + EXPECT_FALSE(fs::exists(path + ".migrating")); + NavMap again; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, again)); + EXPECT_EQ(again.layer_get("occupancy", 1), 255); + fs::remove_all(dir); + } +} + +TEST(FormatMigration, ReadOnlyLocationStillLoadsAndKeepsTheFile) +{ + if (::geteuid() == 0) {GTEST_SKIP() << "root can write anywhere";} + const std::string dir = tmp_dir("navmap_v1ro_"); + const std::string path = dir + "/old.navmap"; + write_v1(rich_map(), path, true); + fs::permissions(dir, fs::perms::owner_read | fs::perms::owner_exec); + NavMap nm; + EXPECT_TRUE(navmap_ros::io::load_from_file(path, nm)); + EXPECT_EQ(nm.layer_get("occupancy", 1), 255); + EXPECT_EQ(file_version(path), 1u); // could not be rewritten + fs::permissions(dir, fs::perms::owner_all); + fs::remove_all(dir); +} + +TEST(FormatMigration, NewerOrBrokenFilesAreRejected) +{ + const std::string dir = tmp_dir("navmap_vbad_"); + const std::string path = dir + "/m.navmap"; + ASSERT_TRUE(navmap_ros::io::save_to_file(rich_map(), path)); + { + std::fstream f(path, std::ios::binary | std::ios::in | std::ios::out); + f.seekp(8); + const uint32_t future = 3; + f.write(reinterpret_cast(&future), 4); + } + NavMap nm; + std::error_code ec; + EXPECT_FALSE(navmap_ros::io::load_from_file(path, nm, &ec)); + EXPECT_TRUE(ec); + EXPECT_FALSE(navmap_ros::io::load_from_file(dir + "/missing.navmap", nm, &ec)); + fs::remove_all(dir); +} + +TEST(LayerMetadata, OccupancyGridUnknownCellsAreDeclaredUnknown) +{ + // As slam_toolbox publishes it: -1 unknown, 0 free, 100 occupied + nav_msgs::msg::OccupancyGrid g; + g.header.frame_id = "map"; + g.info.width = 3; + g.info.height = 1; + g.info.resolution = 0.5f; + g.data = {-1, 0, 100}; + const NavMap nm = navmap_ros::from_occupancy_grid(g); + ASSERT_EQ(nm.navcels.size(), 6u); + EXPECT_TRUE(nm.is_unknown("occupancy", 0)); + EXPECT_TRUE(nm.is_unknown("occupancy", 1)); + EXPECT_FALSE(nm.is_unknown("occupancy", 2)); + EXPECT_FALSE(nm.is_unknown("occupancy", 4)); + // ...and the declaration travels with the map + EXPECT_TRUE(navmap_ros::from_msg(navmap_ros::to_msg(nm)).is_unknown("occupancy", 0)); +} diff --git a/navmap_ros_interfaces/CMakeLists.txt b/navmap_ros_interfaces/CMakeLists.txt index 5573f7c..4e811c1 100644 --- a/navmap_ros_interfaces/CMakeLists.txt +++ b/navmap_ros_interfaces/CMakeLists.txt @@ -18,6 +18,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/NavMap.msg" "msg/NavMapSurface.msg" "msg/NavMapLayer.msg" + "msg/NavMapV1.msg" + "msg/NavMapLayerV1.msg" DEPENDENCIES builtin_interfaces std_msgs ) diff --git a/navmap_ros_interfaces/msg/NavMapLayer.msg b/navmap_ros_interfaces/msg/NavMapLayer.msg index 20e7e50..421f45f 100644 --- a/navmap_ros_interfaces/msg/NavMapLayer.msg +++ b/navmap_ros_interfaces/msg/NavMapLayer.msg @@ -4,12 +4,20 @@ std_msgs/Header header uint8 U8 = 0 uint8 F32 = 1 uint8 F64 = 2 +uint8 I16 = 3 string name -uint8 type # 0=U8, 1=F32, 2=F64 +uint8 type # 0=U8, 1=F32, 2=F64, 3=I16 + +# Metadata +string description +string unit +bool has_unknown # true: unknown_value means "not observed" (NaN allowed) +float64 unknown_value # Exactly one of the data_* arrays must be non-empty, and its length must be # equal to the number of triangles (NavCels) in the NavMap message. uint8[] data_u8 float32[] data_f32 float64[] data_f64 +int16[] data_i16 diff --git a/navmap_ros_interfaces/msg/NavMapLayerV1.msg b/navmap_ros_interfaces/msg/NavMapLayerV1.msg new file mode 100644 index 0000000..4275ffc --- /dev/null +++ b/navmap_ros_interfaces/msg/NavMapLayerV1.msg @@ -0,0 +1,16 @@ +# FROZEN: .navmap file format version 1 (read only, to migrate old maps). Do not change. +# Per-NavCel layer (one scalar per triangle) +std_msgs/Header header + +uint8 U8 = 0 +uint8 F32 = 1 +uint8 F64 = 2 + +string name +uint8 type # 0=U8, 1=F32, 2=F64 + +# Exactly one of the data_* arrays must be non-empty, and its length must be +# equal to the number of triangles (NavCels) in the NavMap message. +uint8[] data_u8 +float32[] data_f32 +float64[] data_f64 diff --git a/navmap_ros_interfaces/msg/NavMapV1.msg b/navmap_ros_interfaces/msg/NavMapV1.msg new file mode 100644 index 0000000..128a3f9 --- /dev/null +++ b/navmap_ros_interfaces/msg/NavMapV1.msg @@ -0,0 +1,26 @@ +# FROZEN: .navmap file format version 1 (read only, to migrate old maps). Do not change. +# Compact NavMap transport. +std_msgs/Header header + +# Vertex positions (SoA) +float32[] positions_x +float32[] positions_y +float32[] positions_z + +# Optional per-vertex colors (RGBA). Fill only if has_vertex_rgba=true. +bool has_vertex_rgba +uint8[] colors_r +uint8[] colors_g +uint8[] colors_b +uint8[] colors_a + +# Global triangle index buffers +uint32[] navcels_v0 +uint32[] navcels_v1 +uint32[] navcels_v2 + +# Surfaces (each lists indices into the global triangle arrays) +navmap_ros_interfaces/NavMapSurface[] surfaces + +# Per-NavCel layers +navmap_ros_interfaces/NavMapLayerV1[] layers diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index 559eaeb..e530f8f 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -14,6 +14,7 @@ // limitations under the License. +#include #include #include @@ -76,6 +77,18 @@ inline Ogre::ColourValue colorFromU8(uint8_t v, float alpha) return Ogre::ColourValue(c, c, c, alpha); } +// Occupancy log-odds (int16, 1/1000): grey 1 - p, as U8 occupancy (free light, occupied black). +inline Ogre::ColourValue colorFromLogodds(int16_t v, float alpha) +{ + const float p = 1.0f / (1.0f + std::exp(-static_cast(v) / 1000.0f)); + return Ogre::ColourValue(1.0f - p, 1.0f - p, 1.0f - p, alpha); +} + +inline bool isLogoddsLayer(const navmap_ros_interfaces::msg::NavMapLayer & L) +{ + return L.type == navmap_ros_interfaces::msg::NavMapLayer::I16 && L.unit == "1/1000 log-odds"; +} + inline Ogre::ColourValue colorFromHeat(float value, float max_value, float alpha) { if (max_value <= 1e-9f) {return Ogre::ColourValue(0.0f, 1.0f, 0.0f, alpha);} @@ -321,14 +334,15 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) const size_t n_u8 = msg->data_u8.size(); const size_t n_f32 = msg->data_f32.size(); const size_t n_f64 = msg->data_f64.size(); - const int non_empty = (n_u8 ? 1 : 0) + (n_f32 ? 1 : 0) + (n_f64 ? 1 : 0); + const size_t n_i16 = msg->data_i16.size(); + const int non_empty = (n_u8 ? 1 : 0) + (n_f32 ? 1 : 0) + (n_f64 ? 1 : 0) + (n_i16 ? 1 : 0); if (non_empty != 1) { setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", - "Exactly one of data_u8 / data_f32 / data_f64 must be non-empty."); + "Exactly one of data_u8 / data_f32 / data_f64 / data_i16 must be non-empty."); return; } - const size_t eff_len = n_u8 ? n_u8 : (n_f32 ? n_f32 : n_f64); + const size_t eff_len = n_u8 ? n_u8 : (n_f32 ? n_f32 : (n_f64 ? n_f64 : n_i16)); if (eff_len != n_tris) { setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", QString("Layer size (%1) does not match number of triangles (%2)") @@ -344,11 +358,10 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) const char * type_str = (msg->type == navmap_ros_interfaces::msg::NavMapLayer::U8) ? "U8" : (msg->type == navmap_ros_interfaces::msg::NavMapLayer::F32) ? "F32" : - (msg->type == navmap_ros_interfaces::msg::NavMapLayer::F64) ? "F64" : "UNKNOWN"; + (msg->type == navmap_ros_interfaces::msg::NavMapLayer::F64) ? "F64" : + (msg->type == navmap_ros_interfaces::msg::NavMapLayer::I16) ? "I16" : "UNKNOWN"; - const size_t len = !msg->data_u8.empty() ? msg->data_u8.size() : - !msg->data_f32.empty() ? msg->data_f32.size() : - msg->data_f64.size(); + const size_t len = eff_len; QString line = QString("OK — updates: %1 | last: \"%2\" (%3, len=%4)") .arg(qulonglong(layer_update_count_)) @@ -624,12 +637,18 @@ void NavMapDisplay::updateColorsOnly_() last_msg_->colors_b.size() == last_msg_->positions_x.size(); } - float max_val = 0.0f; + float max_val = 0.0f, min_val = 0.0f; bool is_u8 = false; if (selected_layer) { const auto & L = *selected_layer; if (L.type == NavMapLayerMsg::U8 && L.data_u8.size() == V0.size()) { is_u8 = true; + } else if (L.type == NavMapLayerMsg::I16 && L.data_i16.size() == V0.size()) { + if (!L.data_i16.empty()) { + const auto [lo, hi] = std::minmax_element(L.data_i16.begin(), L.data_i16.end()); + min_val = *lo; + max_val = *hi; + } } else if (L.type == NavMapLayerMsg::F32 && L.data_f32.size() == V0.size()) { for (float v : L.data_f32) { max_val = std::max(max_val, v); @@ -707,6 +726,17 @@ void NavMapDisplay::updateColorsOnly_() const uint32_t packed = packARGB(col); *p++ = packed; *p++ = packed; *p++ = packed; } + } else if (selected_layer->type == NavMapLayerMsg::I16) { + const bool logodds = isLogoddsLayer(*selected_layer); + for (size_t t = 0; t < V0.size(); ++t) { + const int16_t v = selected_layer->data_i16[t]; + const float shifted = static_cast(v) - min_val; // the scales start at 0 + const Ogre::ColourValue col = logodds ? colorFromLogodds(v, alpha) : + (use_rainbow ? colorFromRainbow(shifted, max_val - min_val, alpha) : + colorFromHeat(shifted, max_val - min_val, alpha)); + const uint32_t packed = packARGB(col); + *p++ = packed; *p++ = packed; *p++ = packed; + } } else { // F64 for (size_t t = 0; t < V0.size(); ++t) { const float v = static_cast(selected_layer->data_f64[t]); @@ -717,6 +747,23 @@ void NavMapDisplay::updateColorsOnly_() *p++ = packed; *p++ = packed; *p++ = packed; } } + // Cells holding the layer's unknown value: translucent grey (U8 keeps its own mapping) + if (!is_u8 && selected_layer->has_unknown) { + const double u = selected_layer->unknown_value; + const uint32_t grey = packARGB(Ogre::ColourValue(0.5f, 0.5f, 0.5f, 0.35f * alpha)); + uint32_t * q = reinterpret_cast(base); + for (size_t t = 0; t < V0.size(); ++t, q += 3) { + double v = 0.0; + if (selected_layer->type == NavMapLayerMsg::I16) { + v = selected_layer->data_i16[t]; + } else if (selected_layer->type == NavMapLayerMsg::F32) { + v = selected_layer->data_f32[t]; + } else { + v = selected_layer->data_f64[t]; + } + if ((std::isnan(u) && std::isnan(v)) || v == u) {q[0] = q[1] = q[2] = grey;} + } + } } else { // Fallback to neutral grey const uint32_t packed = packARGB(Ogre::ColourValue(0.7f, 0.7f, 0.7f, alpha)); @@ -787,7 +834,7 @@ void NavMapDisplay::updateColorSchemeOptions_() if (it != layers_by_name_.end()) { const auto * L = it->second; - if (L->type == navmap_ros_interfaces::msg::NavMapLayer::U8) { + if (L->type == navmap_ros_interfaces::msg::NavMapLayer::U8 || isLogoddsLayer(*L)) { color_scheme_property_->addOption("Occupancy"); color_scheme_property_->setString("Occupancy"); color_scheme_property_->setDescription( diff --git a/navmap_tools/navmap_tools/resample.py b/navmap_tools/navmap_tools/resample.py index d42a8f9..d3b7e0d 100644 --- a/navmap_tools/navmap_tools/resample.py +++ b/navmap_tools/navmap_tools/resample.py @@ -35,24 +35,46 @@ import struct import sys -from navmap_ros_interfaces.msg import NavMap, NavMapLayer, NavMapSurface +from navmap_ros_interfaces.msg import NavMap, NavMapLayer, NavMapSurface, NavMapV1 import numpy as np from rclpy.serialization import deserialize_message, serialize_message MAGIC = b'NAVMAP\x00\x00' -VERSION = 1 +VERSION = 2 # 2: layer metadata (unknown, description, unit) and int16 layers FREE, LETHAL, UNKNOWN = 0, 254, 255 # ------------------------------- Reading --------------------------------- +def migrate_v1(old): + """Convert a file-version-1 NavMap message (no layer metadata) to the current one.""" + msg = NavMap() + for field in ('header', 'positions_x', 'positions_y', 'positions_z', 'has_vertex_rgba', + 'colors_r', 'colors_g', 'colors_b', 'colors_a', 'navcels_v0', 'navcels_v1', + 'navcels_v2', 'surfaces'): + setattr(msg, field, getattr(old, field)) + for lo in old.layers: + layer = NavMapLayer() + layer.header, layer.name, layer.type = lo.header, lo.name, lo.type + layer.data_u8, layer.data_f32, layer.data_f64 = lo.data_u8, lo.data_f32, lo.data_f64 + msg.layers.append(layer) + return msg + + def read_navmap(path): with open(path, 'rb') as f: data = f.read() + version = 1 # files without header are the oldest format if data[:8] == MAGIC: - _, size = struct.unpack(' 0, acc / cnt, np.nan) - layers[layer.name] = (layer.type, grid) + layers[layer.name] = (layer.type, grid, layer) frame = msg.surfaces[0].frame_id or msg.header.frame_id return layers, (xs[0], ys[0], z[0]), res, frame @@ -190,14 +215,16 @@ def resample_occupancy(grid, k): def resample(layers, k): out = {} - for name, (ltype, grid) in layers.items(): + for name, (ltype, grid, *meta) in layers.items(): + meta = meta[0] if meta else None if name == 'occupancy': - out[name] = (ltype, resample_occupancy(grid, k)) + out[name] = (ltype, resample_occupancy(grid, k), meta) elif ltype == NavMapLayer.U8: - out[name] = (ltype, blocks(grid, k, 0).max(axis=2)) + out[name] = (ltype, blocks(grid, k, 0).max(axis=2), meta) else: with np.errstate(invalid='ignore'): - out[name] = (ltype, np.nanmean(blocks(grid.astype(float), k, np.nan), axis=2)) + mean = np.nanmean(blocks(grid.astype(float), k, np.nan), axis=2) + out[name] = (ltype, mean, meta) return out @@ -227,16 +254,25 @@ def build_navmap(layers, origin, res, frame): surface.navcels = list(range(2 * w * h)) msg.surfaces = [surface] - for name, (ltype, grid) in layers.items(): + for name, (ltype, grid, *meta) in layers.items(): layer = NavMapLayer() layer.header.frame_id = frame layer.name = name layer.type = ltype + if meta and meta[0] is not None: # description, unit and unknown of the source layer + layer.description, layer.unit = meta[0].description, meta[0].unit + layer.has_unknown, layer.unknown_value = meta[0].has_unknown, meta[0].unknown_value + elif name == 'occupancy' and ltype == NavMapLayer.U8: # as navmap_ros::from_occupancy_grid + layer.has_unknown, layer.unknown_value = True, float(UNKNOWN) per_triangle = np.repeat(grid.ravel(), 2) # both triangles of a cell if ltype == NavMapLayer.U8: layer.data_u8 = per_triangle.astype(np.uint8).tobytes() elif ltype == NavMapLayer.F32: layer.data_f32 = per_triangle.astype(np.float32).tolist() + elif ltype == NavMapLayer.I16: + fill = layer.unknown_value if layer.has_unknown else 0.0 + vals = np.round(np.where(np.isnan(per_triangle), fill, per_triangle)) + layer.data_i16 = np.clip(vals, -32768, 32767).astype(np.int16).tolist() else: layer.data_f64 = per_triangle.astype(float).tolist() msg.layers.append(layer) diff --git a/navmap_tools/test/test_resample.py b/navmap_tools/test/test_resample.py index 1bd6d69..d3d9d7f 100644 --- a/navmap_tools/test/test_resample.py +++ b/navmap_tools/test/test_resample.py @@ -16,13 +16,14 @@ import sys -from navmap_ros_interfaces.msg import NavMapLayer, NavMapSurface +from navmap_ros_interfaces.msg import NavMapLayer, NavMapLayerV1, NavMapSurface, NavMapV1 from navmap_tools.resample import ( build_navmap, FREE, grid_from_navmap, LETHAL, main, read_navmap, read_yaml_grid, resample_occupancy, UNKNOWN, write_navmap, ) import numpy as np import pytest +from rclpy.serialization import serialize_message def occ_layers(grid): @@ -138,6 +139,64 @@ def test_other_layers_are_resampled_too(tmp_path, monkeypatch): assert out['marks'][1][0, 0] == 7 # max +def test_int16_layers_and_metadata_survive_a_resample(tmp_path, monkeypatch): + src, dst = tmp_path / 'in.navmap', tmp_path / 'out.navmap' + msg = build_navmap(occ_layers(np.zeros((2, 2))), (0, 0, 0), 0.1, 'map') + lo = NavMapLayer() + lo.name, lo.type = 'logodds', NavMapLayer.I16 + lo.description, lo.unit = 'occupancy log-odds', '1/1000 log-odds' + lo.has_unknown, lo.unknown_value = True, 0.0 + lo.data_i16 = [-400, -400, 800, 800, 0, 0, 0, 0] # cells (0,0), (1,0) known; row 1 unknown + msg.layers.append(lo) + write_navmap(msg, str(src)) + run_main(monkeypatch, src, dst, 0.2) + out = read_navmap(str(dst)) + layer = next(lr for lr in out.layers if lr.name == 'logodds') + assert layer.type == NavMapLayer.I16 + assert (layer.description, layer.unit) == ('occupancy log-odds', '1/1000 log-odds') + assert layer.has_unknown and layer.unknown_value == 0.0 + assert list(layer.data_i16) == [200, 200] # mean of the known cells only + + +def write_v1(msg, path, header=True): + """Write a file of format version 1, from the frozen v1 message.""" + old = NavMapV1() + for field in ('header', 'positions_x', 'positions_y', 'positions_z', 'has_vertex_rgba', + 'navcels_v0', 'navcels_v1', 'navcels_v2', 'surfaces'): + setattr(old, field, getattr(msg, field)) + for lr in msg.layers: + lo = NavMapLayerV1() + lo.name, lo.type, lo.data_u8 = lr.name, lr.type, lr.data_u8 + old.layers.append(lo) + payload = serialize_message(old) + with open(path, 'wb') as f: + if header: + f.write(b'NAVMAP\x00\x00' + (1).to_bytes(4, 'little') + + len(payload).to_bytes(8, 'little')) + f.write(payload) + + +@pytest.mark.parametrize('header', [True, False]) +def test_an_old_version_file_is_read_through_a_migration(tmp_path, capsys, header): + g = np.array([[0, 254], [255, 50]], np.uint8) + path = tmp_path / 'old.navmap' + write_v1(build_navmap(occ_layers(g), (0, 0, 0), 0.5, 'map'), str(path), header) + layers, _, res, _ = grid_from_navmap(read_navmap(str(path))) + assert layers['occupancy'][1].tolist() == g.tolist() + assert res == pytest.approx(0.5) + assert 'deprecated' in capsys.readouterr().err + + +def test_a_newer_version_file_is_rejected(tmp_path): + path = tmp_path / 'm.navmap' + write_navmap(build_navmap(occ_layers([[0]]), (0, 0, 0), 1.0, 'map'), str(path)) + data = bytearray(path.read_bytes()) + data[8:12] = (3).to_bytes(4, 'little') + path.write_bytes(bytes(data)) + with pytest.raises(SystemExit): + read_navmap(str(path)) + + # ------------------------------- Command ---------------------------------- def write_yaml_map(tmp_path, pixels): @@ -169,6 +228,8 @@ def test_a_yaml_map_is_resampled_to_a_navmap(tmp_path, monkeypatch): assert res == pytest.approx(0.1) assert origin[:2] == pytest.approx((-1.0, -2.0)) assert layers['occupancy'][1].tolist() == [[FREE, FREE], [LETHAL, FREE]] + occ = next(lr for lr in read_navmap(str(out)).layers if lr.name == 'occupancy') + assert occ.has_unknown and occ.unknown_value == UNKNOWN # declared, as from_occupancy_grid @pytest.mark.parametrize('resolution', [0.07, 0.03, 0.125]) From b862344fee8f00809c8e04a5b140acb6b026ae99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 09:48:41 +0200 Subject: [PATCH 03/11] Deep-copy layers when copying a NavMap MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The copy constructor and the full-copy assignment copied the layer registry, which holds shared pointers, so a copy shared every layer buffer with the original: a write on either side reached the other (EasyNav filters copy the map in and out of NavState every cycle). LayerRegistry copies are now deep, through LayerViewBase::clone(). Moves are unchanged. Signed-off-by: Francisco Martín Rico --- navmap_core/include/navmap_core/NavMap.hpp | 29 ++++++++++++++++++ navmap_core/tests/test_navmap_assign.cpp | 35 ++++++++++++++++++++++ 2 files changed, 64 insertions(+) diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 13ac76f..dbfe70e 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -140,6 +140,9 @@ struct LayerViewBase /// @brief Mark content dirty (forces hash recompute on next query). void mark_dirty() const noexcept {hash_dirty_ = true;} + /// @brief Independent copy of this layer (data, name, type and cached hash). + virtual std::shared_ptr clone() const = 0; + protected: mutable bool hash_dirty_{true}; mutable std::uint64_t hash_cache_{0}; @@ -190,6 +193,11 @@ struct LayerView : LayerViewBase } void set_data(const std::vector & v) {data_ = v; hash_dirty_ = true;} std::uint64_t content_hash() const override; + + std::shared_ptr clone() const override + { + return std::make_shared>(*this); + } ///@} }; @@ -237,6 +245,27 @@ std::uint64_t LayerView::content_hash() const class LayerRegistry { public: + LayerRegistry() = default; + LayerRegistry(LayerRegistry &&) noexcept = default; + LayerRegistry & operator=(LayerRegistry &&) noexcept = default; + + /// Copies are deep: each copy owns its layers (writes never reach the original). + LayerRegistry(const LayerRegistry & other) + { + for (const auto & [name, view] : other.layers_) { + layers_.emplace(name, view ? view->clone() : nullptr); + } + } + + LayerRegistry & operator=(const LayerRegistry & other) + { + if (this != &other) { + LayerRegistry tmp(other); + layers_.swap(tmp.layers_); + } + return *this; + } + /** * \brief Add a new typed layer or return an existing one with the same name. * diff --git a/navmap_core/tests/test_navmap_assign.cpp b/navmap_core/tests/test_navmap_assign.cpp index e766e21..7a33b03 100644 --- a/navmap_core/tests/test_navmap_assign.cpp +++ b/navmap_core/tests/test_navmap_assign.cpp @@ -249,3 +249,38 @@ TEST(NavMapAssign, LayerMetaSynchronized) EXPECT_EQ(dst.layer_meta.at("occ").unit, "unitless"); EXPECT_TRUE(dst.layer_meta.at("occ").per_cell); } + +TEST(NavMapAssign, CopiesNeverShareLayerData) +{ + // Copy constructor, assignment with different geometry (full copy) and with the same geometry + // (layer sync): writes on either side must never reach the other. + NavMap src; + fill_one_tri_map(src); + NavMap copy(src); + NavMap assigned; + assigned = src; + NavMap synced; + fill_one_tri_map(synced); + synced = src; + + src.layer_set("occ", 0, 7); + for (const NavMap * m : {©, &assigned, &synced}) { + EXPECT_EQ(m->layer_get("occ", 0), 254); + } + copy.layer_set("occ", 0, 1); + assigned.layer_set("cost", 0, -1.0f); + synced.layer_set("occ", 0, 2); + EXPECT_EQ(src.layer_get("occ", 0), 7); + EXPECT_FLOAT_EQ(src.layer_get("cost", 0), 10.0f); + EXPECT_EQ(copy.layer_get("occ", 0), 1); + EXPECT_EQ(synced.layer_get("occ", 0), 2); + + // A layer added to a copy does not appear in the original + copy.add_layer("extra", "", "", 3.0f); + EXPECT_FALSE(src.has_layer("extra")); + + // Moving keeps the data (and leaves nothing shared behind) + NavMap moved(std::move(copy)); + EXPECT_EQ(moved.layer_get("occ", 0), 1); + EXPECT_FLOAT_EQ(moved.layer_get("extra", 0), 3.0f); +} From 60f1913bc7355b973db28d0352d4a8e2b01b961d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 10:04:35 +0200 Subject: [PATCH 04/11] Add layer change tracking and an incremental distance field MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Layers record the NavCels changed since the last clear_changes(): single writes (layer_set, OccupancyLayer, mark_changed) once each, bulk writes (set_data, layer_clear, copies, messages, a new layer) as the whole layer. - navmap_core/DistanceField.hpp: distance to seed cells and open edges, kept up to date from the seeds added and removed. With a finite max_distance it is exact (each seed paints the NavCels it reaches through adjacency within that distance; removing one repaints the seeds within twice the distance, found through a spatial index), and an update gives the same field as a full recomputation. changed() lists the NavCels whose distance changed. - point_segment_distance moved to Geometry.hpp; resize_all handles int16. - navmap_bench: full and incremental distance field timings. Signed-off-by: Francisco Martín Rico --- README.md | 17 + navmap_core/CMakeLists.txt | 1 + navmap_core/bench/navmap_bench.cpp | 42 +++ .../include/navmap_core/DistanceField.hpp | 127 +++++++ navmap_core/include/navmap_core/Geometry.hpp | 11 + navmap_core/include/navmap_core/NavMap.hpp | 64 +++- navmap_core/include/navmap_core/Occupancy.hpp | 4 +- navmap_core/src/navmap_core/DistanceField.cpp | 306 ++++++++++++++++ navmap_core/src/navmap_core/NavMap.cpp | 11 - navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_incremental.cpp | 337 ++++++++++++++++++ navmap_ros/src/navmap_ros/conversions.cpp | 2 +- 12 files changed, 905 insertions(+), 22 deletions(-) create mode 100644 navmap_core/include/navmap_core/DistanceField.hpp create mode 100644 navmap_core/src/navmap_core/DistanceField.cpp create mode 100644 navmap_core/tests/test_incremental.cpp diff --git a/README.md b/README.md index c92c53d..c77cae2 100644 --- a/README.md +++ b/README.md @@ -276,6 +276,23 @@ The unknown value, description and unit of each layer travel in the message and --- +## 7d. Incremental updates (change tracking, `DistanceField.hpp`) + +Layers record which NavCels changed since the last `clear_changes()` (`layer_set`, `OccupancyLayer`; bulk writes mark the whole layer), so a consumer only reprocesses what changed. `DistanceField` keeps a distance-to-seed field up to date from the seed cells added and removed: + +```cpp +#include + +navmap::DistanceField df(nm, /*from_boundary=*/true, /*max_distance=*/0.7f); +df.compute(lethal_cells); // once +df.update(newly_lethal, no_longer_lethal); // every cycle: cost of the area that changes +for (navmap::NavCelId c : df.changed()) {/* recompute the cost of c from df[c] */} +``` + +With a finite `max_distance` the field is exact (straight distance to the nearest seed reaching the NavCel over the surface), and identical after any sequence of updates to a full recomputation. + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index f4f9dd8..eb9d3c8 100644 --- a/navmap_core/CMakeLists.txt +++ b/navmap_core/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(Eigen3 REQUIRED) add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/NavMap.cpp + src/${PROJECT_NAME}/DistanceField.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/navmap_core/bench/navmap_bench.cpp b/navmap_core/bench/navmap_bench.cpp index cf06c79..706565e 100644 --- a/navmap_core/bench/navmap_bench.cpp +++ b/navmap_core/bench/navmap_bench.cpp @@ -25,6 +25,7 @@ #include #include +#include "navmap_core/DistanceField.hpp" #include "navmap_core/Iterators.hpp" #include "navmap_core/NavMap.hpp" @@ -233,5 +234,46 @@ int main(int argc, char ** argv) } std::printf("\n"); } + + // Distance fields (borders + obstacle cells as seeds), radius r: full computation with + // NavMap::distance_field (nearest-feature propagation) and DistanceField (exact), and + // incremental updates of DistanceField (k obstacle cells moved), in ms. + std::printf( + "\ndistance fields, ms\n%9s %5s %14s %14s %14s %14s %14s\n", "navcels", "r", + "propag_full", "exact_full", "upd_1", "upd_10", "upd_100"); + for (int side : {100, 300, 1000}) { + if (side > max_side) {break;} + NavMap nm = make_map(side, 1, res); + nm.rebuild_geometry_accels(); + std::mt19937 rng(5); + std::uniform_int_distribution C(0, static_cast(nm.navcels.size() - 1)); + std::vector obstacles(nm.navcels.size() / 50); + for (auto & o : obstacles) { + o = C(rng); + } + for (const float r : {0.7f, 2.0f}) { + auto ms = [](auto && f) { + const auto t0 = Clock::now(); + f(); + return std::chrono::duration(Clock::now() - t0).count(); + }; + const double propag = ms([&] {(void)nm.distance_field(obstacles, true, r);}); + navmap::DistanceField df(nm, true, r); + const double exact = ms([&] {df.compute(obstacles);}); + double upd[3]; + int i = 0; + for (const int k : {1, 10, 100}) { + std::vector rem(obstacles.begin(), obstacles.begin() + k), add(k); + for (auto & a : add) { + a = C(rng); + } + upd[i++] = ms([&] {df.update(add, rem);}); + df.update(rem, add); // back + } + std::printf( + "%9zu %5.1f %14.2f %14.2f %14.3f %14.3f %14.3f\n", nm.navcels.size(), r, propag, exact, + upd[0], upd[1], upd[2]); + } + } return 0; } diff --git a/navmap_core/include/navmap_core/DistanceField.hpp b/navmap_core/include/navmap_core/DistanceField.hpp new file mode 100644 index 0000000..b898f7a --- /dev/null +++ b/navmap_core/include/navmap_core/DistanceField.hpp @@ -0,0 +1,127 @@ +// 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. + +/** + * @file DistanceField.hpp + * @brief Incremental distance field over a NavMap, updated by the seed cells added and removed + * instead of recomputed: the cost of an update depends on the area it changes. + */ + +#ifndef NAVMAP_CORE__DISTANCEFIELD_HPP_ +#define NAVMAP_CORE__DISTANCEFIELD_HPP_ + +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/** + * @brief Distance from each NavCel to the nearest seed (seed NavCels and, optionally, the open + * edges of the mesh), kept up to date incrementally. + * + * With a finite max_distance the field is exact for this definition: the distance of a NavCel is + * the straight 3D distance from its centroid to the nearest seed (point, or open-edge segment) + * that reaches it through adjacent NavCels all within max_distance of that seed, so it never + * jumps to another floor or across a hole. Each seed paints its disc; removing one repaints the + * seeds within 2 * max_distance of it (found through a spatial index). Without max_distance it falls back to NavMap::distance_field() (nearest-feature + * propagation, approximate) and updates recompute everything. + * + * Bound to the geometry of the NavMap it was built from: rebuild it if the geometry changes. + * The map must outlive it. + */ +class DistanceField +{ +public: + /** + * @param nm Map (geometry and adjacency must be built). + * @param from_boundary Also seed the open edges (walls, drops, unmapped space). + * @param max_distance Distances beyond this are +inf (bounds the work of every update). + */ + DistanceField( + const NavMap & nm, bool from_boundary, + float max_distance = std::numeric_limits::infinity()); + + /// Full computation from these seed cells. + void compute(const std::vector & seed_cells); + + /** + * @brief Incremental update: NavCels that became seeds and NavCels that stopped being seeds + * since the last compute() / update(). Unknown or repeated ids are ignored. + */ + void update(const std::vector & added, const std::vector & removed); + + const std::vector & distances() const {return dist_;} + float operator[](NavCelId cid) const {return dist_[cid];} + + /// NavCels whose distance changed in the last compute() / update(). + const std::vector & changed() const {return changed_;} + + /// Whether @p cid is currently a seed cell. + bool is_seed(NavCelId cid) const {return cid < seed_of_.size() && seed_of_[cid] != kNoSeed;} + +private: + static constexpr std::uint32_t kNoSeed = std::numeric_limits::max(); + struct Seed + { + Eigen::Vector3f a, b; ///< point (a == b) or segment + NavCelId cell; ///< the NavCel it starts from + bool alive; + }; + + float distance_to(NavCelId c, std::uint32_t seed) const; + void touch(NavCelId c); + void begin_operation(); + void finish(); + template + void flood(std::uint32_t seed, F && f); + void splat(std::uint32_t seed); + std::uint32_t new_point_seed(NavCelId c); + static std::int64_t bucket_key(int bx, int by, int bz); + void index_seed(std::uint32_t seed, bool insert); + + const NavMap & nm_; + bool from_boundary_; + float max_distance_; + float bucket_{1.0f}; + std::unordered_map> buckets_; ///< seed index + std::vector centroid_; + std::vector seeds_; ///< open-edge seeds first, then point seeds + std::size_t n_boundary_{0}; + std::vector boundary_start_; ///< per cell: its open-edge seeds (CSR) + std::vector seed_of_; ///< per cell: its point seed, if a seed cell + std::vector nearest_; ///< per cell: the seed its distance comes from + std::vector dist_; + std::vector free_seed_slots_; + // Cells touched by the current operation and their previous distance + std::vector touched_; + std::vector before_; + std::vector touched_stamp_; + std::uint32_t stamp_{0}; + std::vector changed_; + // Flood buffers + std::vector flood_; + std::vector flood_stamp_; + std::uint32_t flood_id_{0}; +}; + +} // namespace navmap + +#endif // NAVMAP_CORE__DISTANCEFIELD_HPP_ diff --git a/navmap_core/include/navmap_core/Geometry.hpp b/navmap_core/include/navmap_core/Geometry.hpp index f97367a..fbff8d6 100644 --- a/navmap_core/include/navmap_core/Geometry.hpp +++ b/navmap_core/include/navmap_core/Geometry.hpp @@ -139,6 +139,17 @@ inline bool ray_triangle_intersect( // Axis-aligned bounding box (AABB) // ----------------------------------------------------------------------------- +/** + * @brief Distance from point @p p to segment [@p a, @p b] (a point if a == b). + */ +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(); +} + /** * @brief Axis-aligned bounding box. * diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index dbfe70e..7d51d0a 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -140,12 +140,55 @@ struct LayerViewBase /// @brief Mark content dirty (forces hash recompute on next query). void mark_dirty() const noexcept {hash_dirty_ = true;} - /// @brief Independent copy of this layer (data, name, type and cached hash). + /// @brief Independent copy of this layer (data, name, type, cached hash and pending changes). virtual std::shared_ptr clone() const = 0; + // ---- Change tracking (for incremental consumers: inflation, publication...) ---- + // Cells written since the last clear_changes(): recorded by NavMap::layer_set(), OccupancyLayer + // and mark_changed(). Bulk writes (set_data, layer_clear, copies) mark the whole layer, and so + // does creating it. Direct writes through data() must call mark_changed() themselves. + + /// @brief Record that cell @p cid changed (deduplicated). + void mark_changed(NavCelId cid) const + { + hash_dirty_ = true; + if (all_changed_) {return;} + const std::size_t word = cid / 64; + if (changed_bits_.size() <= word) { + changed_bits_.resize(std::max(word + 1, (size() + 63) / 64)); + } + const std::uint64_t bit = std::uint64_t{1} << (cid % 64); + if (!(changed_bits_[word] & bit)) { + changed_bits_[word] |= bit; + changed_.push_back(cid); + } + } + + /// @brief Record that the whole layer changed. + void mark_all_changed() const noexcept {hash_dirty_ = true; all_changed_ = true;} + + /// @brief Whether the whole layer changed (then changed() is not meaningful). + bool all_changed() const noexcept {return all_changed_;} + + /// @brief Cells changed one by one since the last clear_changes(), in order of first change. + const std::vector & changed() const noexcept {return changed_;} + + bool has_changes() const noexcept {return all_changed_ || !changed_.empty();} + + /// @brief Forget the recorded changes (call it once every consumer has seen them). + void clear_changes() const + { + for (const NavCelId c : changed_) {changed_bits_[c / 64] &= ~(std::uint64_t{1} << (c % 64));} + changed_.clear(); + all_changed_ = false; + } + protected: mutable bool hash_dirty_{true}; mutable std::uint64_t hash_cache_{0}; + mutable bool all_changed_{true}; // a new layer is all new + mutable std::vector changed_; + mutable std::vector changed_bits_; // allocated on the first single change }; /** @@ -191,7 +234,7 @@ struct LayerView : LayerViewBase { hash_dirty_ = true; return const_cast &>(data_); } - void set_data(const std::vector & v) {data_ = v; hash_dirty_ = true;} + void set_data(const std::vector & v) {data_ = v; mark_all_changed();} std::uint64_t content_hash() const override; std::shared_ptr clone() const override @@ -339,17 +382,22 @@ class LayerRegistry for (auto & kv : layers_) { if (auto v = std::dynamic_pointer_cast>(kv.second)) { v->data_.resize(nitems); - kv.second->mark_dirty(); + kv.second->mark_all_changed(); continue; } if (auto v = std::dynamic_pointer_cast>(kv.second)) { v->data_.resize(nitems); - kv.second->mark_dirty(); + kv.second->mark_all_changed(); continue; } if (auto v = std::dynamic_pointer_cast>(kv.second)) { v->data_.resize(nitems); - kv.second->mark_dirty(); + kv.second->mark_all_changed(); + continue; + } + if (auto v = std::dynamic_pointer_cast>(kv.second)) { + v->data_.resize(nitems); + kv.second->mark_all_changed(); continue; } } @@ -781,7 +829,7 @@ class NavMap auto v = layers.add_or_get(name, navcels.size(), layer_type_tag()); if (!existed || v->data().size() != navcels.size()) { // new layers start at default_value v->data().assign(navcels.size(), default_value); - v->mark_dirty(); + v->mark_all_changed(); } auto & meta = layer_meta[name]; meta.description = description; @@ -812,7 +860,7 @@ class NavMap auto view = layers.add_or_get(name, navcels.size(), layer_type_tag()); if (static_cast(cid) < view->data().size()) { view->data()[cid] = value; - view->mark_dirty(); + view->mark_changed(cid); } } @@ -929,7 +977,7 @@ class NavMap { auto view = layers.add_or_get(name, navcels.size(), layer_type_tag()); std::fill(view->data().begin(), view->data().end(), value); - view->mark_dirty(); + view->mark_all_changed(); } /** diff --git a/navmap_core/include/navmap_core/Occupancy.hpp b/navmap_core/include/navmap_core/Occupancy.hpp index 6320324..e0048a6 100644 --- a/navmap_core/include/navmap_core/Occupancy.hpp +++ b/navmap_core/include/navmap_core/Occupancy.hpp @@ -96,14 +96,14 @@ class OccupancyLayer if (cid >= d.size()) {return;} const int v = static_cast(d[cid]) + delta; d[cid] = static_cast(std::clamp(v, static_cast(min_), static_cast(max_))); - view_->mark_dirty(); + view_->mark_changed(cid); } /// Forgets @p cid (back to unknown). void reset(NavCelId cid) { auto & d = view_->data(); - if (cid < d.size()) {d[cid] = 0; view_->mark_dirty();} + if (cid < d.size()) {d[cid] = 0; view_->mark_changed(cid);} } float logodds(NavCelId cid) const {return value(cid) / kScale;} diff --git a/navmap_core/src/navmap_core/DistanceField.cpp b/navmap_core/src/navmap_core/DistanceField.cpp new file mode 100644 index 0000000..76c8735 --- /dev/null +++ b/navmap_core/src/navmap_core/DistanceField.cpp @@ -0,0 +1,306 @@ +// 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 "navmap_core/DistanceField.hpp" + +#include +#include +#include +#include + +#include "navmap_core/Geometry.hpp" + +namespace navmap +{ + +namespace +{ +constexpr float kInf = std::numeric_limits::infinity(); +constexpr NavCelId kNone = std::numeric_limits::max(); +} // namespace + +DistanceField::DistanceField(const NavMap & nm, bool from_boundary, float max_distance) +: nm_(nm), from_boundary_(from_boundary), max_distance_(max_distance) +{ + const std::size_t n = nm_.navcels.size(); + centroid_.resize(n); + for (NavCelId c = 0; c < n; ++c) { + centroid_[c] = nm_.navcel_centroid(c); + } + seed_of_.assign(n, kNoSeed); + nearest_.assign(n, kNoSeed); + dist_.assign(n, kInf); + touched_stamp_.assign(n, 0); + flood_stamp_.assign(n, 0); + before_.assign(n, kInf); + // Open edges: fixed segment seeds, indexed by their cell (CSR) + boundary_start_.assign(n + 1, 0); + if (from_boundary_) { + for (NavCelId c = 0; c < n; ++c) { + for (int e = 0; e < 3; ++e) { + if (nm_.navcels[c].neighbor[e] != kNone) {continue;} + seeds_.push_back( + {nm_.positions.at(nm_.navcels[c].v[e]), + nm_.positions.at(nm_.navcels[c].v[(e + 1) % 3]), c, true}); + } + boundary_start_[c + 1] = static_cast(seeds_.size()); + } + } + n_boundary_ = seeds_.size(); + bucket_ = std::isfinite(max_distance_) && max_distance_ > 0.0f ? max_distance_ : 1.0f; +} + +float DistanceField::distance_to(NavCelId c, std::uint32_t seed) const +{ + return point_segment_distance(centroid_[c], seeds_[seed].a, seeds_[seed].b); +} + +void DistanceField::touch(NavCelId c) +{ + if (touched_stamp_[c] == stamp_) {return;} + touched_stamp_[c] = stamp_; + before_[c] = dist_[c]; + touched_.push_back(c); +} + +void DistanceField::begin_operation() +{ + if (++stamp_ == 0) { + std::fill(touched_stamp_.begin(), touched_stamp_.end(), 0); + stamp_ = 1; + } +} + +void DistanceField::finish() +{ + changed_.clear(); + for (const NavCelId c : touched_) { + if (!(before_[c] == dist_[c])) {changed_.push_back(c);} // inf == inf: unchanged + } + touched_.clear(); +} + +// Floods from the seed's cell over adjacency, calling f(cell, distance) on each cell it enters; +// f returns whether to expand from that cell (the origin always expands). +template +void DistanceField::flood(std::uint32_t s, F && f) +{ + if (++flood_id_ == 0) { + std::fill(flood_stamp_.begin(), flood_stamp_.end(), 0); + flood_id_ = 1; + } + const std::size_t n = nm_.navcels.size(); + const NavCelId origin = seeds_[s].cell; + flood_.clear(); + flood_.push_back(origin); + flood_stamp_[origin] = flood_id_; + while (!flood_.empty()) { + const NavCelId c = flood_.back(); + flood_.pop_back(); + if (!f(c, distance_to(c, s)) && c != origin) {continue;} + for (const NavCelId v : nm_.navcels[c].neighbor) { + if (v == kNone || v >= n || flood_stamp_[v] == flood_id_) {continue;} + flood_stamp_[v] = flood_id_; + flood_.push_back(v); + } + } +} + +// Paints seed s over its whole disc (the cells it reaches through adjacency within max_distance), +// where it is nearer. Exact by construction: no pruning (regions are not star-shaped around holes). +void DistanceField::splat(std::uint32_t s) +{ + flood( + s, [&](NavCelId c, float d) { + if (d > max_distance_) {return false;} + if (d < dist_[c]) { + touch(c); + dist_[c] = d; + nearest_[c] = s; + } + return true; + }); +} + +std::int64_t DistanceField::bucket_key(int bx, int by, int bz) +{ + return (static_cast(bx) * 73856093) ^ (static_cast(by) * 19349663) ^ + (static_cast(bz) * 83492791); +} + +void DistanceField::index_seed(std::uint32_t s, bool insert) +{ + const Eigen::Vector3f lo = seeds_[s].a.cwiseMin(seeds_[s].b) / bucket_; + const Eigen::Vector3f hi = seeds_[s].a.cwiseMax(seeds_[s].b) / bucket_; + for (int bx = static_cast(std::floor(lo.x())); bx <= static_cast(std::floor(hi.x())); + ++bx) + { + for (int by = static_cast(std::floor(lo.y())); by <= static_cast(std::floor(hi.y())); + ++by) + { + for (int bz = + static_cast(std::floor(lo.z())); bz <= static_cast(std::floor(hi.z())); + ++bz) + { + auto & v = buckets_[bucket_key(bx, by, bz)]; + if (insert) { + v.push_back(s); + } else { + const auto it = std::find(v.begin(), v.end(), s); + if (it != v.end()) {*it = v.back(); v.pop_back();} + } + } + } + } +} + +std::uint32_t DistanceField::new_point_seed(NavCelId c) +{ + std::uint32_t s; + if (!free_seed_slots_.empty()) { + s = free_seed_slots_.back(); + free_seed_slots_.pop_back(); + seeds_[s] = {centroid_[c], centroid_[c], c, true}; + } else { + seeds_.push_back({centroid_[c], centroid_[c], c, true}); + s = static_cast(seeds_.size() - 1); + } + seed_of_[c] = s; + index_seed(s, true); + return s; +} + +void DistanceField::compute(const std::vector & seed_cells) +{ + const std::size_t n = nm_.navcels.size(); + begin_operation(); + for (NavCelId c = 0; c < n; ++c) { + touch(c); + } + std::fill(seed_of_.begin(), seed_of_.end(), kNoSeed); + seeds_.resize(n_boundary_); + free_seed_slots_.clear(); + buckets_.clear(); + if (std::isfinite(max_distance_)) { + for (std::uint32_t b = 0; b < n_boundary_; ++b) { + index_seed(b, true); + } + } + std::vector unique; + unique.reserve(seed_cells.size()); + for (const NavCelId c : seed_cells) { + if (c < n && seed_of_[c] == kNoSeed) { + new_point_seed(c); + unique.push_back(c); + } + } + if (!std::isfinite(max_distance_)) { + // Unbounded: nearest-feature propagation (NavMap::distance_field), approximate + dist_ = nm_.distance_field(unique, from_boundary_, max_distance_); + std::fill(nearest_.begin(), nearest_.end(), kNoSeed); + } else { + std::fill(dist_.begin(), dist_.end(), kInf); + std::fill(nearest_.begin(), nearest_.end(), kNoSeed); + for (std::uint32_t s = 0; s < seeds_.size(); ++s) { + splat(s); + } + } + finish(); +} + +void DistanceField::update( + const std::vector & added, const std::vector & removed) +{ + const std::size_t n = nm_.navcels.size(); + if (!std::isfinite(max_distance_)) { + // Unbounded: no local update, recompute with the new seed set + std::vector seeds; + for (NavCelId c = 0; c < n; ++c) { + if (seed_of_[c] != kNoSeed) {seeds.push_back(c);} + } + for (const NavCelId c : removed) { + if (c < n) {std::erase(seeds, c);} + } + for (const NavCelId c : added) { + if (c < n && std::find(seeds.begin(), seeds.end(), c) == seeds.end()) {seeds.push_back(c);} + } + const std::vector prev = dist_; + compute(seeds); + changed_.clear(); + for (NavCelId c = 0; c < n; ++c) { + if (!(prev[c] == dist_[c])) {changed_.push_back(c);} + } + return; + } + + begin_operation(); + // 1) Removed seeds: the cells measured to them go back to +inf, and every seed that may cover + // one of them is painted again. Such a seed is within max_distance of that cell, so within + // 2 * max_distance of the removed seed in a straight line: the seed index finds them. + std::vector repaint; + for (const NavCelId c : removed) { + if (c >= n || seed_of_[c] == kNoSeed) {continue;} + const std::uint32_t s = seed_of_[c]; + seed_of_[c] = kNoSeed; + seeds_[s].alive = false; + index_seed(s, false); + flood( + s, [&](NavCelId x, float d) { + if (d > max_distance_) {return false;} + if (nearest_[x] == s) { + touch(x); + dist_[x] = kInf; + nearest_[x] = kNoSeed; + } + return true; + }); + const Eigen::Vector3f p = seeds_[s].a; + const float r2 = 2.0f * max_distance_; + const Eigen::Vector3f lo = (p.array() - r2).matrix() / bucket_; + const Eigen::Vector3f hi = (p.array() + r2).matrix() / bucket_; + for (int bx = static_cast(std::floor(lo.x())); bx <= static_cast(std::floor(hi.x())); + ++bx) + { + for (int by = + static_cast(std::floor(lo.y())); by <= static_cast(std::floor(hi.y())); + ++by) + { + for (int bz = static_cast(std::floor(lo.z())); + bz <= static_cast(std::floor(hi.z())); ++bz) + { + const auto it = buckets_.find(bucket_key(bx, by, bz)); + if (it == buckets_.end()) {continue;} + for (const std::uint32_t q : it->second) { + if (point_segment_distance(p, seeds_[q].a, seeds_[q].b) <= r2) {repaint.push_back(q);} + } + } + } + } + free_seed_slots_.push_back(s); + } + std::sort(repaint.begin(), repaint.end()); + repaint.erase(std::unique(repaint.begin(), repaint.end()), repaint.end()); + for (const std::uint32_t s : repaint) { + if (seeds_[s].alive) {splat(s);} + } + // 2) New seeds + for (const NavCelId c : added) { + if (c >= n || seed_of_[c] != kNoSeed) {continue;} + splat(new_point_seed(c)); + } + finish(); +} + +} // namespace navmap diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index d314578..8ba8b95 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -1109,17 +1109,6 @@ bool NavMap::locate_navcel( // ---- 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 { diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index 2b61c06..c531a65 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -62,3 +62,8 @@ ament_add_gtest(test_layers_unknown_occupancy test_layers_unknown_occupancy.cpp) target_link_libraries(test_layers_unknown_occupancy ${PROJECT_NAME} ) + +ament_add_gtest(test_incremental test_incremental.cpp) +target_link_libraries(test_incremental + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_incremental.cpp b/navmap_core/tests/test_incremental.cpp new file mode 100644 index 0000000..67ccaca --- /dev/null +++ b/navmap_core/tests/test_incremental.cpp @@ -0,0 +1,337 @@ +// 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 + +#include "navmap_core/DistanceField.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_core/Occupancy.hpp" + +using navmap::DistanceField; +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +constexpr float kInf = std::numeric_limits::infinity(); + +void 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))); + } + } +} + +std::vector maps() +{ + std::vector out(3); + add_grid(out[0], 20, 20, 0.5f, [](float, float) {return 0.0f;}, + [](int i, int j) { + return (i == 8 && j > 3 && j < 15) || (i > 12 && i < 15 && j > 12 && j < 15); + }); + add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 0.0f;}); + add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 3.0f;}); + add_grid(out[2], 30, 6, 0.4f, [](float x, float) {return 0.25f * x;}); + for (auto & m : out) { + m.rebuild_geometry_accels(); + } + return out; +} + +float max_diff(const std::vector & a, const std::vector & b) +{ + float m = 0.0f; + for (std::size_t i = 0; i < a.size(); ++i) { + if (std::isinf(a[i]) || std::isinf(b[i])) { + if (std::isinf(a[i]) != std::isinf(b[i])) {return kInf;} + continue; + } + m = std::max(m, std::abs(a[i] - b[i])); + } + return m; +} + +} // namespace + +// ------------------------------------------------------------------ distance field + +// Exact field by brute force: for every seed, the cells it reaches through adjacency within +// max_distance of it, at their straight distance. +std::vector brute( + const NavMap & nm, const std::vector & seeds, bool boundary, + float maxd) +{ + struct S {Eigen::Vector3f a, b; NavCelId cell;}; + std::vector all; + for (NavCelId c : seeds) { + all.push_back({nm.navcel_centroid(c), nm.navcel_centroid(c), c}); + } + if (boundary) { + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + for (int e = 0; e < 3; ++e) { + if (nm.navcels[c].neighbor[e] != std::numeric_limits::max()) {continue;} + all.push_back({nm.positions.at(nm.navcels[c].v[e]), + nm.positions.at(nm.navcels[c].v[(e + 1) % 3]), c}); + } + } + } + std::vector d(nm.navcels.size(), kInf); + for (const auto & sd : all) { + auto dist = [&](NavCelId c) { + return navmap::point_segment_distance(nm.navcel_centroid(c), sd.a, sd.b); + }; + std::vector seen(nm.navcels.size(), 0); + std::vector st{sd.cell}; + seen[sd.cell] = 1; + while (!st.empty()) { + const NavCelId c = st.back(); + st.pop_back(); + const float x = dist(c); + if (x > maxd && c != sd.cell) {continue;} + if (x <= maxd) {d[c] = std::min(d[c], x);} + for (NavCelId v : nm.navcels[c].neighbor) { + if (v != std::numeric_limits::max() && !seen[v]) {seen[v] = 1; st.push_back(v);} + } + } + } + return d; +} + +TEST(DistanceFieldIncremental, ComputeIsExactWithABoundAndMatchesNavMapWithout) +{ + for (const auto & nm : maps()) { + std::mt19937 rng(1); + std::uniform_int_distribution C(0, static_cast(nm.navcels.size() - 1)); + std::vector seeds; + for (int k = 0; k < 6; ++k) { + seeds.push_back(C(rng)); + } + for (const bool boundary : {false, true}) { + DistanceField bounded(nm, boundary, 2.0f); + bounded.compute(seeds); + EXPECT_EQ(max_diff(bounded.distances(), brute(nm, seeds, boundary, 2.0f)), 0.0f); + DistanceField unbounded(nm, boundary); + unbounded.compute(seeds); + EXPECT_EQ(unbounded.distances(), nm.distance_field(seeds, boundary)); + for (NavCelId s : seeds) { + EXPECT_TRUE(bounded.is_seed(s) && unbounded.is_seed(s)); + } + } + } +} + +TEST(DistanceFieldIncremental, RandomUpdatesMatchARecomputationAndReportTheChanges) +{ + int m = 0; + for (const auto & nm : maps()) { + for (const bool boundary : {false, true}) { + for (const float maxd : {kInf, 1.5f}) { + std::mt19937 rng(10 + m++); + std::uniform_int_distribution C(0, static_cast(nm.navcels.size() - 1)); + std::set seeds; + DistanceField df(nm, boundary, maxd); + df.compute({}); + float worst = 0.0f; + for (int step = 0; step < 60; ++step) { + std::vector add, rem; + const int na = static_cast(rng() % 4), + nr = seeds.empty() ? 0 : static_cast(rng() % 3); + for (int k = 0; k < na; ++k) { + add.push_back(C(rng)); + } + for (int k = 0; k < nr; ++k) { + auto it = seeds.begin(); + std::advance(it, rng() % seeds.size()); + rem.push_back(*it); + } + if (rng() % 5 == 0) {rem.push_back(C(rng));} // removing a non-seed: ignored + if (rng() % 5 == 0 && !add.empty()) {add.push_back(add.front());} // repeated + const std::vector prev = df.distances(); + df.update(add, rem); + for (NavCelId r : rem) { + seeds.erase(r); + } + for (NavCelId a : add) { + seeds.insert(a); + } + DistanceField ref(nm, boundary, maxd); + ref.compute({seeds.begin(), seeds.end()}); + const float diff = max_diff(df.distances(), ref.distances()); + if (diff > 1e-5f && worst <= 1e-5f) { + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (std::abs(df.distances()[c] - ref.distances()[c]) > 1e-5f) { + const auto p = nm.navcel_centroid(c); + ADD_FAILURE() << "step " << step << " cell " << c << " at " << p.transpose() << + " inc " + << df.distances()[c] << " full " << ref.distances()[c]; + break; + } + } + } + worst = std::max(worst, diff); + // changed() is exactly the set of cells whose value differs + std::set want; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const float a = prev[c], b = df.distances()[c]; + if (!(a == b)) {want.insert(c);} + } + const std::set got(df.changed().begin(), df.changed().end()); + ASSERT_EQ(got, want) << "step " << step; + EXPECT_EQ(got.size(), df.changed().size()); // no duplicates + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + ASSERT_EQ(df.is_seed(c), seeds.count(c) > 0); + } + } + EXPECT_LE(worst, 1e-5f) << "map " << m - 1 << " boundary " << boundary << " max " << maxd; + } + } + } +} + +TEST(DistanceFieldIncremental, AddingAndRemovingTheSameCellAndEdgeIds) +{ + const NavMap nm = maps()[1]; + DistanceField df(nm, false, 2.0f); + df.compute({5}); + // Same cell removed and added in one call: removal first, so it stays a seed + df.update({5}, {5}); + EXPECT_TRUE(df.is_seed(5)); + EXPECT_EQ(df[5], 0.0f); + // Out-of-range ids are ignored + df.update({99999}, {99999}); + EXPECT_FALSE(df.is_seed(99999)); + // Removing the only seed: everything unreachable again + df.update({}, {5}); + for (float d : df.distances()) { + EXPECT_TRUE(std::isinf(d)); + } + // A seed on the upper floor never reaches the lower one + NavCelId upper = 0; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (nm.navcel_centroid(c).z() > 2.0f) {upper = c; break;} + } + df.update({upper}, {}); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (nm.navcel_centroid(c).z() < 1.0f) {EXPECT_TRUE(std::isinf(df[c]));} + } +} + +TEST(DistanceFieldIncremental, AnUpdateOnlyTouchesItsNeighbourhood) +{ + // A large map: adding one seed changes only the cells within max_distance of it. + NavMap nm; + add_grid(nm, 200, 200, 0.1f, [](float, float) {return 0.0f;}); + nm.rebuild_geometry_accels(); + DistanceField df(nm, true, 0.5f); + df.compute({}); + std::size_t s; + NavCelId c; + Eigen::Vector3f bary, hit; + ASSERT_TRUE(nm.locate_navcel({10.03f, 10.07f, 0.0f}, s, c, bary, &hit)); + df.update({c}, {}); + EXPECT_GT(df.changed().size(), 100u); + EXPECT_LT(df.changed().size(), 2u * 3.2f * 25.0f * 2.0f); // ~ pi r^2 / (res^2 / 2), with margin + for (NavCelId x : df.changed()) { + EXPECT_LE((nm.navcel_centroid(x) - nm.navcel_centroid(c)).norm(), 0.5f + 1e-4f); + } + df.update({}, {c}); + EXPECT_EQ(df.changed().size(), df.changed().size()); + for (NavCelId x = 0; x < nm.navcels.size(); ++x) { + ASSERT_TRUE(std::isinf(df[x]) || df[x] <= 0.5f); + } +} + +// ------------------------------------------------------------------ layer change tracking + +TEST(LayerChanges, SingleWritesAreRecordedOnceAndBulkWritesMarkTheLayer) +{ + NavMap nm = maps()[1]; + auto v = nm.add_layer("occ", "", "", uint8_t{0}); + EXPECT_TRUE(v->all_changed()); // new layer: all new + v->clear_changes(); + EXPECT_FALSE(v->has_changes()); + nm.layer_set("occ", 7, 254); + nm.layer_set("occ", 3, 254); + nm.layer_set("occ", 7, 100); // again: recorded once + EXPECT_FALSE(v->all_changed()); + EXPECT_EQ(v->changed(), (std::vector{7, 3})); + v->clear_changes(); + EXPECT_TRUE(v->changed().empty()); + nm.layer_set("occ", 7, 1); // recorded again after a clear + EXPECT_EQ(v->changed(), (std::vector{7})); + nm.layer_clear("occ", 0); + EXPECT_TRUE(v->all_changed()); + v->clear_changes(); + v->set_data(std::vector(nm.navcels.size(), 3)); + EXPECT_TRUE(v->all_changed()); + v->clear_changes(); + // Out-of-range writes record nothing; mark_changed() for direct writes through data() + nm.layer_set("occ", 99999, 1); + EXPECT_FALSE(v->has_changes()); + v->data()[11] = 9; + v->mark_changed(11); + EXPECT_EQ(v->changed(), (std::vector{11})); +} + +TEST(LayerChanges, HashFollowsChangesAndCopiesCarryPendingOnes) +{ + NavMap nm = maps()[0]; + auto v = nm.add_layer("f", "", "", 0.0f); + v->clear_changes(); + const auto h0 = v->content_hash(); + nm.layer_set("f", 2, 1.0f); + EXPECT_NE(v->content_hash(), h0); + NavMap copy(nm); + auto cv = copy.layers.get("f"); + EXPECT_EQ(cv->changed(), (std::vector{2})); + cv->clear_changes(); // independent from the original's + EXPECT_EQ(v->changed(), (std::vector{2})); +} + +TEST(LayerChanges, OccupancyUpdatesAreRecorded) +{ + NavMap nm = maps()[1]; + navmap::OccupancyLayer occ(nm); + occ.view()->clear_changes(); + occ.hit(4); + occ.miss(9); + occ.hit(4); + occ.reset(12); + EXPECT_EQ(occ.view()->changed(), (std::vector{4, 9, 12})); +} diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 055c6de..f79367d 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -111,7 +111,7 @@ void set_layer(navmap::NavMap & nm, const NavMapLayer & msg, const V & data) using T = typename V::value_type; auto dst = nm.add_layer(msg.name, msg.description, msg.unit, T{}); dst->data().assign(data.begin(), data.end()); - dst->mark_dirty(); + dst->mark_all_changed(); } // Message -> layer data and metadata (the layer is created or retyped as needed). From cd5dc980a2dea715a73d920b6371a034d1e85420 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 10:08:58 +0200 Subject: [PATCH 05/11] Add per-NavCel geometric descriptors MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit navmap_core/Descriptors.hpp: compute_descriptors() adds F32 layers slope (deg), roughness (RMS distance to the plane fitted in a disc, m), step (height span in a small disc, m) and clearance (free height to the next surface of the map above, by an upward ray through the BVH; +inf if none). Discs grow over the mesh adjacency, so floors never mix. They describe the surface only; a planner decides how to weigh them. Signed-off-by: Francisco Martín Rico --- README.md | 6 + navmap_core/CMakeLists.txt | 1 + .../include/navmap_core/Descriptors.hpp | 59 ++++++ navmap_core/src/navmap_core/Descriptors.cpp | 109 ++++++++++ navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_descriptors.cpp | 191 ++++++++++++++++++ 6 files changed, 371 insertions(+) create mode 100644 navmap_core/include/navmap_core/Descriptors.hpp create mode 100644 navmap_core/src/navmap_core/Descriptors.cpp create mode 100644 navmap_core/tests/test_descriptors.cpp diff --git a/README.md b/README.md index c77cae2..19a8b1c 100644 --- a/README.md +++ b/README.md @@ -293,6 +293,12 @@ With a finite `max_distance` the field is exact (straight distance to the neares --- +## 7e. Geometric descriptors (`Descriptors.hpp`) + +`navmap::compute_descriptors(nm)` adds F32 layers per NavCel: `slope` (deg), `roughness` (RMS distance to the plane fitted in a disc, m), `step` (height span in a small disc, m) and `clearance` (free height to the next surface of the map above, m; +inf if none). Discs grow over the surface, so floors never mix. They only describe the surface: weighing them into a cost is up to the planner. + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index eb9d3c8..266fb3b 100644 --- a/navmap_core/CMakeLists.txt +++ b/navmap_core/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(Eigen3 REQUIRED) add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/NavMap.cpp src/${PROJECT_NAME}/DistanceField.cpp + src/${PROJECT_NAME}/Descriptors.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/navmap_core/include/navmap_core/Descriptors.hpp b/navmap_core/include/navmap_core/Descriptors.hpp new file mode 100644 index 0000000..6c6e7de --- /dev/null +++ b/navmap_core/include/navmap_core/Descriptors.hpp @@ -0,0 +1,59 @@ +// 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. + +/** + * @file Descriptors.hpp + * @brief Geometric descriptors per NavCel (slope, roughness, step, clearance), as layers. + * + * They describe the surface only. Turning them into traversability or cost is a decision of the + * algorithm that uses them (a planner), not of the map. + */ + +#ifndef NAVMAP_CORE__DESCRIPTORS_HPP_ +#define NAVMAP_CORE__DESCRIPTORS_HPP_ + +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/// Which descriptors to compute, and over which neighbourhoods. +struct DescriptorParams +{ + bool slope{true}; ///< "slope": angle of the NavCel normal to the vertical (deg) + bool roughness{true}; ///< "roughness": RMS distance to the plane fitted in a disc (m) + bool step{true}; ///< "step": height span of the vertices in a disc (m) + bool clearance{true}; ///< "clearance": free height to the next surface above (m) + float roughness_radius{0.3f}; ///< Disc for the roughness plane fit (m) + float step_radius{0.2f}; ///< Disc for the step (m) + float clearance_max{10.0f}; ///< Beyond this, no surface above: +inf +}; + +/** + * @brief Compute the selected descriptors as F32 layers of @p nm (created or overwritten). + * + * Discs grow over the mesh adjacency from each NavCel (navmap/Iterators.hpp), so a descriptor + * never mixes floors. "clearance" casts a ray up from each centroid against every surface of the + * map: it is the room up to the next surface the map holds (the floor above), not to a ceiling + * that is not in the map; +inf when there is none within clearance_max. + */ +void compute_descriptors(NavMap & nm, const DescriptorParams & params = {}); + +} // namespace navmap + +#endif // NAVMAP_CORE__DESCRIPTORS_HPP_ diff --git a/navmap_core/src/navmap_core/Descriptors.cpp b/navmap_core/src/navmap_core/Descriptors.cpp new file mode 100644 index 0000000..467d7a5 --- /dev/null +++ b/navmap_core/src/navmap_core/Descriptors.cpp @@ -0,0 +1,109 @@ +// 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 "navmap_core/Descriptors.hpp" + +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "navmap_core/Iterators.hpp" + +namespace navmap +{ + +void compute_descriptors(NavMap & nm, const DescriptorParams & p) +{ + const std::size_t n = nm.navcels.size(); + constexpr float kInf = std::numeric_limits::infinity(); + TraversalScratch scratch; + std::vector pts; + + if (p.slope) { + auto v = nm.add_layer("slope", "angle of the surface to the horizontal", "deg", 0.0f); + for (NavCelId c = 0; c < n; ++c) { + const float nz = std::clamp(std::abs(nm.navcels[c].normal.z()), 0.0f, 1.0f); + v->data()[c] = std::acos(nz) * 180.0f / static_cast(M_PI); + } + v->mark_all_changed(); + } + + if (p.roughness) { + auto v = nm.add_layer( + "roughness", "RMS distance to the plane fitted around the NavCel", "m", 0.0f); + for (NavCelId c = 0; c < n; ++c) { + const Eigen::Vector3f center = nm.navcel_centroid(c); + pts.clear(); + for_each_in_disc( + nm, c, center, p.roughness_radius, + [&](NavCelId x) {pts.push_back(nm.navcel_centroid(x));}, Metric::EUCLIDEAN, &scratch); + float rms = 0.0f; + if (pts.size() >= 3) { + Eigen::Vector3f mean = Eigen::Vector3f::Zero(); + for (const auto & q : pts) { + mean += q; + } + mean /= static_cast(pts.size()); + Eigen::Matrix3f cov = Eigen::Matrix3f::Zero(); + for (const auto & q : pts) { + cov += (q - mean) * (q - mean).transpose(); + } + // The smallest eigenvalue is the summed squared distance to the best plane + const Eigen::SelfAdjointEigenSolver es(cov, Eigen::EigenvaluesOnly); + rms = std::sqrt(std::max(0.0f, es.eigenvalues()[0]) / static_cast(pts.size())); + } + v->data()[c] = rms; + } + v->mark_all_changed(); + } + + if (p.step) { + auto v = nm.add_layer( + "step", "height span of the surface around the NavCel", "m", 0.0f); + for (NavCelId c = 0; c < n; ++c) { + float lo = kInf, hi = -kInf; + for_each_in_disc( + nm, c, nm.navcel_centroid(c), p.step_radius, + [&](NavCelId x) { + for (const auto vid : nm.navcels[x].v) { + const float z = nm.positions.z[vid]; + lo = std::min(lo, z); + hi = std::max(hi, z); + } + }, Metric::EUCLIDEAN, &scratch); + v->data()[c] = hi - lo; + } + v->mark_all_changed(); + } + + if (p.clearance) { + auto v = nm.add_layer( + "clearance", "free height to the next surface above", "m", kInf); + const Eigen::Vector3f up(0.0f, 0.0f, 1.0f); + for (NavCelId c = 0; c < n; ++c) { + const Eigen::Vector3f o = nm.navcel_centroid(c) + Eigen::Vector3f(0.0f, 0.0f, 1e-3f); + NavCelId hit_cid; + float t; + Eigen::Vector3f hit; + const bool hit_any = nm.raycast(o, up, hit_cid, t, hit) && hit_cid != c; + v->data()[c] = hit_any && t + 1e-3f <= p.clearance_max ? t + 1e-3f : kInf; + } + v->mark_all_changed(); + } +} + +} // namespace navmap diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index c531a65..21e8113 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -67,3 +67,8 @@ ament_add_gtest(test_incremental test_incremental.cpp) target_link_libraries(test_incremental ${PROJECT_NAME} ) + +ament_add_gtest(test_descriptors test_descriptors.cpp) +target_link_libraries(test_descriptors + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_descriptors.cpp b/navmap_core/tests/test_descriptors.cpp new file mode 100644 index 0000000..ad05a3a --- /dev/null +++ b/navmap_core/tests/test_descriptors.cpp @@ -0,0 +1,191 @@ +// 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/Descriptors.hpp" +#include "navmap_core/NavMap.hpp" + +using navmap::DescriptorParams; +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +void add_grid( + NavMap & nm, int nx, int ny, float res, const std::function & f) +{ + 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) { + 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))); + } + } +} + +float at(const NavMap & nm, const std::string & layer, 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 nm.layer_get(layer, c); +} + +// Interior NavCels (away from the map border by `margin`) +std::vector interior(const NavMap & nm, float x0, float x1, float y0, float y1) +{ + std::vector out; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.navcel_centroid(c); + if (p.x() > x0 && p.x() < x1 && p.y() > y0 && p.y() < y1) {out.push_back(c);} + } + return out; +} + +} // namespace + +TEST(Descriptors, FlatGroundIsAllZeroWithNothingAbove) +{ + NavMap nm; + add_grid(nm, 30, 30, 0.1f, [](float, float) {return 0.0f;}); + nm.rebuild_geometry_accels(); + navmap::compute_descriptors(nm); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_NEAR(nm.layer_get("slope", c), 0.0f, 1e-3f); + EXPECT_NEAR(nm.layer_get("roughness", c), 0.0f, 1e-5f); + EXPECT_NEAR(nm.layer_get("step", c), 0.0f, 1e-6f); + EXPECT_TRUE(std::isinf(nm.layer_get("clearance", c))); + } + EXPECT_EQ(nm.get_layer_meta("slope")->unit, "deg"); + EXPECT_EQ(nm.get_layer_meta("clearance")->unit, "m"); +} + +TEST(Descriptors, AnInclineHasItsSlopeAStepOfItsDiscAndNoRoughness) +{ + const float th = 20.0f * static_cast(M_PI) / 180.0f; + NavMap nm; + add_grid(nm, 40, 20, 0.1f, [th](float x, float) {return std::tan(th) * x;}); + nm.rebuild_geometry_accels(); + navmap::compute_descriptors(nm); + for (NavCelId c : interior(nm, 0.5f, 3.5f, 0.5f, 1.5f)) { + EXPECT_NEAR(nm.layer_get("slope", c), 20.0f, 0.01f); + EXPECT_LT(nm.layer_get("roughness", c), 1e-4f); + // Vertices of the NavCels within 0.2 m: about 0.4-0.6 m across along the incline + const float step = nm.layer_get("step", c); + EXPECT_GE(step, 0.39f * std::tan(th)); + EXPECT_LE(step, 0.65f * std::tan(th)); + } +} + +TEST(Descriptors, RoughnessFollowsTheNoise) +{ + for (const float sigma : {0.005f, 0.02f, 0.05f}) { + NavMap nm; + std::mt19937 rng(3); + std::normal_distribution N(0.0f, sigma); + add_grid(nm, 40, 40, 0.1f, [&](float, float) {return N(rng);}); + nm.rebuild_geometry_accels(); + DescriptorParams p; + p.roughness_radius = 0.4f; + navmap::compute_descriptors(nm, p); + std::vector r; + for (NavCelId c : interior(nm, 0.5f, 3.5f, 0.5f, 3.5f)) { + r.push_back(nm.layer_get("roughness", c)); + } + std::nth_element(r.begin(), r.begin() + r.size() / 2, r.end()); + const float med = r[r.size() / 2]; + // Centroids average three vertices: their spread is about sigma / sqrt(3) + EXPECT_GT(med, 0.4f * sigma) << sigma; + EXPECT_LT(med, 0.8f * sigma) << sigma; + } +} + +TEST(Descriptors, ACurbIsAStepAndASteepStrip) +{ + // 0.15 m curb rising over one 0.1 m column at x in [2.0, 2.1] + NavMap nm; + add_grid(nm, 40, 20, 0.1f, [](float x, float) {return x > 2.05f ? 0.15f : 0.0f;}); + nm.rebuild_geometry_accels(); + navmap::compute_descriptors(nm); + EXPECT_NEAR(at(nm, "step", 1.95f, 1.0f, 0.0f), 0.15f, 1e-5f); // next to it + EXPECT_NEAR(at(nm, "step", 2.15f, 1.0f, 0.15f), 0.15f, 1e-5f); + EXPECT_NEAR(at(nm, "step", 0.8f, 1.0f, 0.0f), 0.0f, 1e-6f); // far from it + EXPECT_NEAR(at(nm, "step", 3.5f, 1.0f, 0.15f), 0.0f, 1e-6f); + EXPECT_GT(at(nm, "slope", 2.05f, 1.0f, 0.07f), 50.0f); // the riser + EXPECT_LT(at(nm, "slope", 0.8f, 1.0f, 0.0f), 0.01f); +} + +TEST(Descriptors, StackedFloorsClearanceAndNoMixing) +{ + NavMap nm; + add_grid(nm, 20, 20, 0.1f, [](float, float) {return 0.0f;}); + add_grid(nm, 20, 20, 0.1f, [](float, float) {return 3.0f;}); + nm.rebuild_geometry_accels(); + navmap::compute_descriptors(nm); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const bool lower = nm.navcel_centroid(c).z() < 1.0f; + const float cl = nm.layer_get("clearance", c); + if (lower) {EXPECT_NEAR(cl, 3.0f, 1e-3f);} else {EXPECT_TRUE(std::isinf(cl));} + // Discs stay on their floor: no 3 m step, no roughness from the other floor + EXPECT_NEAR(nm.layer_get("step", c), 0.0f, 1e-6f); + EXPECT_NEAR(nm.layer_get("roughness", c), 0.0f, 1e-5f); + } + // Beyond clearance_max there is nothing above + DescriptorParams p; + p.clearance_max = 2.0f; + navmap::compute_descriptors(nm, p); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_TRUE(std::isinf(nm.layer_get("clearance", c))); + } +} + +TEST(Descriptors, OnlyTheSelectedOnesAndExistingLayersAreOverwritten) +{ + NavMap nm; + add_grid(nm, 10, 10, 0.1f, [](float, float) {return 0.0f;}); + nm.rebuild_geometry_accels(); + nm.add_layer("slope", "", "", 99.0f); + DescriptorParams p; + p.roughness = p.step = p.clearance = false; + navmap::compute_descriptors(nm, p); + EXPECT_TRUE(nm.has_layer("slope")); + EXPECT_FALSE(nm.has_layer("roughness")); + EXPECT_FALSE(nm.has_layer("step")); + EXPECT_FALSE(nm.has_layer("clearance")); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_NEAR(nm.layer_get("slope", c), 0.0f, 1e-3f); + } + EXPECT_TRUE(nm.layers.get("slope")->all_changed()); + // An empty map: nothing to do, no crash + NavMap empty; + navmap::compute_descriptors(empty); + EXPECT_EQ(empty.layer_size("slope"), 0u); +} From d63b284df0a838675b869a2d829e8604d5f71723 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 11:06:17 +0200 Subject: [PATCH 06/11] Add links between non-adjacent NavCels MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit SurfaceLink (from, to, type, length, bidirectional) records connections such as stairs, elevators or doors between NavCels that do not share an edge: NavMap::add_link / remove_link / clear_links / links() / links_from(), with a lazily rebuilt index. The map only records them; a planner decides whether a robot can take one. Copies and assignments carry them. They travel in the new NavMapLink[] links field of NavMap (file format version 2; version 1 files have none), invalid ones are dropped on load, and RViz2 draws them coloured by type (Draw Links). Signed-off-by: Francisco Martín Rico --- README.md | 12 ++ navmap_core/include/navmap_core/NavMap.hpp | 45 ++++++ navmap_core/src/navmap_core/NavMap.cpp | 49 +++++++ navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_links.cpp | 133 ++++++++++++++++++ navmap_ros/src/navmap_ros/conversions.cpp | 20 +++ navmap_ros/src/navmap_ros/navmap_io.cpp | 3 +- .../test_layer_metadata_and_migration.cpp | 33 +++++ navmap_ros_interfaces/CMakeLists.txt | 1 + navmap_ros_interfaces/msg/NavMap.msg | 3 + navmap_ros_interfaces/msg/NavMapLink.msg | 14 ++ .../navmap_rviz_plugin/NavMapDisplay.hpp | 4 + .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 67 +++++++++ 13 files changed, 388 insertions(+), 1 deletion(-) create mode 100644 navmap_core/tests/test_links.cpp create mode 100644 navmap_ros_interfaces/msg/NavMapLink.msg diff --git a/README.md b/README.md index 19a8b1c..26ada0b 100644 --- a/README.md +++ b/README.md @@ -299,6 +299,18 @@ With a finite `max_distance` the field is exact (straight distance to the neares --- +## 7f. Links between surfaces (`SurfaceLink`) + +Stairs, elevators or doors between NavCels that do not share an edge. The map only records them; whether a robot can take one, and at which cost, is up to the planner. They travel in the message and the file, and RViz2 draws them (*Draw Links*). + +```cpp +nm.add_link(ground_cid, upper_cid, navmap::LinkType::ELEVATOR); // both ways +nm.add_link(top_cid, bottom_cid, navmap::LinkType::STAIRS, 4.5f, false); // one way, 4.5 m +for (std::uint32_t i : nm.links_from(cid)) {const auto & l = nm.links()[i]; /* ... */} +``` + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 7d51d0a..95d0d32 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -539,6 +539,24 @@ enum class AreaShape { CIRCULAR, RECTANGULAR }; * - Create layers via \ref LayerRegistry::add_or_get() sized to navcels.size(). * - Query with \ref locate_navcel(), \ref raycast(), or \ref closest_navcel(). */ +/// Kind of a link between NavCels that are not adjacent in the mesh. +enum class LinkType : uint8_t { GENERIC = 0, RAMP = 1, STAIRS = 2, ELEVATOR = 3, DOOR = 4 }; + +/** + * \brief Connection between two NavCels that do not share an edge (stairs, an elevator, a door + * between surfaces that are not welded...). + * + * The map only records it; whether a robot can take it, and at which cost, is up to the planner. + */ +struct SurfaceLink +{ + NavCelId from{0}; + NavCelId to{0}; + LinkType type{LinkType::GENERIC}; + float length{0.0f}; ///< Travel length (m) + bool bidirectional{true}; ///< Also from `to` to `from` +}; + class NavMap { public: @@ -778,6 +796,30 @@ class NavMap /// \brief Return up to 3 neighbor cell ids (skips invalid entries). std::vector navcel_neighbors(NavCelId cid) const; + // ---- Links between NavCels that are not adjacent (SurfaceLink) ---- + + /// Value returned by add_link() when a NavCel id is out of range. + static constexpr std::size_t kInvalidLink = std::numeric_limits::max(); + + /** + * \brief Add a link from \p from to \p to. A negative \p length takes the distance between + * their centroids. + * \return Index of the link, or kInvalidLink if an id is out of range or from == to. + */ + std::size_t add_link( + NavCelId from, NavCelId to, LinkType type = LinkType::GENERIC, float length = -1.0f, + bool bidirectional = true); + + /// \brief Remove link \p index (the last link takes its index). False if out of range. + bool remove_link(std::size_t index); + + void clear_links(); + + const std::vector & links() const {return links_;} + + /// \brief Indices of the links that can be taken from \p cid (both ends of bidirectional ones). + const std::vector & links_from(NavCelId cid) const; + /** * \brief Distance (m, 3D) from each NavCel centroid to the nearest seed. * @@ -1251,6 +1293,9 @@ class NavMap // Owning surface per NavCel, built in rebuild_geometry_accels(). std::vector navcel_surface_; + std::vector links_; + mutable std::vector> link_index_; + mutable bool link_index_dirty_{true}; void build_navcel_surface_index_(); // Builders and traversal helpers. diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 8ba8b95..332b5a0 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -60,6 +60,8 @@ NavMap::NavMap(const NavMap & other) layer_meta = other.layer_meta; navcel_surface_ = other.navcel_surface_; nonmanifold_edges_ = other.nonmanifold_edges_; + links_ = other.links_; + link_index_dirty_ = true; geometry_dirty_ = true; geometry_fp_ = 0; @@ -181,6 +183,8 @@ NavMap & NavMap::operator=(const NavMap & other) } layer_meta = other.layer_meta; + links_ = other.links_; + link_index_dirty_ = true; return *this; } @@ -193,6 +197,8 @@ NavMap & NavMap::operator=(const NavMap & other) layer_meta = other.layer_meta; navcel_surface_ = other.navcel_surface_; nonmanifold_edges_ = other.nonmanifold_edges_; + links_ = other.links_; + link_index_dirty_ = true; geometry_dirty_ = true; return *this; @@ -211,6 +217,8 @@ NavMap & NavMap::operator=(NavMap && other) noexcept layer_meta = std::move(other.layer_meta); navcel_surface_ = std::move(other.navcel_surface_); nonmanifold_edges_ = other.nonmanifold_edges_; + links_ = std::move(other.links_); + link_index_dirty_ = true; geometry_fp_ = other.geometry_fp_; geometry_dirty_ = other.geometry_dirty_; @@ -1170,6 +1178,47 @@ std::vector NavMap::distance_field( return dist; } +// ---- Links ---- +std::size_t NavMap::add_link( + NavCelId from, NavCelId to, LinkType type, float length, bool bidirectional) +{ + if (from >= navcels.size() || to >= navcels.size() || from == to) {return kInvalidLink;} + if (length < 0.0f) {length = (navcel_centroid(to) - navcel_centroid(from)).norm();} + links_.push_back({from, to, type, length, bidirectional}); + link_index_dirty_ = true; + return links_.size() - 1; +} + +bool NavMap::remove_link(std::size_t index) +{ + if (index >= links_.size()) {return false;} + links_[index] = links_.back(); + links_.pop_back(); + link_index_dirty_ = true; + return true; +} + +void NavMap::clear_links() +{ + links_.clear(); + link_index_dirty_ = true; +} + +const std::vector & NavMap::links_from(NavCelId cid) const +{ + static const std::vector kEmpty; + if (link_index_dirty_ || link_index_.size() != navcels.size()) { + link_index_.assign(navcels.size(), {}); + for (std::uint32_t i = 0; i < links_.size(); ++i) { + const auto & l = links_[i]; + if (l.from < navcels.size()) {link_index_[l.from].push_back(i);} + if (l.bidirectional && l.to < navcels.size()) {link_index_[l.to].push_back(i);} + } + link_index_dirty_ = false; + } + return cid < link_index_.size() ? link_index_[cid] : kEmpty; +} + // ---- 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 21e8113..ac4fb3b 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -72,3 +72,8 @@ ament_add_gtest(test_descriptors test_descriptors.cpp) target_link_libraries(test_descriptors ${PROJECT_NAME} ) + +ament_add_gtest(test_links test_links.cpp) +target_link_libraries(test_links + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_links.cpp b/navmap_core/tests/test_links.cpp new file mode 100644 index 0000000..e9846c6 --- /dev/null +++ b/navmap_core/tests/test_links.cpp @@ -0,0 +1,133 @@ +// 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 "navmap_core/NavMap.hpp" + +using navmap::LinkType; +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +// Two separate 2 x 2 m floors, 3 m apart (8 NavCels each): surfaces 0 and 1. +NavMap two_floors() +{ + NavMap nm; + for (const float z : {0.0f, 3.0f}) { + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(nm.positions.size()); + for (int j = 0; j <= 2; ++j) { + for (int i = 0; i <= 2; ++i) { + nm.add_vertex({static_cast(i), static_cast(j), z}); + } + } + auto v = [&](int i, int j) {return base + static_cast(j * 3 + i);}; + for (int j = 0; j < 2; ++j) { + for (int i = 0; i < 2; ++i) { + 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))); + } + } + } + nm.rebuild_geometry_accels(); + return nm; +} + +bool has(const std::vector & v, uint32_t x) +{ + return std::find(v.begin(), v.end(), x) != v.end(); +} + +} // namespace + +TEST(Links, AddQueryAndDefaultLength) +{ + NavMap nm = two_floors(); + // An elevator from the ground floor (cell 0) to the upper one (cell 8), both ways + const auto e = nm.add_link(0, 8, LinkType::ELEVATOR); + ASSERT_NE(e, NavMap::kInvalidLink); + EXPECT_NEAR(nm.links()[e].length, (nm.navcel_centroid(8) - nm.navcel_centroid(0)).norm(), 1e-6f); + EXPECT_TRUE(nm.links()[e].bidirectional); + EXPECT_TRUE(has(nm.links_from(0), e)); + EXPECT_TRUE(has(nm.links_from(8), e)); + // One-way stairs with a given length + const auto st = nm.add_link(3, 12, LinkType::STAIRS, 4.5f, false); + EXPECT_FLOAT_EQ(nm.links()[st].length, 4.5f); + EXPECT_TRUE(has(nm.links_from(3), st)); + EXPECT_FALSE(has(nm.links_from(12), st)); + EXPECT_TRUE(nm.links_from(5).empty()); + EXPECT_TRUE(nm.links_from(99999).empty()); // out of range: no links + // Links do not change the mesh adjacency + for (NavCelId n : nm.navcel_neighbors(0)) { + EXPECT_LT(n, 8u); + } +} + +TEST(Links, InvalidLinksAreRejected) +{ + NavMap nm = two_floors(); + EXPECT_EQ(nm.add_link(0, 99, LinkType::DOOR), NavMap::kInvalidLink); + EXPECT_EQ(nm.add_link(99, 0, LinkType::DOOR), NavMap::kInvalidLink); + EXPECT_EQ(nm.add_link(4, 4, LinkType::DOOR), NavMap::kInvalidLink); // to itself + EXPECT_TRUE(nm.links().empty()); + EXPECT_FALSE(nm.remove_link(0)); +} + +TEST(Links, RemoveAndClearKeepTheIndexConsistent) +{ + NavMap nm = two_floors(); + const auto a = nm.add_link(0, 8); + const auto b = nm.add_link(1, 9); + const auto c = nm.add_link(2, 10); + ASSERT_TRUE(has(nm.links_from(1), b)); // builds the index + EXPECT_TRUE(nm.remove_link(a)); // c takes index a + ASSERT_EQ(nm.links().size(), 2u); + EXPECT_TRUE(nm.links_from(0).empty()); + EXPECT_TRUE(has(nm.links_from(2), a)); + EXPECT_EQ(nm.links()[a].from, 2u); + (void)c; + nm.clear_links(); + EXPECT_TRUE(nm.links().empty()); + EXPECT_TRUE(nm.links_from(1).empty()); +} + +TEST(Links, CopiesAndAssignmentsCarryTheLinks) +{ + NavMap src = two_floors(); + src.add_link(0, 8, LinkType::RAMP, 7.0f); + NavMap copy(src); + NavMap assigned; + assigned = src; + NavMap synced = two_floors(); // same geometry: the layer-sync path of operator= + synced = src; + for (const NavMap * m : {©, &assigned, &synced}) { + ASSERT_EQ(m->links().size(), 1u); + EXPECT_EQ(m->links()[0].type, LinkType::RAMP); + EXPECT_FLOAT_EQ(m->links()[0].length, 7.0f); + EXPECT_FALSE(m->links_from(8).empty()); + } + // Independent afterwards + copy.clear_links(); + EXPECT_EQ(src.links().size(), 1u); + NavMap moved(std::move(assigned)); + EXPECT_EQ(moved.links().size(), 1u); +} diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index f79367d..cd39adc 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -34,6 +34,7 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "navmap_ros_interfaces/msg/nav_map_link.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "pcl_conversions/pcl_conversions.h" @@ -189,6 +190,18 @@ NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) out.layers.push_back(std::move(lmsg)); } + // links between non-adjacent NavCels + out.links.reserve(nm.links().size()); + for (const auto & l : nm.links()) { + navmap_ros_interfaces::msg::NavMapLink lm; + lm.from_navcel = l.from; + lm.to_navcel = l.to; + lm.type = static_cast(l.type); + lm.length = l.length; + lm.bidirectional = l.bidirectional; + out.links.push_back(lm); + } + return out; } @@ -249,6 +262,13 @@ navmap::NavMap from_msg(const NavMap & msg, std_msgs::msg::Header & header) apply_layer_msg(l, nm); } + // links (out-of-range ones are dropped) + for (const auto & l : msg.links) { + nm.add_link( + l.from_navcel, l.to_navcel, static_cast(l.type), l.length, + l.bidirectional); + } + nm.rebuild_geometry_accels(); return nm; } diff --git a/navmap_ros/src/navmap_ros/navmap_io.cpp b/navmap_ros/src/navmap_ros/navmap_io.cpp index dd8f86a..a345cf7 100644 --- a/navmap_ros/src/navmap_ros/navmap_io.cpp +++ b/navmap_ros/src/navmap_ros/navmap_io.cpp @@ -38,7 +38,8 @@ namespace // Magic header "NAVMAP\0" (7 chars + NUL), version uint32_t, payload_size uint64_t (LE). // Version 1 (also files without header) is migrated on load. static constexpr char kMagic[8] = {'N', 'A', 'V', 'M', 'A', 'P', '\0', '\0'}; -static constexpr uint32_t kVersion = 2; // 2: layer metadata (unknown, description, unit) and int16 layers +// 2: layer metadata (unknown, description, unit), int16 layers and links +static constexpr uint32_t kVersion = 2; inline void set_error(std::error_code * ec, int ev = EIO) { diff --git a/navmap_ros/tests/test_layer_metadata_and_migration.cpp b/navmap_ros/tests/test_layer_metadata_and_migration.cpp index 375210f..5bcec71 100644 --- a/navmap_ros/tests/test_layer_metadata_and_migration.cpp +++ b/navmap_ros/tests/test_layer_metadata_and_migration.cpp @@ -284,3 +284,36 @@ TEST(LayerMetadata, OccupancyGridUnknownCellsAreDeclaredUnknown) // ...and the declaration travels with the map EXPECT_TRUE(navmap_ros::from_msg(navmap_ros::to_msg(nm)).is_unknown("occupancy", 0)); } + +TEST(Links, TravelInMessagesAndFilesAndInvalidOnesAreDropped) +{ + NavMap nm = small_map(); + nm.add_link(0, 7, navmap::LinkType::ELEVATOR, 12.0f, false); + nm.add_link(2, 5, navmap::LinkType::STAIRS); + auto msg = navmap_ros::to_msg(nm); + ASSERT_EQ(msg.links.size(), 2u); + EXPECT_EQ(msg.links[0].type, navmap_ros_interfaces::msg::NavMapLink::ELEVATOR); + EXPECT_FALSE(msg.links[0].bidirectional); + navmap_ros_interfaces::msg::NavMapLink bad; + bad.from_navcel = 0; + bad.to_navcel = 999; // out of range + msg.links.push_back(bad); + const NavMap back = navmap_ros::from_msg(msg); + ASSERT_EQ(back.links().size(), 2u); + EXPECT_FLOAT_EQ(back.links()[0].length, 12.0f); + EXPECT_EQ(back.links()[1].type, navmap::LinkType::STAIRS); + // And in a file + const std::string dir = tmp_dir("navmap_links_"); + ASSERT_TRUE(navmap_ros::io::save_to_file(nm, dir + "/m.navmap")); + NavMap loaded; + ASSERT_TRUE(navmap_ros::io::load_from_file(dir + "/m.navmap", loaded)); + ASSERT_EQ(loaded.links().size(), 2u); + EXPECT_EQ(loaded.links()[0].to, 7u); + fs::remove_all(dir); + // Version-1 files have no links + write_v1(nm, dir + "_v1.navmap", true); + NavMap old; + ASSERT_TRUE(navmap_ros::io::load_from_file(dir + "_v1.navmap", old)); + EXPECT_TRUE(old.links().empty()); + fs::remove(dir + "_v1.navmap"); +} diff --git a/navmap_ros_interfaces/CMakeLists.txt b/navmap_ros_interfaces/CMakeLists.txt index 4e811c1..c2f70a4 100644 --- a/navmap_ros_interfaces/CMakeLists.txt +++ b/navmap_ros_interfaces/CMakeLists.txt @@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/NavMap.msg" "msg/NavMapSurface.msg" "msg/NavMapLayer.msg" + "msg/NavMapLink.msg" "msg/NavMapV1.msg" "msg/NavMapLayerV1.msg" DEPENDENCIES builtin_interfaces std_msgs diff --git a/navmap_ros_interfaces/msg/NavMap.msg b/navmap_ros_interfaces/msg/NavMap.msg index 8f147b3..9be36df 100644 --- a/navmap_ros_interfaces/msg/NavMap.msg +++ b/navmap_ros_interfaces/msg/NavMap.msg @@ -23,3 +23,6 @@ navmap_ros_interfaces/NavMapSurface[] surfaces # Per-NavCel layers navmap_ros_interfaces/NavMapLayer[] layers + +# Links between NavCels that are not adjacent (file format version 2) +navmap_ros_interfaces/NavMapLink[] links diff --git a/navmap_ros_interfaces/msg/NavMapLink.msg b/navmap_ros_interfaces/msg/NavMapLink.msg new file mode 100644 index 0000000..736d945 --- /dev/null +++ b/navmap_ros_interfaces/msg/NavMapLink.msg @@ -0,0 +1,14 @@ +# Link between two NavCels that do not share an edge (stairs, elevator, door...). +# The map only records it: whether a robot can take it is up to the planner. + +uint8 GENERIC = 0 +uint8 RAMP = 1 +uint8 STAIRS = 2 +uint8 ELEVATOR = 3 +uint8 DOOR = 4 + +uint32 from_navcel +uint32 to_navcel +uint8 type +float32 length # travel length (m) +bool bidirectional # also from to_navcel to from_navcel diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp index 9097a01..191dd60 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp @@ -99,6 +99,7 @@ private Q_SLOTS: void updateLayerUpdateTopic(); void onLayerSelectionChanged(); void onDrawNormalsChanged(); + void onDrawLinksChanged(); void onAlphaChanged(); void onNormalScaleChanged(); void onColorSchemeChanged(); @@ -118,6 +119,7 @@ private Q_SLOTS: // Rendering void updateGeometry_(); // Build geometry (first time) and update colors. void updateNormals_(); // Rebuild normals debug object. + void updateLinks_(); // Rebuild the links between non-adjacent NavCels. void updateColorSchemeOptions_(); // Refresh color-scheme choices for current layer. // Mesh/Entity path (Option B) @@ -132,6 +134,7 @@ private Q_SLOTS: rviz_common::properties::QosProfileProperty * layer_profile_property_{nullptr}; rviz_common::properties::EnumProperty * color_scheme_property_{nullptr}; rviz_common::properties::BoolProperty * draw_normals_property_{nullptr}; + rviz_common::properties::BoolProperty * draw_links_property_{nullptr}; rviz_common::properties::FloatProperty * normal_scale_property_{nullptr}; rviz_common::properties::FloatProperty * alpha_property_{nullptr}; rviz_common::properties::StringProperty * info_property_{nullptr}; @@ -154,6 +157,7 @@ private Q_SLOTS: // ---- OGRE scene objects ---- Ogre::SceneNode * root_node_{nullptr}; Ogre::ManualObject * normals_obj_{nullptr}; + Ogre::ManualObject * links_obj_{nullptr}; // Option B: static mesh + entity + dynamic colour buffer Ogre::Entity * entity_{nullptr}; diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index e530f8f..50ffd5e 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -124,6 +124,10 @@ NavMapDisplay::NavMapDisplay() draw_normals_property_ = new rviz_common::properties::BoolProperty( "Draw Normals", false, "Draw one normal per triangle.", this, SLOT(onDrawNormalsChanged())); + draw_links_property_ = new rviz_common::properties::BoolProperty( + "Draw Links", true, + "Draw the links between non-adjacent NavCels (stairs, elevators, doors...), coloured by type.", + this, SLOT(onDrawLinksChanged())); normal_scale_property_ = new rviz_common::properties::FloatProperty( "Normal Scale", 0.15f, "Scale of the per-triangle normal arrow/line.", @@ -142,6 +146,11 @@ NavMapDisplay::NavMapDisplay() NavMapDisplay::~NavMapDisplay() { + if (links_obj_) { + links_obj_->detachFromParent(); + context_->getSceneManager()->destroyManualObject(links_obj_); + links_obj_ = nullptr; + } if (normals_obj_) { normals_obj_->detachFromParent(); context_->getSceneManager()->destroyManualObject(normals_obj_); @@ -170,6 +179,11 @@ void NavMapDisplay::onInitialize() } // Prepare normals object + if (!links_obj_) { + links_obj_ = context_->getSceneManager()->createManualObject(); + links_obj_->setDynamic(true); + root_node_->attachObject(links_obj_); + } if (!normals_obj_) { normals_obj_ = context_->getSceneManager()->createManualObject(); normals_obj_->setDynamic(true); @@ -196,6 +210,7 @@ void NavMapDisplay::reset() destroyMesh_(); updateNormals_(); + updateLinks_(); } void NavMapDisplay::onEnable() @@ -259,6 +274,7 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg) ensureMeshBuilt_(); updateColorsOnly_(); updateNormals_(); + updateLinks_(); context_->queueRender(); } @@ -824,6 +840,57 @@ void NavMapDisplay::updateNormals_() normals_obj_->end(); } +void NavMapDisplay::onDrawLinksChanged() +{ + updateLinks_(); + context_->queueRender(); +} + +// Links between non-adjacent NavCels: a line between their centroids, raised in the middle so a +// link between stacked NavCels stays visible, coloured by type. +void NavMapDisplay::updateLinks_() +{ + if (!context_ || !last_msg_ || !links_obj_) {return;} + links_obj_->clear(); + if (!draw_links_property_->getBool() || last_msg_->links.empty()) {return;} + + const auto & X = last_msg_->positions_x; + const auto & Y = last_msg_->positions_y; + const auto & Z = last_msg_->positions_z; + const auto & V0 = last_msg_->navcels_v0; + const auto & V1 = last_msg_->navcels_v1; + const auto & V2 = last_msg_->navcels_v2; + auto centroid = [&](uint32_t t) { + return Ogre::Vector3( + (X[V0[t]] + X[V1[t]] + X[V2[t]]) / 3.0f, (Y[V0[t]] + Y[V1[t]] + Y[V2[t]]) / 3.0f, + (Z[V0[t]] + Z[V1[t]] + Z[V2[t]]) / 3.0f); + }; + auto colour = [](uint8_t type) { + switch (type) { + case navmap_ros_interfaces::msg::NavMapLink::RAMP: return Ogre::ColourValue(0, 0.8f, 0, 1); + case navmap_ros_interfaces::msg::NavMapLink::STAIRS: return Ogre::ColourValue(1, 0.5f, 0, + 1); + case navmap_ros_interfaces::msg::NavMapLink::ELEVATOR: + return Ogre::ColourValue(0.6f, 0, 1, 1); + case navmap_ros_interfaces::msg::NavMapLink::DOOR: return Ogre::ColourValue(0, 0.6f, 1, 1); + default: return Ogre::ColourValue(1, 1, 0, 1); + } + }; + + links_obj_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + for (const auto & l : last_msg_->links) { + if (l.from_navcel >= V0.size() || l.to_navcel >= V0.size()) {continue;} + const Ogre::Vector3 a = centroid(l.from_navcel), b = centroid(l.to_navcel); + const Ogre::Vector3 mid = 0.5f * (a + b) + Ogre::Vector3(0, 0, 0.25f * (a - b).length()); + const Ogre::ColourValue c = colour(l.type); + links_obj_->position(a); links_obj_->colour(c); + links_obj_->position(mid); links_obj_->colour(c); + links_obj_->position(mid); links_obj_->colour(c); + links_obj_->position(b); links_obj_->colour(c); + } + links_obj_->end(); +} + void NavMapDisplay::updateColorSchemeOptions_() { const QString prev = color_scheme_property_->getString(); From 8d4b307889096610aa10ad33477e750051480efe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 11:10:47 +0200 Subject: [PATCH 07/11] Add geometry ids and sparse layer updates MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - NavMap::geometry_id(): the geometry fingerprint, now also recomputed after add_vertex(), add_navcel() and rebuild_geometry_accels() (it went stale after a first query). - NavMap and NavMapLayer messages carry geometry_id; NavMapLayer can carry indices for a sparse update (values for those NavCels only). - navmap_ros::to_layer_update_msg(): sparse from the layer's change log when few NavCels changed, the full layer otherwise. from_msg rejects layer messages for another geometry and malformed sparse updates; RViz2 ignores the former and patches its cached layer with the latter. Signed-off-by: Francisco Martín Rico --- README.md | 6 ++ navmap_core/include/navmap_core/NavMap.hpp | 13 +++ navmap_core/src/navmap_core/NavMap.cpp | 3 + navmap_core/tests/test_links.cpp | 22 +++++ navmap_ros/include/navmap_ros/conversions.hpp | 24 ++++- navmap_ros/src/navmap_ros/conversions.cpp | 93 ++++++++++++++++++- .../test_layer_metadata_and_migration.cpp | 67 +++++++++++++ navmap_ros_interfaces/msg/NavMap.msg | 3 + navmap_ros_interfaces/msg/NavMapLayer.msg | 9 +- .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 48 ++++++++++ 10 files changed, 282 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 26ada0b..87b6514 100644 --- a/README.md +++ b/README.md @@ -328,6 +328,12 @@ navmap::NavMap nm2 = navmap_ros::from_msg(msg); --- +### Layer updates + +`navmap_ros::to_layer_update_msg(nm, "obstacles")` publishes only what changed since the layer's last `clear_changes()`: a sparse `NavMapLayer` (`indices` + values) when few NavCels changed, the full layer otherwise. Every `NavMap` and `NavMapLayer` message carries the map's `geometry_id`, so `from_msg` (and RViz2) reject updates meant for another geometry. + +--- + ## 9. Save and load from disk NavMap supports saving and loading using YAML + mesh files: diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 95d0d32..bbf94b6 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -600,6 +600,19 @@ class NavMap /// \brief Fast check for identical geometry (vertices and NavCel indices). bool has_same_geometry(const NavMap & other) const; + /** + * \brief Fingerprint of the geometry (vertex positions and triangles), 64-bit FNV-1a. + * + * Equal maps have equal ids, so a layer update can name the geometry it belongs to. Recomputed + * lazily after add_vertex(), add_navcel() or rebuild_geometry_accels(); call the latter after + * editing positions or navcels directly. + */ + std::uint64_t geometry_id() const + { + ensure_geometry_fingerprint_(); + return geometry_fp_; + } + /** * \brief Build topological adjacency between neighboring NavCels. * diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 332b5a0..0286140 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -439,6 +439,7 @@ inline float box_sq_dist(const AABB & box, const Vec3 & p) void NavMap::rebuild_geometry_accels() { + geometry_dirty_ = true; // Normals and areas. for (auto & c : navcels) { Vec3 a = positions.at(c.v[0]); @@ -948,6 +949,7 @@ bool NavMap::remove_surface(std::size_t surface_index) uint32_t NavMap::add_vertex(const Eigen::Vector3f & p) { + geometry_dirty_ = true; positions.x.push_back(p.x()); positions.y.push_back(p.y()); positions.z.push_back(p.z()); @@ -956,6 +958,7 @@ uint32_t NavMap::add_vertex(const Eigen::Vector3f & p) NavCelId NavMap::add_navcel(uint32_t v0, uint32_t v1, uint32_t v2) { + geometry_dirty_ = true; NavCel t; t.v[0] = v0; t.v[1] = v1; t.v[2] = v2; navcels.push_back(t); return static_cast(navcels.size() - 1); diff --git a/navmap_core/tests/test_links.cpp b/navmap_core/tests/test_links.cpp index e9846c6..a793163 100644 --- a/navmap_core/tests/test_links.cpp +++ b/navmap_core/tests/test_links.cpp @@ -131,3 +131,25 @@ TEST(Links, CopiesAndAssignmentsCarryTheLinks) NavMap moved(std::move(assigned)); EXPECT_EQ(moved.links().size(), 1u); } + +TEST(GeometryId, EqualForEqualGeometryAndFollowsEdits) +{ + NavMap a = two_floors(), b = two_floors(); + EXPECT_EQ(a.geometry_id(), b.geometry_id()); + EXPECT_NE(a.geometry_id(), 0u); + NavMap copy(a); + EXPECT_EQ(copy.geometry_id(), a.geometry_id()); + // Layers and links are not geometry + a.add_layer("f", "", "", 1.0f); + a.add_link(0, 8); + EXPECT_EQ(a.geometry_id(), b.geometry_id()); + // Editing the geometry changes it (the fingerprint used to go stale after a first query) + const auto before = b.geometry_id(); + b.add_vertex({9.0f, 9.0f, 9.0f}); + EXPECT_NE(b.geometry_id(), before); + NavMap c = two_floors(); + const auto c0 = c.geometry_id(); + c.positions.z[0] += 0.1f; + c.rebuild_geometry_accels(); + EXPECT_NE(c.geometry_id(), c0); +} diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index c0b6519..1990f9c 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -150,6 +150,20 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, const std::string & layer); +/** + * @brief Layer update with only what changed since the layer's last clear_changes(). + * + * Sparse (indices + values) when few NavCels changed one by one; the full layer when the whole + * layer changed, nothing did, or more than @p max_sparse_fraction of it did. Carries the + * geometry id, so a receiver can reject an update for another map. Does not clear the changes. + * + * @throw std::runtime_error If the layer does not exist. + */ +navmap_ros_interfaces::msg::NavMapLayer to_layer_update_msg( + const navmap::NavMap & nm, const std::string & layer, + const std_msgs::msg::Header & header = std_msgs::msg::Header(), + float max_sparse_fraction = 0.25f); + /** * @brief Import a single NavMapLayer message into a NavMap. * @@ -161,11 +175,13 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * @param[out] header Header extracted from the message. * * @details - * - The function verifies that the length of the populated data array matches - * the number of triangles (NavCels) in @p nm. - * - Exactly one of the arrays `data_u8`, `data_f32`, or `data_f64` must be set. + * - Exactly one of the arrays `data_u8`, `data_f32`, `data_f64` or `data_i16` must be set. + * - With `indices`, only those NavCels are written (sparse update of an existing layer of the + * same type); without them, the whole layer is replaced, with its metadata. + * - A message whose `geometry_id` is set and differs from nm.geometry_id() is rejected. * - * @throw std::runtime_error If sizes are inconsistent or the message is ill-formed. + * @throw std::runtime_error If the message belongs to another geometry, a sparse update does + * not match its layer, or the type is unsupported. */ void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index cd39adc..dae0448 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -82,6 +82,7 @@ void fill_layer_msg( { lmsg.name = name; lmsg.type = static_cast(base.type()); + lmsg.geometry_id = nm.geometry_id(); switch (base.type()) { case navmap::LayerType::U8: lmsg.data_u8 = static_cast &>(base).data(); @@ -115,9 +116,48 @@ void set_layer(navmap::NavMap & nm, const NavMapLayer & msg, const V & data) dst->mark_all_changed(); } -// Message -> layer data and metadata (the layer is created or retyped as needed). +template +void patch_layer(navmap::NavMap & nm, const NavMapLayer & msg, const V & data) +{ + auto view = std::dynamic_pointer_cast>(nm.layers.get(msg.name)); + if (!view) { + throw std::runtime_error( + "from_msg(NavMapLayer): sparse update of '" + msg.name + + "', which is missing or of another type"); + } + if (data.size() != msg.indices.size()) { + throw std::runtime_error("from_msg(NavMapLayer): sparse update sizes differ"); + } + auto & d = view->data(); + for (std::size_t k = 0; k < msg.indices.size(); ++k) { + const auto c = msg.indices[k]; + if (c < d.size()) { + d[c] = data[k]; + view->mark_changed(c); + } + } +} + +// Message -> layer data and metadata (the layer is created or retyped as needed). A message +// naming another geometry, or a malformed sparse update, throws. void apply_layer_msg(const NavMapLayer & msg, navmap::NavMap & nm) { + if (msg.geometry_id != 0 && nm.geometry_id() != msg.geometry_id && !nm.navcels.empty()) { + throw std::runtime_error( + "from_msg(NavMapLayer): layer '" + msg.name + "' belongs to another geometry"); + } + if (!msg.indices.empty()) { + switch (msg.type) { + case NavMapLayer::U8: patch_layer(nm, msg, msg.data_u8); break; + case NavMapLayer::F32: patch_layer(nm, msg, msg.data_f32); break; + case NavMapLayer::F64: patch_layer(nm, msg, msg.data_f64); break; + case NavMapLayer::I16: patch_layer(nm, msg, msg.data_i16); break; + default: + throw std::runtime_error("from_msg(NavMapLayer): unsupported type value " + + std::to_string(msg.type)); + } + return; + } if (auto existing = nm.layers.get(msg.name); existing && static_cast(existing->type()) != msg.type) { @@ -190,6 +230,8 @@ NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) out.layers.push_back(std::move(lmsg)); } + out.geometry_id = nm.geometry_id(); + // links between non-adjacent NavCels out.links.reserve(nm.links().size()); for (const auto & l : nm.links()) { @@ -303,6 +345,55 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( return to_msg(nm, layer_name, std_msgs::msg::Header()); } +navmap_ros_interfaces::msg::NavMapLayer to_layer_update_msg( + const navmap::NavMap & nm, const std::string & layer_name, + const std_msgs::msg::Header & header, float max_sparse_fraction) +{ + auto base = nm.layers.get(layer_name); + if (!base) { + throw std::runtime_error("to_layer_update_msg: layer '" + layer_name + "' not found"); + } + const auto & changed = base->changed(); + if (base->all_changed() || changed.empty() || + static_cast(changed.size()) > max_sparse_fraction * static_cast(base->size())) + { + return to_msg(nm, layer_name, header); // full layer + } + NavMapLayer msg; + msg.header = header; + msg.name = layer_name; + msg.type = static_cast(base->type()); + msg.geometry_id = nm.geometry_id(); + if (const auto meta = nm.get_layer_meta(layer_name)) { + msg.description = meta->description; + msg.unit = meta->unit; + msg.has_unknown = meta->unknown.has_value(); + msg.unknown_value = meta->unknown.value_or(0.0); + } + msg.indices.assign(changed.begin(), changed.end()); + auto gather = [&](const auto & data, auto & out) { + out.reserve(changed.size()); + for (const auto c : changed) { + out.push_back(data[c]); + } + }; + switch (base->type()) { + case navmap::LayerType::U8: + gather(static_cast &>(*base).data(), msg.data_u8); + break; + case navmap::LayerType::F32: + gather(static_cast &>(*base).data(), msg.data_f32); + break; + case navmap::LayerType::F64: + gather(static_cast &>(*base).data(), msg.data_f64); + break; + case navmap::LayerType::I16: + gather(static_cast &>(*base).data(), msg.data_i16); + break; + } + return msg; +} + void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, navmap::NavMap & nm, diff --git a/navmap_ros/tests/test_layer_metadata_and_migration.cpp b/navmap_ros/tests/test_layer_metadata_and_migration.cpp index 5bcec71..51aa85a 100644 --- a/navmap_ros/tests/test_layer_metadata_and_migration.cpp +++ b/navmap_ros/tests/test_layer_metadata_and_migration.cpp @@ -35,6 +35,7 @@ #include "rclcpp/serialized_message.hpp" namespace fs = std::filesystem; +using navmap::NavCelId; using navmap::NavMap; namespace @@ -317,3 +318,69 @@ TEST(Links, TravelInMessagesAndFilesAndInvalidOnesAreDropped) EXPECT_TRUE(old.links().empty()); fs::remove(dir + "_v1.navmap"); } + +TEST(LayerUpdates, SparseWhenFewCellsChangedAndAppliedOnTheSameGeometry) +{ + NavMap src = small_map(); + auto v = src.add_layer("cost", "c", "", 0.0f); + NavMap dst = small_map(); + navmap_ros::from_msg(navmap_ros::to_msg(src, "cost"), dst); + // A new layer: full update + auto m = navmap_ros::to_layer_update_msg(src, "cost"); + EXPECT_TRUE(m.indices.empty()); + EXPECT_EQ(m.data_f32.size(), src.navcels.size()); + EXPECT_EQ(m.geometry_id, src.geometry_id()); + v->clear_changes(); + // Nothing changed: full (an empty sparse update would mean the whole layer) + EXPECT_TRUE(navmap_ros::to_layer_update_msg(src, "cost").indices.empty()); + // One cell changed: sparse + src.layer_set("cost", 5, 2.5f); + m = navmap_ros::to_layer_update_msg(src, "cost"); + ASSERT_EQ(m.indices, (std::vector{5})); + ASSERT_EQ(m.data_f32, (std::vector{2.5f})); + dst.layers.get("cost")->clear_changes(); // the full load above marked the whole layer + navmap_ros::from_msg(m, dst); + EXPECT_FLOAT_EQ(dst.layer_get("cost", 5), 2.5f); + EXPECT_FLOAT_EQ(dst.layer_get("cost", 4), 0.0f); + EXPECT_EQ(dst.layers.get("cost")->changed(), (std::vector{5})); + // More than the sparse fraction changed: full + for (NavCelId c = 0; c < 4; ++c) { + src.layer_set("cost", c, 1.0f); + } + EXPECT_TRUE(navmap_ros::to_layer_update_msg(src, "cost", {}, 0.25f).indices.empty()); + EXPECT_FALSE(navmap_ros::to_layer_update_msg(src, "cost", {}, 0.9f).indices.empty()); +} + +TEST(LayerUpdates, UpdatesForAnotherGeometryOrMalformedAreRejected) +{ + NavMap src = small_map(); + src.add_layer("occ", "", "", uint8_t{0}); + src.layers.get("occ")->clear_changes(); + src.layer_set("occ", 1, 254); + const auto sparse = navmap_ros::to_layer_update_msg(src, "occ"); + ASSERT_FALSE(sparse.indices.empty()); + // Another geometry + NavMap other = small_map(); + other.add_vertex({5.0f, 5.0f, 0.0f}); + other.add_layer("occ", "", "", uint8_t{0}); + EXPECT_THROW(navmap_ros::from_msg(sparse, other), std::runtime_error); + EXPECT_THROW(navmap_ros::from_msg(navmap_ros::to_msg(src, "occ"), other), std::runtime_error); + // geometry_id 0 (an older publisher): accepted + auto anon = navmap_ros::to_msg(src, "occ"); + anon.geometry_id = 0; + EXPECT_NO_THROW(navmap_ros::from_msg(anon, other)); + // Sparse update of a missing layer, or of another type, or with mismatched sizes + NavMap same = small_map(); + EXPECT_THROW(navmap_ros::from_msg(sparse, same), std::runtime_error); + same.add_layer("occ", "", "", 0.0f); + EXPECT_THROW(navmap_ros::from_msg(sparse, same), std::runtime_error); + NavMap good = small_map(); + good.add_layer("occ", "", "", uint8_t{0}); + auto bad = sparse; + bad.data_u8.push_back(7); + EXPECT_THROW(navmap_ros::from_msg(bad, good), std::runtime_error); + EXPECT_NO_THROW(navmap_ros::from_msg(sparse, good)); + EXPECT_EQ(good.layer_get("occ", 1), 254); + // The full map message carries the geometry id too + EXPECT_EQ(navmap_ros::to_msg(src).geometry_id, src.geometry_id()); +} diff --git a/navmap_ros_interfaces/msg/NavMap.msg b/navmap_ros_interfaces/msg/NavMap.msg index 9be36df..dcbd5df 100644 --- a/navmap_ros_interfaces/msg/NavMap.msg +++ b/navmap_ros_interfaces/msg/NavMap.msg @@ -24,5 +24,8 @@ navmap_ros_interfaces/NavMapSurface[] surfaces # Per-NavCel layers navmap_ros_interfaces/NavMapLayer[] layers +# Fingerprint of the geometry (NavMap::geometry_id(); 0 = unspecified), as in NavMapLayer +uint64 geometry_id + # Links between NavCels that are not adjacent (file format version 2) navmap_ros_interfaces/NavMapLink[] links diff --git a/navmap_ros_interfaces/msg/NavMapLayer.msg b/navmap_ros_interfaces/msg/NavMapLayer.msg index 421f45f..61546ad 100644 --- a/navmap_ros_interfaces/msg/NavMapLayer.msg +++ b/navmap_ros_interfaces/msg/NavMapLayer.msg @@ -15,8 +15,15 @@ string unit bool has_unknown # true: unknown_value means "not observed" (NaN allowed) float64 unknown_value +# Geometry this layer belongs to (NavMap::geometry_id(); 0 = unspecified) +uint64 geometry_id + +# Sparse update: if non-empty, the data_* array holds one value per listed NavCel and the +# rest of the layer is unchanged. If empty, the data_* array covers every NavCel. +uint32[] indices + # Exactly one of the data_* arrays must be non-empty, and its length must be -# equal to the number of triangles (NavCels) in the NavMap message. +# equal to the number of triangles (NavCels) in the NavMap message (or to indices). uint8[] data_u8 float32[] data_f32 float64[] data_f64 diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index 50ffd5e..9f78c3e 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -346,6 +346,54 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) return; } + if (msg->geometry_id != 0 && last_msg_->geometry_id != 0 && + msg->geometry_id != last_msg_->geometry_id) + { + setStatus(rviz_common::properties::StatusProperty::Warn, "Layer Update", + QString("Ignored \"%1\": it belongs to another geometry").arg( + QString::fromStdString(msg->name))); + return; + } + + // Sparse update: patch the cached layer of the same name and type + if (!msg->indices.empty()) { + NavMapLayerMsg * cached = nullptr; + for (auto & L : last_msg_->layers) { + if (L.name == msg->name && L.type == msg->type) {cached = &L;} + } + auto patch = [&](auto & dst, const auto & src) { + if (src.size() != msg->indices.size()) {return false;} + for (size_t k = 0; k < msg->indices.size(); ++k) { + if (msg->indices[k] < dst.size()) {dst[msg->indices[k]] = src[k];} + } + return true; + }; + bool ok = cached != nullptr; + if (ok) { + switch (msg->type) { + case NavMapLayerMsg::U8: ok = patch(cached->data_u8, msg->data_u8); break; + case NavMapLayerMsg::F32: ok = patch(cached->data_f32, msg->data_f32); break; + case NavMapLayerMsg::F64: ok = patch(cached->data_f64, msg->data_f64); break; + case NavMapLayerMsg::I16: ok = patch(cached->data_i16, msg->data_i16); break; + default: ok = false; + } + } + if (!ok) { + setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", + QString("Sparse update of \"%1\" does not match a received layer").arg( + QString::fromStdString(msg->name))); + return; + } + ++layer_update_count_; + if (currentSelectedLayer_() == msg->name) {updateColorsOnly_();} + setStatus(rviz_common::properties::StatusProperty::Ok, "Layer Update", + QString("OK — updates: %1 | last: \"%2\" (sparse, %3 cells)") + .arg(qulonglong(layer_update_count_)).arg(QString::fromStdString(msg->name)) + .arg(qulonglong(msg->indices.size()))); + context_->queueRender(); + return; + } + const size_t n_tris = last_msg_->navcels_v0.size(); const size_t n_u8 = msg->data_u8.size(); const size_t n_f32 = msg->data_f32.size(); From b08501a501c46378e2ca826e8d13f86f442e5263 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 16:07:58 +0200 Subject: [PATCH 08/11] Compress .navmap files with zlib MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The version 2 envelope records the codec and both sizes. Files are zlib-compressed by default (SaveOptions::compression, level 1-9); playground maps shrink to 20-41 %. zlib is available on every ROS distro and platform, and in the Python standard library, which resample.py uses to read and write the same format. Corrupt or truncated files are rejected. Signed-off-by: Francisco Martín Rico --- navmap_ros/CMakeLists.txt | 2 + navmap_ros/include/navmap_ros/navmap_io.hpp | 33 ++-- navmap_ros/package.xml | 1 + navmap_ros/src/navmap_ros/navmap_io.cpp | 91 +++++++--- .../test_layer_metadata_and_migration.cpp | 155 +++++++++++++++++- navmap_tools/navmap_tools/resample.py | 30 +++- navmap_tools/test/test_resample.py | 34 ++++ 7 files changed, 295 insertions(+), 51 deletions(-) diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index a716163..e43188f 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(PCL REQUIRED COMPONENTS common io kdtree search) find_package(pcl_conversions REQUIRED) +find_package(ZLIB REQUIRED) add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/conversions.cpp @@ -39,6 +40,7 @@ target_link_libraries(${PROJECT_NAME} PRIVATE ${pcl_conversions_TARGETS} ${pcl_conversions_LIBRARIES} ${PCL_LIBRARIES} + ZLIB::ZLIB ) target_link_libraries(${PROJECT_NAME} PUBLIC rclcpp::rclcpp diff --git a/navmap_ros/include/navmap_ros/navmap_io.hpp b/navmap_ros/include/navmap_ros/navmap_io.hpp index 1500b28..6dabaf4 100644 --- a/navmap_ros/include/navmap_ros/navmap_io.hpp +++ b/navmap_ros/include/navmap_ros/navmap_io.hpp @@ -26,15 +26,15 @@ * 2) **Core-level (optional)**: save/load `navmap::NavMap` if the core headers * are available, using `navmap_ros::to_msg` / `navmap_ros::from_msg`. * - * @par File format - * A small binary envelope followed by the ROS 2-serialized payload: - * - Magic header - * - Version (uint32) - * - Payload size (uint64, little-endian) - * - Serialized message payload (CDR) + * @par File format (version 2) + * A small binary envelope followed by the ROS 2-serialized payload (little endian): + * - Magic "NAVMAP\0\0" + * - Version (uint32), codec (uint32: 0 none, 1 zlib) + * - Raw (CDR) size and stored size (uint64 each) + * - Payload: the CDR message, compressed by the codec * - * If the magic header is not present, the loader falls back to interpreting the whole file - * as a raw CDR payload. + * Version 1 files (magic, version, size, CDR; or a bare CDR payload) are migrated on load, with a + * deprecation warning, and rewritten in the current format. */ #include @@ -51,15 +51,16 @@ namespace navmap_ros::io { -/** - * @brief Save options. - * - * @note Currently a placeholder for future extensions (e.g., compression). - */ +/// Payload compression. zlib is available on every ROS distro and platform, and in Python. +enum class Compression { NONE, ZLIB }; + +/// Save options. struct SaveOptions { - /// If @c true, save in binary (CDR). Reserved for future options. + /// Unused (always binary CDR); kept for source compatibility. bool binary{true}; + Compression compression{Compression::ZLIB}; + int level{6}; ///< zlib level, 1 (fast) to 9 (small) }; /** @@ -69,7 +70,7 @@ struct SaveOptions * * @param[in] msg Message to save. * @param[in] path Destination file path. - * @param[in] options Save options (currently unused, reserved). + * @param[in] options Save options (compression). * @param[out] ec Optional error code; if not null, it will be set on failure. * @return @c true on success, @c false otherwise (and @p ec is set if provided). * @@ -104,7 +105,7 @@ bool load_msg_from_file( * * @param[in] map Core map to save. * @param[in] path Destination file path. - * @param[in] options Save options (currently unused, reserved). + * @param[in] options Save options (compression). * @param[out] ec Optional error code; if not null, it will be set on failure. * @return @c true on success, @c false otherwise. * diff --git a/navmap_ros/package.xml b/navmap_ros/package.xml index c969fd4..af81323 100644 --- a/navmap_ros/package.xml +++ b/navmap_ros/package.xml @@ -20,6 +20,7 @@ sensor_msgs std_msgs std_srvs + zlib ament_cmake_gtest ament_lint_auto diff --git a/navmap_ros/src/navmap_ros/navmap_io.cpp b/navmap_ros/src/navmap_ros/navmap_io.cpp index a345cf7..b1fc74c 100644 --- a/navmap_ros/src/navmap_ros/navmap_io.cpp +++ b/navmap_ros/src/navmap_ros/navmap_io.cpp @@ -16,6 +16,7 @@ #include +#include #include #include #include @@ -29,17 +30,21 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/logging.hpp" #include "navmap_ros_interfaces/msg/nav_map_v1.hpp" +#include "zlib.h" namespace navmap_ros::io { namespace { -// Magic header "NAVMAP\0" (7 chars + NUL), version uint32_t, payload_size uint64_t (LE). -// Version 1 (also files without header) is migrated on load. +// Magic "NAVMAP\0\0", then (little endian): +// v1: version u32, payload_size u64, CDR payload (also files without header). Migrated on load. +// v2: version u32, codec u32, raw_size u64, stored_size u64, payload (CDR, compressed by codec). +// Adds layer metadata (unknown, description, unit), int16 layers, links and geometry ids. static constexpr char kMagic[8] = {'N', 'A', 'V', 'M', 'A', 'P', '\0', '\0'}; -// 2: layer metadata (unknown, description, unit), int16 layers and links static constexpr uint32_t kVersion = 2; +static constexpr uint32_t kCodecNone = 0; +static constexpr uint32_t kCodecZlib = 1; inline void set_error(std::error_code * ec, int ev = EIO) { @@ -51,32 +56,43 @@ inline void set_error(std::error_code * ec, int ev = EIO) bool save_msg_to_file( const navmap_ros_interfaces::msg::NavMap & msg, const std::string & path, - const SaveOptions & /*options*/, + const SaveOptions & options, std::error_code * ec) { using MsgT = navmap_ros_interfaces::msg::NavMap; try { - // Serialize to ROS 2 CDR rclcpp::SerializedMessage serialized; rclcpp::Serialization ser; ser.serialize_message(&msg, &serialized); - const auto & rcl_ser = serialized.get_rcl_serialized_message(); - const auto payload = static_cast(rcl_ser.buffer); - const size_t payload_size = rcl_ser.buffer_length; + const auto * raw = static_cast(rcl_ser.buffer); + const uint64_t raw_size = rcl_ser.buffer_length; + + uint32_t codec = kCodecNone; + const uint8_t * payload = raw; + uint64_t stored_size = raw_size; + std::vector packed; + if (options.compression == Compression::ZLIB) { + uLongf n = compressBound(static_cast(raw_size)); + packed.resize(n); + const int level = std::clamp(options.level, 1, 9); + if (compress2(packed.data(), &n, raw, static_cast(raw_size), level) != Z_OK) { + set_error(ec); return false; + } + codec = kCodecZlib; + payload = packed.data(); + stored_size = n; + } std::ofstream ofs(path, std::ios::binary); if (!ofs) {set_error(ec, EACCES); return false;} - - // Write header - ofs.write(reinterpret_cast(kMagic), sizeof(kMagic)); - uint32_t ver = kVersion; + ofs.write(kMagic, sizeof(kMagic)); + const uint32_t ver = kVersion; ofs.write(reinterpret_cast(&ver), sizeof(ver)); - uint64_t size64 = static_cast(payload_size); - ofs.write(reinterpret_cast(&size64), sizeof(size64)); - - // Write payload - ofs.write(reinterpret_cast(payload), payload_size); + ofs.write(reinterpret_cast(&codec), sizeof(codec)); + ofs.write(reinterpret_cast(&raw_size), sizeof(raw_size)); + ofs.write(reinterpret_cast(&stored_size), sizeof(stored_size)); + ofs.write(reinterpret_cast(payload), static_cast(stored_size)); ofs.flush(); if (!ofs) {set_error(ec); return false;} if (ec) {*ec = {};} @@ -149,28 +165,49 @@ bool load_msg_from_file( // Files without header are the oldest format: version 1 uint32_t version = 1; - uint64_t payload_size = 0; - bool has_header = std::memcmp(magic, kMagic, sizeof(kMagic)) == 0; - if (has_header) { + uint32_t codec = kCodecNone; + uint64_t raw_size = 0, stored_size = 0; + if (std::memcmp(magic, kMagic, sizeof(kMagic)) == 0) { ifs.read(reinterpret_cast(&version), sizeof(version)); - ifs.read(reinterpret_cast(&payload_size), sizeof(payload_size)); if (!ifs || version < 1 || version > kVersion) {set_error(ec); return false;} + if (version >= 2) { + ifs.read(reinterpret_cast(&codec), sizeof(codec)); + ifs.read(reinterpret_cast(&raw_size), sizeof(raw_size)); + } + ifs.read(reinterpret_cast(&stored_size), sizeof(stored_size)); + if (!ifs || (codec != kCodecNone && codec != kCodecZlib)) {set_error(ec); return false;} + if (version == 1) {raw_size = stored_size;} } else { - // No header: treat whole file as payload ifs.clear(); ifs.seekg(0, std::ios::end); auto end = ifs.tellg(); if (end <= 0) {set_error(ec); return false;} - payload_size = static_cast(end); + stored_size = raw_size = static_cast(end); ifs.seekg(0, std::ios::beg); } + // A truncated or corrupt header must not allocate blindly + const auto here = ifs.tellg(); + ifs.seekg(0, std::ios::end); + const auto left = static_cast(ifs.tellg() - here); + ifs.seekg(here); + if (stored_size > left || (codec == kCodecNone && raw_size != stored_size)) { + set_error(ec); return false; + } - // Read payload - std::vector buffer; - buffer.resize(static_cast(payload_size)); - ifs.read(reinterpret_cast(buffer.data()), static_cast(payload_size)); + std::vector buffer(static_cast(stored_size)); + ifs.read(reinterpret_cast(buffer.data()), static_cast(stored_size)); if (!ifs) {set_error(ec); return false;} ifs.close(); + if (codec == kCodecZlib) { + std::vector raw(static_cast(raw_size)); + uLongf n = static_cast(raw_size); + if (uncompress(raw.data(), &n, buffer.data(), static_cast(stored_size)) != Z_OK || + n != raw_size) + { + set_error(ec); return false; + } + buffer.swap(raw); + } if (version == kVersion) { deserialize(buffer, out_msg); diff --git a/navmap_ros/tests/test_layer_metadata_and_migration.cpp b/navmap_ros/tests/test_layer_metadata_and_migration.cpp index 51aa85a..12d3b09 100644 --- a/navmap_ros/tests/test_layer_metadata_and_migration.cpp +++ b/navmap_ros/tests/test_layer_metadata_and_migration.cpp @@ -267,6 +267,157 @@ TEST(FormatMigration, NewerOrBrokenFilesAreRejected) fs::remove_all(dir); } +// ------------------------------------------------------------------ compression + +// A map large enough for compression to matter: a 60 x 60 grid with a few layers +NavMap grid_map() +{ + nav_msgs::msg::OccupancyGrid g; + g.header.frame_id = "map"; + g.info.resolution = 0.1f; + g.info.width = 60; + g.info.height = 60; + g.data.assign(60 * 60, 0); + for (int i = 0; i < 60; ++i) {g.data[30 * 60 + i] = 100; g.data[i] = -1;} + NavMap nm = navmap_ros::from_occupancy_grid(g); + nm.add_layer("elevation", "", "m", 0.25f); + return nm; +} + +struct Envelope +{ + uint32_t version{0}, codec{0}; + uint64_t raw{0}, stored{0}, file{0}; +}; + +Envelope envelope(const std::string & path) +{ + Envelope e; + std::ifstream f(path, std::ios::binary); + f.seekg(8); + f.read(reinterpret_cast(&e.version), 4); + f.read(reinterpret_cast(&e.codec), 4); + f.read(reinterpret_cast(&e.raw), 8); + f.read(reinterpret_cast(&e.stored), 8); + e.file = fs::file_size(path); + return e; +} + +void patch(const std::string & path, std::streamoff at, const void * bytes, std::size_t n) +{ + std::fstream f(path, std::ios::binary | std::ios::in | std::ios::out); + f.seekp(at); + f.write(static_cast(bytes), static_cast(n)); +} + +void expect_same(const NavMap & a, const NavMap & b) +{ + EXPECT_EQ(a.positions.x, b.positions.x); + EXPECT_EQ(a.positions.z, b.positions.z); + ASSERT_EQ(a.navcels.size(), b.navcels.size()); + for (navmap::NavCelId c = 0; c < a.navcels.size(); ++c) { + ASSERT_EQ(a.layer_get("occupancy", c), b.layer_get("occupancy", c)); + ASSERT_EQ(a.layer_get("elevation", c), b.layer_get("elevation", c)); + } + EXPECT_TRUE(b.get_layer_meta("occupancy")->unknown.has_value()); + EXPECT_EQ(a.geometry_id(), b.geometry_id()); +} + +TEST(Compression, ZlibByDefaultSmallerAndIdenticalOnLoad) +{ + const std::string dir = tmp_dir("navmap_z_"); + const NavMap nm = grid_map(); + navmap_ros::io::SaveOptions raw_opt; + raw_opt.compression = navmap_ros::io::Compression::NONE; + ASSERT_TRUE(navmap_ros::io::save_to_file(nm, dir + "/z.navmap")); + ASSERT_TRUE(navmap_ros::io::save_to_file(nm, dir + "/raw.navmap", raw_opt)); + const Envelope z = envelope(dir + "/z.navmap"), r = envelope(dir + "/raw.navmap"); + EXPECT_EQ(z.version, 2u); + EXPECT_EQ(z.codec, 1u); + EXPECT_EQ(r.codec, 0u); + EXPECT_EQ(z.raw, r.raw); // same CDR payload + EXPECT_EQ(r.stored, r.raw); + EXPECT_EQ(z.file, 32 + z.stored); // header + payload, nothing else + EXPECT_LT(z.stored, r.raw / 3); // a mostly uniform grid compresses well + for (const auto * f : {"/z.navmap", "/raw.navmap"}) { + NavMap back; + ASSERT_TRUE(navmap_ros::io::load_from_file(dir + f, back)) << f; + expect_same(nm, back); + } + fs::remove_all(dir); +} + +TEST(Compression, EveryLevelLoadsAndOutOfRangeLevelsAreClamped) +{ + const std::string dir = tmp_dir("navmap_zl_"); + const NavMap nm = grid_map(); + // The zlib stream header records the level class: 0x01 fastest, 0x9C default, 0xDA best + auto flevel = [&](int level) { + std::ifstream f(dir + "/l" + std::to_string(level) + ".navmap", std::ios::binary); + f.seekg(33); + return static_cast(static_cast(f.get())); + }; + for (const int level : {-5, 0, 1, 6, 9, 42}) { + navmap_ros::io::SaveOptions o; + o.level = level; + const std::string path = dir + "/l" + std::to_string(level) + ".navmap"; + ASSERT_TRUE(navmap_ros::io::save_to_file(nm, path, o)) << level; + NavMap back; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, back)) << level; + expect_same(nm, back); + } + // The level is applied, and clamped to [1, 9] + EXPECT_EQ(flevel(1), 0x01u); + EXPECT_EQ(flevel(6), 0x9Cu); + EXPECT_EQ(flevel(9), 0xDAu); + EXPECT_EQ(flevel(-5), 0x01u); + EXPECT_EQ(flevel(0), 0x01u); + EXPECT_EQ(flevel(42), 0xDAu); + fs::remove_all(dir); +} + +TEST(Compression, EmptyMapRoundTrips) +{ + const std::string dir = tmp_dir("navmap_ze_"); + ASSERT_TRUE(navmap_ros::io::save_to_file(NavMap{}, dir + "/e.navmap")); + NavMap back; + ASSERT_TRUE(navmap_ros::io::load_from_file(dir + "/e.navmap", back)); + EXPECT_TRUE(back.navcels.empty()); + fs::remove_all(dir); +} + +TEST(Compression, CorruptOrTruncatedFilesAreRejectedWithoutCrashing) +{ + const std::string dir = tmp_dir("navmap_zc_"); + const std::string good = dir + "/good.navmap"; + ASSERT_TRUE(navmap_ros::io::save_to_file(grid_map(), good)); + const Envelope e = envelope(good); + auto fails = [&](const std::string & what, auto && damage) { + const std::string path = dir + "/bad.navmap"; + fs::copy_file(good, path, fs::copy_options::overwrite_existing); + damage(path); + NavMap nm; + std::error_code ec; + EXPECT_FALSE(navmap_ros::io::load_from_file(path, nm, &ec)) << what; + EXPECT_TRUE(ec) << what; + }; + fails("unknown codec", [](const std::string & p) {const uint32_t c = 7; patch(p, 12, &c, 4);}); + fails("raw size too small", [&](const std::string & p) { + const uint64_t v = e.raw / 2; patch(p, 16, &v, 8);}); + fails("raw size too large", [&](const std::string & p) { + const uint64_t v = e.raw + 1; patch(p, 16, &v, 8);}); + fails("stored size beyond the file", [](const std::string & p) { + const uint64_t v = uint64_t{1} << 40; patch(p, 24, &v, 8);}); + fails("garbage payload", [](const std::string & p) { + const char junk[16] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + patch(p, 32, junk, sizeof(junk));}); + fails("truncated payload", [&](const std::string & p) {fs::resize_file(p, 32 + e.stored / 2);}); + fails("truncated header", [](const std::string & p) {fs::resize_file(p, 20);}); + fails("uncompressed with mismatched sizes", [&](const std::string & p) { + const uint32_t c = 0; patch(p, 12, &c, 4);}); + fs::remove_all(dir); +} + TEST(LayerMetadata, OccupancyGridUnknownCellsAreDeclaredUnknown) { // As slam_toolbox publishes it: -1 unknown, 0 free, 100 occupied @@ -347,8 +498,8 @@ TEST(LayerUpdates, SparseWhenFewCellsChangedAndAppliedOnTheSameGeometry) for (NavCelId c = 0; c < 4; ++c) { src.layer_set("cost", c, 1.0f); } - EXPECT_TRUE(navmap_ros::to_layer_update_msg(src, "cost", {}, 0.25f).indices.empty()); - EXPECT_FALSE(navmap_ros::to_layer_update_msg(src, "cost", {}, 0.9f).indices.empty()); + EXPECT_TRUE(navmap_ros::to_layer_update_msg(src, "cost", std_msgs::msg::Header(), 0.25f).indices.empty()); + EXPECT_FALSE(navmap_ros::to_layer_update_msg(src, "cost", std_msgs::msg::Header(), 0.9f).indices.empty()); } TEST(LayerUpdates, UpdatesForAnotherGeometryOrMalformedAreRejected) diff --git a/navmap_tools/navmap_tools/resample.py b/navmap_tools/navmap_tools/resample.py index d3b7e0d..66bf7a3 100644 --- a/navmap_tools/navmap_tools/resample.py +++ b/navmap_tools/navmap_tools/resample.py @@ -34,13 +34,15 @@ import os import struct import sys +import zlib from navmap_ros_interfaces.msg import NavMap, NavMapLayer, NavMapSurface, NavMapV1 import numpy as np from rclpy.serialization import deserialize_message, serialize_message MAGIC = b'NAVMAP\x00\x00' -VERSION = 2 # 2: layer metadata (unknown, description, unit) and int16 layers +VERSION = 2 # 2: layer metadata, int16 layers, links, geometry ids; compressed payload +CODEC_NONE, CODEC_ZLIB = 0, 1 FREE, LETHAL, UNKNOWN = 0, 254, 255 @@ -66,8 +68,22 @@ def read_navmap(path): data = f.read() version = 1 # files without header are the oldest format if data[:8] == MAGIC: - version, size = struct.unpack(' Date: Thu, 15 Oct 2026 16:12:12 +0200 Subject: [PATCH 09/11] Combine layers with explicit operators and provenance MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit combine_layer() composes a source layer into a destination with the costmap_2d operators (overwrite, true overwrite, max, max keeping the unknown, min, saturating add). Each layer's declared unknown value is respected, types may differ, the work can be limited to the changed cells, and an optional U8 layer records which source set each cell. The cost values move to navmap_core/Costs.hpp (navmap_ros keeps the names). README: combining layers, and the save/load section now shows the real API. Signed-off-by: Francisco Martín Rico --- README.md | 32 ++- navmap_core/CMakeLists.txt | 1 + navmap_core/include/navmap_core/Combine.hpp | 87 ++++++ navmap_core/include/navmap_core/Costs.hpp | 37 +++ navmap_core/src/navmap_core/Combine.cpp | 170 ++++++++++++ navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_combine.cpp | 259 ++++++++++++++++++ navmap_ros/include/navmap_ros/conversions.hpp | 26 +- 8 files changed, 589 insertions(+), 28 deletions(-) create mode 100644 navmap_core/include/navmap_core/Combine.hpp create mode 100644 navmap_core/include/navmap_core/Costs.hpp create mode 100644 navmap_core/src/navmap_core/Combine.cpp create mode 100644 navmap_core/tests/test_combine.cpp diff --git a/README.md b/README.md index 87b6514..2f9f695 100644 --- a/README.md +++ b/README.md @@ -311,6 +311,24 @@ for (std::uint32_t i : nm.links_from(cid)) {const auto & l = nm.links()[i]; /* . --- +## 7g. Combining layers (`Combine.hpp`) + +Each source keeps its own layer (a static prior, obstacles from a sensor, a keep-out zone), and a master layer is composed from them with `navmap::combine_layer(nm, dst, src, op, options)`. The operators are those of costmap_2d: `OVERWRITE`, `TRUE_OVERWRITE`, `MAX`, `MAX_KEEP_UNKNOWN`, `MIN` and `ADD` (saturating at `options.cap`). Each layer's declared unknown value is respected, types may differ, `options.cells` restricts the work to the cells that changed, and `options.provenance` names a U8 layer that records which source set each cell. Clearing the sensor layer never erases the prior, because the sensor never writes it. The cost values (`navmap_core/Costs.hpp`) are those of costmap_2d. + +```cpp +#include + +navmap::CombineOptions o; +o.provenance = "source"; +o.source_id = 1; +navmap::combine_layer(nm, "cost", "prior", navmap::CombineOp::TRUE_OVERWRITE, o); +o.source_id = 2; +o.cells = nm.layers.get("obstacles")->changed(); +navmap::combine_layer(nm, "cost", "obstacles", navmap::CombineOp::MAX, o); +``` + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: @@ -336,21 +354,17 @@ navmap::NavMap nm2 = navmap_ros::from_msg(msg); ## 9. Save and load from disk -NavMap supports saving and loading using YAML + mesh files: - ```cpp -#include - -// Save NavMap to disk -navmap_ros::saveMapToFile(nm, "/tmp/navmap.yaml"); +#include -// Load NavMap from disk -navmap::NavMap nm3 = navmap_ros::loadMapFromYaml("/tmp/navmap.yaml"); +navmap_ros::io::save_to_file(nm, "/tmp/map.navmap"); // zlib-compressed by default +navmap::NavMap nm3; +navmap_ros::io::load_from_file("/tmp/map.navmap", nm3); ``` --- -> **File format.** `.navmap` files are version 2 (layer metadata and int16 layers). Version 1 files, and files without header, still load: they are migrated with a deprecation warning and rewritten in place when the location is writable. +> **File format.** `.navmap` files are version 2: the CDR-serialized `NavMap` message (layer metadata, int16 layers, links, geometry id), zlib-compressed by default (`SaveOptions::compression`, `level`). zlib is available on every ROS distro and in the Python standard library. Version 1 files, and files without header, still load: they are migrated with a deprecation warning and rewritten in place when the location is writable. ## 10. Classic low-level API diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index 266fb3b..1480b39 100644 --- a/navmap_core/CMakeLists.txt +++ b/navmap_core/CMakeLists.txt @@ -17,6 +17,7 @@ add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/NavMap.cpp src/${PROJECT_NAME}/DistanceField.cpp src/${PROJECT_NAME}/Descriptors.cpp + src/${PROJECT_NAME}/Combine.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/navmap_core/include/navmap_core/Combine.hpp b/navmap_core/include/navmap_core/Combine.hpp new file mode 100644 index 0000000..6422eff --- /dev/null +++ b/navmap_core/include/navmap_core/Combine.hpp @@ -0,0 +1,87 @@ +// 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. + +/** + * @file Combine.hpp + * @brief Combining one layer into another with explicit operators, unknown-aware, with provenance. + * + * Each source (a static prior, obstacles from a sensor, a keep-out zone) keeps its own layer, and + * a master layer is composed from them. Clearing a source layer then never erases another source: + * a prior survives sensor clearing because it is never written by the sensor. + * + * The cost convention (Costs.hpp) is that of costmap_2d / Nav2. + */ + +#ifndef NAVMAP_CORE__COMBINE_HPP_ +#define NAVMAP_CORE__COMBINE_HPP_ + +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/// How a source value s is combined into the destination value d (as costmap_2d's update modes). +enum class CombineOp : uint8_t +{ + OVERWRITE, ///< d = s, except where s is unknown + TRUE_OVERWRITE, ///< d = s, unknown included + MAX, ///< d = max(d, s); s replaces an unknown d; an unknown s is ignored + MAX_KEEP_UNKNOWN, ///< as MAX, but an unknown d stays unknown + MIN, ///< d = min(d, s); s replaces an unknown d; an unknown s is ignored + ADD, ///< d = min(d + s, cap); s replaces an unknown d; an unknown s is ignored +}; + +/// Options of combine_layer(). +struct CombineOptions +{ + /// Cells to combine (e.g. the source's changed()); all cells if empty. + std::vector cells; + /// ADD saturates here (and at the range of the destination type). For a cost layer use + /// INSCRIBED_INFLATED_OBSTACLE, as costmap_2d does, so that a sum is never read as lethal. + double cap{std::numeric_limits::infinity()}; + /// U8 layer recording which source set each cell (created if absent, 0 = none). Empty: no record. + std::string provenance; + /// Value written to the provenance layer for this source. + std::uint8_t source_id{1}; +}; + +/** + * @brief Combine layer @p src into layer @p dst cell by cell with @p op. + * + * Both layers must exist with one value per NavCel; their types may differ (values convert as in + * NavMap::layer_get: integers are rounded and clamped to their range). Which value is unknown is + * each layer's own LayerMeta::unknown (NaN allowed). Writing an unknown value into @p dst writes + * the unknown of @p dst; if @p dst has none, nothing is written. + * + * Cells whose value changes are recorded with mark_changed() in @p dst. The provenance layer, if + * requested, gets source_id wherever the result is the source's value (MAX/MIN: it won or tied; + * ADD: it added something), even if the value was already the same. + * + * @return Cells whose @p dst value changed, or -1 if a layer is missing, sizes differ or + * @p dst == @p src. + */ +std::ptrdiff_t combine_layer( + NavMap & nm, const std::string & dst, const std::string & src, CombineOp op, + const CombineOptions & options = {}); + +} // namespace navmap + +#endif // NAVMAP_CORE__COMBINE_HPP_ diff --git a/navmap_core/include/navmap_core/Costs.hpp b/navmap_core/include/navmap_core/Costs.hpp new file mode 100644 index 0000000..78141d1 --- /dev/null +++ b/navmap_core/include/navmap_core/Costs.hpp @@ -0,0 +1,37 @@ +// 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. + +/** + * @file Costs.hpp + * @brief Values of a U8 cost layer, as in costmap_2d / Nav2 (so that bridging to them is a copy). + */ + +#ifndef NAVMAP_CORE__COSTS_HPP_ +#define NAVMAP_CORE__COSTS_HPP_ + +#include + +namespace navmap +{ + +constexpr std::uint8_t NO_INFORMATION = 255; ///< Unknown, not observed +constexpr std::uint8_t LETHAL_OBSTACLE = 254; ///< Obstacle +constexpr std::uint8_t INSCRIBED_INFLATED_OBSTACLE = 253; ///< The robot would touch an obstacle +constexpr std::uint8_t MAX_NON_OBSTACLE = 252; ///< Highest traversable cost +constexpr std::uint8_t FREE_SPACE = 0; ///< Known free + +} // namespace navmap + +#endif // NAVMAP_CORE__COSTS_HPP_ diff --git a/navmap_core/src/navmap_core/Combine.cpp b/navmap_core/src/navmap_core/Combine.cpp new file mode 100644 index 0000000..9f91c8b --- /dev/null +++ b/navmap_core/src/navmap_core/Combine.cpp @@ -0,0 +1,170 @@ +// 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 "navmap_core/Combine.hpp" + +#include +#include +#include +#include +#include + +namespace navmap +{ + +namespace +{ + +// Calls f(LayerView&) with the typed view of @p base; false if its type is not supported. +template +bool with_view(const std::shared_ptr & base, F && f) +{ + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return true;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return true;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return true;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return true;} + return false; +} + +bool is_unk(double v, const std::optional & u) +{ + if (!u) {return false;} + return std::isnan(*u) ? std::isnan(v) : v == *u; +} + +// Value as stored in a layer of type T (rounded and clamped as NavMap::layer_get); nullopt if T +// cannot hold it (NaN into an integer). +template +std::optional to_type(double v) +{ + if constexpr (std::is_floating_point_v) { + return static_cast(v); + } else { + if (std::isnan(v)) {return std::nullopt;} + const double lo = static_cast(std::numeric_limits::lowest()); + const double hi = static_cast(std::numeric_limits::max()); + return static_cast(std::llround(std::clamp(v, lo, hi))); + } +} + +template +bool same(T a, T b) +{ + if constexpr (std::is_floating_point_v) { + if (std::isnan(a) && std::isnan(b)) {return true;} + } + return a == b; +} + +template +std::ptrdiff_t run( + LayerView & dst, const LayerView & src, const std::optional & dst_unk, + const std::optional & src_unk, CombineOp op, const CombineOptions & o, + LayerView * prov) +{ + std::ptrdiff_t changed = 0; + auto & d = dst.data(); + const auto & s = src.data(); + auto one = [&](NavCelId c) { + if (c >= d.size()) {return;} + const double sv = static_cast(s[c]); + const double dv = static_cast(d[c]); + const bool su = is_unk(sv, src_unk), du = is_unk(dv, dst_unk); + double nv; + bool from_src = true; // the result is the source's decision + switch (op) { + case CombineOp::TRUE_OVERWRITE: + if (su && !dst_unk) {return;} + nv = su ? *dst_unk : sv; + break; + case CombineOp::OVERWRITE: + if (su) {return;} + nv = sv; + break; + case CombineOp::MAX: + case CombineOp::MAX_KEEP_UNKNOWN: + case CombineOp::MIN: + if (su || (du && op == CombineOp::MAX_KEEP_UNKNOWN)) {return;} + if (du) { + nv = sv; + } else { + nv = op == CombineOp::MIN ? std::min(dv, sv) : std::max(dv, sv); + from_src = nv == sv; + } + break; + case CombineOp::ADD: + if (su) {return;} + nv = std::min(du ? sv : dv + sv, o.cap); + from_src = du || sv != 0.0; + break; + default: + return; + } + const auto t = to_type(nv); + if (!t) {return;} + if (!same(*t, d[c])) { + d[c] = *t; + dst.mark_changed(c); + ++changed; + } + if (prov && from_src && (*prov)[c] != o.source_id) { + (*prov)[c] = o.source_id; + prov->mark_changed(c); + } + }; + if (o.cells.empty()) { + for (NavCelId c = 0; c < d.size(); ++c) { + one(c); + } + } else { + for (const NavCelId c : o.cells) { + one(c); + } + } + return changed; +} + +} // namespace + +std::ptrdiff_t combine_layer( + NavMap & nm, const std::string & dst, const std::string & src, CombineOp op, + const CombineOptions & options) +{ + const auto d = nm.layers.get(dst), s = nm.layers.get(src); + const std::size_t n = nm.navcels.size(); + if (!d || !s || dst == src || d->size() != n || s->size() != n) {return -1;} + std::shared_ptr> prov; + if (!options.provenance.empty()) { + if (options.provenance == dst || options.provenance == src) {return -1;} + if (!nm.has_layer(options.provenance)) { + nm.add_layer(options.provenance, "source that set each cell (0: none)", "", 0); + } + prov = std::dynamic_pointer_cast>(nm.layers.get(options.provenance)); + if (!prov || prov->size() != n) {return -1;} + } + const auto meta = [&](const std::string & name) { + const auto m = nm.get_layer_meta(name); + return m ? m->unknown : std::nullopt; + }; + const auto du = meta(dst), su = meta(src); + std::ptrdiff_t out = -1; + with_view( + d, [&](auto & dv) { + with_view(s, [&](auto & sv) {out = run(dv, sv, du, su, op, options, prov.get());}); + }); + return out; +} + +} // namespace navmap diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index ac4fb3b..c869955 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -77,3 +77,8 @@ ament_add_gtest(test_links test_links.cpp) target_link_libraries(test_links ${PROJECT_NAME} ) + +ament_add_gtest(test_combine test_combine.cpp) +target_link_libraries(test_combine + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_combine.cpp b/navmap_core/tests/test_combine.cpp new file mode 100644 index 0000000..bf04616 --- /dev/null +++ b/navmap_core/tests/test_combine.cpp @@ -0,0 +1,259 @@ +// 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 "navmap_core/Combine.hpp" +#include "navmap_core/Costs.hpp" +#include "navmap_core/NavMap.hpp" + +using navmap::CombineOp; +using navmap::CombineOptions; +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +constexpr uint8_t U = navmap::NO_INFORMATION; +const float kNaN = std::numeric_limits::quiet_NaN(); + +// A strip of n NavCels +NavMap strip(std::size_t n) +{ + NavMap nm; + const std::size_t s = nm.create_surface("map"); + for (std::size_t i = 0; i <= n / 2 + 1; ++i) { + nm.add_vertex({static_cast(i), 0.0f, 0.0f}); + nm.add_vertex({static_cast(i), 1.0f, 0.0f}); + } + for (std::size_t k = 0; k < n; ++k) { + const uint32_t i = static_cast(k / 2) * 2; + const NavCelId c = (k % 2 == 0) ? nm.add_navcel(i, i + 2, i + 3) : nm.add_navcel(i, i + 3, + i + 1); + nm.add_navcel_to_surface(s, c); + } + nm.rebuild_geometry_accels(); + return nm; +} + +template +void set(NavMap & nm, const std::string & name, const std::vector & v) +{ + auto l = nm.add_layer(name); + l->data() = v; + l->clear_changes(); +} + +template +std::vector get(const NavMap & nm, const std::string & name) +{ + std::vector out; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + out.push_back(nm.layer_get(name, c)); + } + return out; +} + +// Combines s into d (both U8 cost layers with 255 unknown) and returns d +std::vector cost( + const std::vector & d, const std::vector & s, CombineOp op, + CombineOptions o = {}, std::ptrdiff_t * changed = nullptr) +{ + NavMap nm = strip(d.size()); + set(nm, "d", d); + set(nm, "s", s); + nm.set_layer_unknown("d", U); + nm.set_layer_unknown("s", U); + const auto n = navmap::combine_layer(nm, "d", "s", op, o); + if (changed) {*changed = n;} + return get(nm, "d"); +} + +} // namespace + +// Cells: (d, s) = (10, 50) (50, 10) (U, 30) (40, U) (U, U) (0, 0) +const std::vector D = {10, 50, U, 40, U, 0}; +const std::vector S = {50, 10, 30, U, U, 0}; + +TEST(Combine, EachOperatorOnCostLayersWithUnknown) +{ + using V = std::vector; + EXPECT_EQ(cost(D, S, CombineOp::OVERWRITE), (V{50, 10, 30, 40, U, 0})); + EXPECT_EQ(cost(D, S, CombineOp::TRUE_OVERWRITE), (V{50, 10, 30, U, U, 0})); + EXPECT_EQ(cost(D, S, CombineOp::MAX), (V{50, 50, 30, 40, U, 0})); + EXPECT_EQ(cost(D, S, CombineOp::MAX_KEEP_UNKNOWN), (V{50, 50, U, 40, U, 0})); + EXPECT_EQ(cost(D, S, CombineOp::MIN), (V{10, 10, 30, 40, U, 0})); + EXPECT_EQ(cost(D, S, CombineOp::ADD), (V{60, 60, 30, 40, U, 0})); +} + +TEST(Combine, ReturnsTheChangedCellsAndMarksOnlyThem) +{ + std::ptrdiff_t n = -2; + cost(D, S, CombineOp::MAX, {}, &n); + EXPECT_EQ(n, 2); // cells 0 and 2 + NavMap nm = strip(6); + set(nm, "d", D); + set(nm, "s", S); + nm.set_layer_unknown("d", U); + nm.set_layer_unknown("s", U); + ASSERT_EQ(navmap::combine_layer(nm, "d", "s", CombineOp::MAX), 2); + const auto l = nm.layers.get("d"); + EXPECT_FALSE(l->all_changed()); + auto ch = l->changed(); + std::sort(ch.begin(), ch.end()); + EXPECT_EQ(ch, (std::vector{0, 2})); + // Combining again changes nothing + l->clear_changes(); + EXPECT_EQ(navmap::combine_layer(nm, "d", "s", CombineOp::MAX), 0); + EXPECT_FALSE(l->has_changes()); +} + +TEST(Combine, AddSaturatesAtTheCapAndAtTheTypeRange) +{ + using V = std::vector; + CombineOptions o; + o.cap = navmap::INSCRIBED_INFLATED_OBSTACLE; + EXPECT_EQ(cost({200, 100, U}, {100, 100, 250}, CombineOp::ADD, o), (V{253, 200, 250})); + // Without a cap the uint8 range clamps, and 255 would read as unknown: hence the cap + EXPECT_EQ(cost({200}, {100}, CombineOp::ADD), (V{255})); + // int16 layers clamp at their range too + NavMap nm = strip(2); + set(nm, "d", {32000, -32000}); + set(nm, "s", {1000, -1000}); + navmap::combine_layer(nm, "d", "s", CombineOp::ADD); + EXPECT_EQ(get(nm, "d"), (std::vector{32767, -32768})); +} + +TEST(Combine, OnlyTheGivenCells) +{ + CombineOptions o; + o.cells = {1, 3, 99}; // out of range ids are ignored + EXPECT_EQ(cost({0, 0, 0, 0}, {9, 9, 9, 9}, CombineOp::OVERWRITE, o), + (std::vector{0, 9, 0, 9})); +} + +TEST(Combine, MixedTypesConvertAndUnknownsAreEachLayersOwn) +{ + NavMap nm = strip(5); + // float source with NaN unknown into a U8 destination with 255 unknown + set(nm, "s", {3.6f, -2.0f, 400.0f, kNaN, 7.0f}); + nm.set_layer_unknown("s", kNaN); + set(nm, "d", {U, 1, 1, 1, 1}); + nm.set_layer_unknown("d", U); + navmap::combine_layer(nm, "d", "s", CombineOp::OVERWRITE); + EXPECT_EQ(get(nm, "d"), (std::vector{4, 0, 255, 1, 7})); + // TRUE_OVERWRITE turns the source's unknown into the destination's unknown + navmap::combine_layer(nm, "d", "s", CombineOp::TRUE_OVERWRITE); + EXPECT_EQ(nm.layer_get("d", 3), U); + EXPECT_TRUE(nm.is_unknown("d", 3)); + // U8 into a float destination whose unknown is NaN: MAX replaces the unknown + set(nm, "f", {kNaN, 1.5f, 1.5f, kNaN, 300.0f}); + nm.set_layer_unknown("f", kNaN); + set(nm, "u", {7, 1, 2, U, 9}); + nm.set_layer_unknown("u", U); + navmap::combine_layer(nm, "f", "u", CombineOp::MAX); + const auto f = get(nm, "f"); + EXPECT_FLOAT_EQ(f[0], 7.0f); + EXPECT_FLOAT_EQ(f[1], 1.5f); + EXPECT_FLOAT_EQ(f[2], 2.0f); + EXPECT_TRUE(std::isnan(f[3])); // both unknown + EXPECT_FLOAT_EQ(f[4], 300.0f); +} + +TEST(Combine, WithoutDeclaredUnknownsEveryValueCounts) +{ + NavMap nm = strip(3); + set(nm, "d", {255, 10, 255}); + set(nm, "s", {10, 255, 3}); + navmap::combine_layer(nm, "d", "s", CombineOp::MIN); + EXPECT_EQ(get(nm, "d"), (std::vector{10, 10, 3})); + // TRUE_OVERWRITE of an unknown source into a destination without unknown: left as is + nm.set_layer_unknown("s", 255); + navmap::combine_layer(nm, "d", "s", CombineOp::TRUE_OVERWRITE); + EXPECT_EQ(get(nm, "d"), (std::vector{10, 10, 3})); + // NaN from a float source without declared unknown cannot go into an integer layer + set(nm, "nan", {kNaN, kNaN, 1.0f}); + navmap::combine_layer(nm, "d", "nan", CombineOp::OVERWRITE); + EXPECT_EQ(get(nm, "d"), (std::vector{10, 10, 1})); +} + +TEST(Combine, ProvenanceRecordsTheDecidingSource) +{ + NavMap nm = strip(4); + set(nm, "master", {0, 0, 0, 0}); + set(nm, "prior", {0, 254, 0, 0}); + set(nm, "obstacles", {254, 0, 0, 100}); + CombineOptions p; + p.provenance = "who"; + p.source_id = 1; + navmap::combine_layer(nm, "master", "prior", CombineOp::OVERWRITE, p); + EXPECT_EQ(get(nm, "who"), (std::vector{1, 1, 1, 1})); + EXPECT_EQ(nm.get_layer_meta("who")->description, "source that set each cell (0: none)"); + p.source_id = 2; + navmap::combine_layer(nm, "master", "obstacles", CombineOp::MAX, p); + EXPECT_EQ(get(nm, "master"), (std::vector{254, 254, 0, 100})); + // Obstacles won cells 0 and 3, tied in cell 2 (0 == 0); the prior keeps cell 1 + EXPECT_EQ(get(nm, "who"), (std::vector{2, 1, 2, 2})); +} + +TEST(Combine, APriorSurvivesClearingTheSensorLayer) +{ + // master = prior, then MAX obstacles. The sensor clears its own layer; the prior stays + NavMap nm = strip(3); + set(nm, "prior", {254, 0, 0}); + set(nm, "obstacles", {0, 254, 0}); + set(nm, "master", {0, 0, 0}); + auto compose = [&]() { + navmap::combine_layer(nm, "master", "prior", CombineOp::TRUE_OVERWRITE); + navmap::combine_layer(nm, "master", "obstacles", CombineOp::MAX); + return get(nm, "master"); + }; + EXPECT_EQ(compose(), (std::vector{254, 254, 0})); + nm.layer_clear("obstacles", 0); + EXPECT_EQ(compose(), (std::vector{254, 0, 0})); +} + +TEST(Combine, InvalidRequestsAreRejected) +{ + NavMap nm = strip(3); + set(nm, "a", {1, 2, 3}); + set(nm, "b", {1, 2, 3}); + set(nm, "f", {1, 2, 3}); + EXPECT_EQ(navmap::combine_layer(nm, "a", "missing", CombineOp::MAX), -1); + EXPECT_EQ(navmap::combine_layer(nm, "missing", "a", CombineOp::MAX), -1); + EXPECT_EQ(navmap::combine_layer(nm, "a", "a", CombineOp::MAX), -1); + CombineOptions o; + o.provenance = "a"; + EXPECT_EQ(navmap::combine_layer(nm, "a", "b", CombineOp::MAX, o), -1); + o.provenance = "f"; // exists with another type + EXPECT_EQ(navmap::combine_layer(nm, "a", "b", CombineOp::MAX, o), -1); + // A layer of another size (stale after the geometry changed) + nm.layers.add_or_get("short", 2, navmap::LayerType::U8); + EXPECT_EQ(navmap::combine_layer(nm, "a", "short", CombineOp::MAX), -1); + EXPECT_EQ(get(nm, "a"), (std::vector{1, 2, 3})); // untouched + // An empty map: nothing to do + NavMap empty; + empty.add_layer("x"); + empty.add_layer("y"); + EXPECT_EQ(navmap::combine_layer(empty, "x", "y", CombineOp::ADD), 0); +} diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 1990f9c..024c407 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -46,6 +46,7 @@ #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" +#include "navmap_core/Costs.hpp" #include "navmap_core/NavMap.hpp" /** @@ -55,25 +56,12 @@ namespace navmap_ros { -/** - * @name Costmap value semantics - * @brief Standardized occupancy/cost values used when projecting NavMap layers - * onto a 2D grid (compatible with `costmap_2d` conventions). - * - * These constants follow the same meaning as in `costmap_2d`: - * - `NO_INFORMATION` (255): Unknown or unobserved area. - * - `LETHAL_OBSTACLE` (254): Non-traversable obstacle. - * - `INSCRIBED_INFLATED_OBSTACLE` (253): Inside the robot’s inscribed radius. - * - `MAX_NON_OBSTACLE` (252): Highest cost still considered traversable. - * - `FREE_SPACE` (0): Known free space. - * @{ - */ -constexpr uint8_t NO_INFORMATION = 255; -constexpr uint8_t LETHAL_OBSTACLE = 254; -constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253; -constexpr uint8_t MAX_NON_OBSTACLE = 252; -constexpr uint8_t FREE_SPACE = 0; -/** @} */ // end of Costmap value semantics group +// Cost values (costmap_2d convention), defined in navmap_core/Costs.hpp +using navmap::NO_INFORMATION; +using navmap::LETHAL_OBSTACLE; +using navmap::INSCRIBED_INFLATED_OBSTACLE; +using navmap::MAX_NON_OBSTACLE; +using navmap::FREE_SPACE; // --------- NavMap <-> ROS message --------- From 31ca125eb0e033f4733bb27d880bdac0c6ca0007 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 16:23:46 +0200 Subject: [PATCH 10/11] Rays through a shared edge always hit a NavCel MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The barycentric tolerance of ray_triangle_intersect was absolute (1e-8), below float rounding for NavCels of a few decimetres or more. A vertical ray through the edge between two NavCels could miss both, and locate_navcel then fell back to the floor above. The tolerance is now relative (1e-5). Signed-off-by: Francisco Martín Rico --- navmap_core/include/navmap_core/Geometry.hpp | 6 ++++-- navmap_core/tests/test_geometry.cpp | 22 ++++++++++++++++++++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/navmap_core/include/navmap_core/Geometry.hpp b/navmap_core/include/navmap_core/Geometry.hpp index fbff8d6..68b0338 100644 --- a/navmap_core/include/navmap_core/Geometry.hpp +++ b/navmap_core/include/navmap_core/Geometry.hpp @@ -113,6 +113,8 @@ inline bool ray_triangle_intersect( float & v) { const float kEps = 1e-8f; + // Barycentric slack, relative: a ray through a shared edge hits both NavCels, never neither + const float kBary = 1e-5f; Vec3 e1 = v1 - v0; Vec3 e2 = v2 - v0; Vec3 pvec = dir.cross(e2); @@ -123,12 +125,12 @@ inline bool ray_triangle_intersect( float inv_det = 1.0f / det; Vec3 tvec = orig - v0; u = tvec.dot(pvec) * inv_det; - if (u < -kEps || u > 1.0f + kEps) { + if (u < -kBary || u > 1.0f + kBary) { return false; } Vec3 qvec = tvec.cross(e1); v = dir.dot(qvec) * inv_det; - if (v < -kEps || (u + v) > 1.0f + kEps) { + if (v < -kBary || (u + v) > 1.0f + kBary) { return false; } t = e2.dot(qvec) * inv_det; diff --git a/navmap_core/tests/test_geometry.cpp b/navmap_core/tests/test_geometry.cpp index 82a5148..9504d8b 100644 --- a/navmap_core/tests/test_geometry.cpp +++ b/navmap_core/tests/test_geometry.cpp @@ -150,6 +150,28 @@ TEST(Geometry_RayTriangle, HitMissEdgeBackfaceParallel) { ASSERT_FALSE(ray_triangle_intersect(o, d_up, a, b, c, t, u, v)); } +TEST(Geometry_RayTriangle, RaysThroughASharedEdgeAlwaysHit) { + // Two large NavCels sharing a diagonal, far from the origin (float rounding grows with both) + using navmap::Vec3; + for (const float off : {0.0f, 5.0f, 100.0f, 1000.0f}) { + for (const float size : {0.1f, 3.0f, 30.0f}) { + const Vec3 a(off, off, 4.7f), b(off + size, off - 0.3f * size, 4.7f); + const Vec3 c(off + 0.4f * size, off + 0.9f * size, 4.701f), d(off + 1.3f * size, off + size, + 4.702f); + for (int i = 1; i < 500; ++i) { + const float s = i / 500.0f; + const Vec3 p = b + s * (c - b); // on the shared edge b-c + float t, u, v; + const bool h1 = navmap::ray_triangle_intersect(p + Vec3(0, 0, 1), Vec3(0, 0, -1), a, b, c, + t, u, v); + const bool h2 = navmap::ray_triangle_intersect(p + Vec3(0, 0, 1), Vec3(0, 0, -1), b, d, c, + t, u, v); + ASSERT_TRUE(h1 || h2) << "offset " << off << " size " << size << " s " << s; + } + } + } +} + TEST(Geometry_AABB, ExpandPointBoxAxis) { AABB box; From 4309ca5fdb8900d9b113d0e00f4dddbc1263e0e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 16:23:46 +0200 Subject: [PATCH 11/11] Simplify the mesh keeping the layers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit simplify() merges NavCels by half-edge collapses, cheapest quadric error first. A vertex goes only if all its NavCels share surface and layer values, so every layer keeps its value at every point. Open edges keep their shape, the topology is checked (link condition, no folds), link ends survive, and quality, normal change and edge length are bounded. Layers, surfaces, links and colors are carried over. navmap_simplify simplifies a .navmap file. Playground maps drop to 21-23 % of their NavCels (occupancy grids) and 4-11 % (parking garage), staying within millimetres of the original surface. Signed-off-by: Francisco Martín Rico --- README.md | 18 + navmap_core/CMakeLists.txt | 1 + navmap_core/include/navmap_core/Simplify.hpp | 75 ++++ navmap_core/src/navmap_core/Simplify.cpp | 408 ++++++++++++++++++ navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_simplify.cpp | 410 +++++++++++++++++++ navmap_tools/CMakeLists.txt | 7 + navmap_tools/src/navmap_simplify.cpp | 106 +++++ 8 files changed, 1030 insertions(+) create mode 100644 navmap_core/include/navmap_core/Simplify.hpp create mode 100644 navmap_core/src/navmap_core/Simplify.cpp create mode 100644 navmap_core/tests/test_simplify.cpp create mode 100644 navmap_tools/src/navmap_simplify.cpp diff --git a/README.md b/README.md index 2f9f695..7df9c32 100644 --- a/README.md +++ b/README.md @@ -329,6 +329,24 @@ navmap::combine_layer(nm, "cost", "obstacles", navmap::CombineOp::MAX, o); --- +## 7h. Simplification that keeps the layers (`Simplify.hpp`) + +`navmap::simplify(nm, params)` merges NavCels where the surface is flat (within `max_error`) and the layers have the same value, and keeps the original ones where they change: every layer keeps its value at every point, open edges keep their shape and the topology does not change (floors, holes, links). `max_edge_length` bounds the NavCel size. Layers that are per-cell geometry (`elevation`, `support` from `from_points`) go in `ignore_layers`. + +```bash +ros2 run navmap_tools navmap_simplify in.navmap out.navmap --max-error 0.02 \ + --ignore-layer elevation --ignore-layer support +``` + +| Map (playgrounds) | NavCels | `max_error` | After | File (zlib) | +|---|---|---|---|---| +| `home2_20cm` (occupancy grid) | 10 716 | 0.02 m | 22.6 % | 248 KB → 16 KB | +| `warehouse_20cm` (occupancy grid) | 18 720 | 0.02 m | 21.4 % | 433 KB → 25 KB | +| `parking_garage` (`from_points`) | 38 543 | 0.02 / 0.1 m | 10.6 / 4.2 % | 1.06 MB → 56 / 24 KB | +| `excavation_urjc` (`from_points`) | 29 728 | 0.02 / 0.1 m | 67 / 38 % | 822 KB → 235 / 142 KB | + +--- + ## 8. Serialize to / from ROS messages Conversion functions are provided in `navmap_ros`: diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index 1480b39..0b3ae95 100644 --- a/navmap_core/CMakeLists.txt +++ b/navmap_core/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/DistanceField.cpp src/${PROJECT_NAME}/Descriptors.cpp src/${PROJECT_NAME}/Combine.cpp + src/${PROJECT_NAME}/Simplify.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/navmap_core/include/navmap_core/Simplify.hpp b/navmap_core/include/navmap_core/Simplify.hpp new file mode 100644 index 0000000..5270f15 --- /dev/null +++ b/navmap_core/include/navmap_core/Simplify.hpp @@ -0,0 +1,75 @@ +// 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. + +/** + * @file Simplify.hpp + * @brief Mesh simplification that keeps the layers: large NavCels where surface and layers are + * uniform, the original ones where they change. + */ + +#ifndef NAVMAP_CORE__SIMPLIFY_HPP_ +#define NAVMAP_CORE__SIMPLIFY_HPP_ + +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/// Parameters of simplify(). +struct SimplifyParams +{ + /// Geometric error (m): every vertex stays within this distance of the planes of the original + /// NavCels it replaces (quadric error), and of the open edges it moves along. + float max_error{0.02f}; + /// No edge longer than this (m). Bounds the NavCel size, e.g. to keep marking obstacles fine. + float max_edge_length{std::numeric_limits::infinity()}; + /// Minimum NavCel quality, 4·sqrt(3)·area / sum of squared edges (1 equilateral, 0 flat). + float min_quality{0.15f}; + /// Maximum rotation (deg) of a NavCel's normal with respect to the original one. + float max_normal_change{30.0f}; + /// Layers whose values must not change anywhere; empty: all the layers of the map. + std::vector layers; + /// 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; +}; + +/// Sizes before and after simplify(). +struct SimplifyStats +{ + std::size_t navcels_before{0}, navcels_after{0}; + std::size_t vertices_before{0}, vertices_after{0}; +}; + +/** + * @brief Simplify @p nm in place by half-edge collapses, cheapest first. + * + * A vertex is removed only if all its NavCels have the same values in the kept layers and belong to + * the same surface, so every layer keeps its value at every point of the map: borders between + * values (an obstacle, unknown space, a different cost) are kept exactly. Open edges keep their + * shape (within max_error), topology is preserved (no folds, no new holes or bridges), and NavCels + * that are the end of a link survive. Layers, surfaces, links and vertex colors are carried over; + * NavCel and vertex ids change. Geometry accelerations are rebuilt. + */ +SimplifyStats simplify(NavMap & nm, const SimplifyParams & params = {}); + +} // namespace navmap + +#endif // NAVMAP_CORE__SIMPLIFY_HPP_ diff --git a/navmap_core/src/navmap_core/Simplify.cpp b/navmap_core/src/navmap_core/Simplify.cpp new file mode 100644 index 0000000..48e7c3a --- /dev/null +++ b/navmap_core/src/navmap_core/Simplify.cpp @@ -0,0 +1,408 @@ +// 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 "navmap_core/Simplify.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/Geometry.hpp" + +namespace navmap +{ + +namespace +{ + +using Vec3d = Eigen::Vector3d; +using Quadric = Eigen::Matrix4d; +constexpr uint32_t kNone = std::numeric_limits::max(); + +// Calls f(view) with the typed view of @p base (no-op for an unknown type). +template +void with_view(const std::shared_ptr & base, F && f) +{ + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} +} + +Quadric plane_quadric(const Vec3d & n, const Vec3d & p) +{ + const Eigen::Vector4d q(n.x(), n.y(), n.z(), -n.dot(p)); + return q * q.transpose(); +} + +class Simplifier +{ +public: + Simplifier(NavMap & nm, const SimplifyParams & p) + : nm_(nm), p_(p), cos_normal_(std::cos(p.max_normal_change * M_PI / 180.0)), + max_err2_(static_cast(p.max_error) * p.max_error) + {} + + SimplifyStats run() + { + SimplifyStats st; + st.navcels_before = nm_.navcels.size(); + st.vertices_before = nm_.positions.size(); + if (nm_.navcels.empty()) {return st;} + init(); + for (uint32_t u = 0; u < P_.size(); ++u) { + push_best(u); + } + while (!pq_.empty()) { + const Entry e = pq_.top(); + pq_.pop(); + if (dead_[e.u] || e.stamp != stamp_[e.u]) {continue;} + double cost; + if (!valid(e.u, e.v, cost)) {push_best(e.u); continue;} + collapse(e.u, e.v); + for (const uint32_t x : neighbors(e.v)) { + push_best(x); + } + push_best(e.v); + } + compact(); + st.navcels_after = nm_.navcels.size(); + st.vertices_after = nm_.positions.size(); + return st; + } + +private: + struct Entry + { + double cost; + uint32_t u, v, stamp; + bool operator<(const Entry & o) const {return cost > o.cost;} // min-heap + }; + + void init() + { + const std::size_t nv = nm_.positions.size(), nt = nm_.navcels.size(); + P_.resize(nv); + for (uint32_t i = 0; i < nv; ++i) { + P_[i] = nm_.positions.at(i).cast(); + } + T_.resize(nt); + N0_.resize(nt); + alive_.assign(nt, 1); + VT_.assign(nv, {}); + Q_.assign(nv, Quadric::Zero()); + dead_.assign(nv, 0); + locked_.assign(nv, 0); + stamp_.assign(nv, 0); + for (uint32_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + T_[t][k] = nm_.navcels[t].v[k]; + VT_[T_[t][k]].push_back(t); + } + const Vec3d a = P_[T_[t][0]], b = P_[T_[t][1]], c = P_[T_[t][2]]; + const Vec3d n = (b - a).cross(c - a); + N0_[t] = n.norm() > 0.0 ? Vec3d(n.normalized()) : Vec3d::UnitZ(); + const Quadric q = plane_quadric(N0_[t], a); + for (int k = 0; k < 3; ++k) { + Q_[T_[t][k]] += q; + } + } + // Open edges: a plane through the edge, perpendicular to its NavCel, keeps the border's shape. + // Edges of 3+ NavCels (non-manifold) lock their vertices. + for (uint32_t t = 0; t < nt; ++t) { + for (int k = 0; k < 3; ++k) { + const uint32_t a = T_[t][k], b = T_[t][(k + 1) % 3]; + const std::size_t m = edge_tris(a, b).size(); + if (m > 2) {locked_[a] = locked_[b] = 1;} + if (m != 1) {continue;} + const Vec3d e = P_[b] - P_[a]; + const Vec3d n = e.cross(N0_[t]); + if (n.norm() == 0.0) {continue;} + const Quadric q = plane_quadric(n.normalized(), P_[a]); + Q_[a] += q; + Q_[b] += q; + } + } + // Same values in every kept layer <=> same class + std::vector names = p_.layers.empty() ? nm_.list_layers() : p_.layers; + std::vector> cols; + for (const auto & name : names) { + if (std::find(p_.ignore_layers.begin(), p_.ignore_layers.end(), name) != + p_.ignore_layers.end()) + { + continue; + } + const auto base = nm_.layers.get(name); + if (!base || base->size() != nt) {continue;} + std::vector col(nt); + with_view( + base, [&](const auto & v) { + for (std::size_t c = 0; c < nt; ++c) { + col[c] = static_cast(v.data()[c]); + } + }); + cols.push_back(std::move(col)); + } + cls_.assign(nt, 0); + std::unordered_map ids; + std::string key(cols.size() * sizeof(double), '\0'); + for (std::size_t t = 0; t < nt; ++t) { + for (std::size_t l = 0; l < cols.size(); ++l) { + double v = cols[l][t]; + if (std::isnan(v)) {v = std::numeric_limits::quiet_NaN();} // one NaN + if (v == 0.0) {v = 0.0;} // one zero + std::memcpy(key.data() + l * sizeof(double), &v, sizeof(double)); + } + cls_[t] = ids.try_emplace(key, static_cast(ids.size())).first->second; + } + surf_.assign(nt, NavMap::kNoSurface); + for (std::size_t s = 0; s < nm_.surfaces.size(); ++s) { + for (const NavCelId c : nm_.surfaces[s].navcels) { + if (c < nt) {surf_[c] = s;} + } + } + // NavCels at the end of a link must survive as they are + for (const auto & l : nm_.links()) { + for (const NavCelId c : {l.from, l.to}) { + if (c >= nt) {continue;} + for (const uint32_t x : T_[c]) { + locked_[x] = 1; + } + } + } + for (uint32_t u = 0; u < nv; ++u) { + if (VT_[u].empty()) {dead_[u] = 1;} + } + } + + std::vector edge_tris(uint32_t a, uint32_t b) const + { + std::vector out; + for (const uint32_t t : VT_[a]) { + if (T_[t][0] == b || T_[t][1] == b || T_[t][2] == b) {out.push_back(t);} + } + return out; + } + + std::vector neighbors(uint32_t u) const + { + std::vector out; + for (const uint32_t t : VT_[u]) { + for (const uint32_t x : T_[t]) { + if (x != u) {out.push_back(x);} + } + } + std::sort(out.begin(), out.end()); + out.erase(std::unique(out.begin(), out.end()), out.end()); + return out; + } + + // 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 auto & fan = VT_[u]; + for (const uint32_t t : fan) { + if (cls_[t] != cls_[fan[0]] || surf_[t] != surf_[fan[0]]) {return false;} + } + const auto et = edge_tris(u, v); + if (et.empty() || et.size() > 2) {return false;} + // Border: u on an open edge moves only along it, onto a neighbour on the same open edge + const auto nu = neighbors(u); + std::vector open; + for (const uint32_t w : nu) { + if (edge_tris(u, w).size() == 1) {open.push_back(w);} + } + if (open.size() == 2) { + if (et.size() != 1) {return false;} + const uint32_t w = open[0] == v ? open[1] : (open[1] == v ? open[0] : kNone); + if (w == kNone) {return false;} + const Vec3d d = P_[w] - P_[v]; + const double len2 = d.squaredNorm(); + 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;} + } else if (!open.empty() || et.size() != 2) { + return false; + } + // Link condition: u and v share exactly the opposite vertices of their common NavCels + const auto nv = neighbors(v); + std::size_t common = 0; + for (const uint32_t x : nu) { + if (std::binary_search(nv.begin(), nv.end(), x)) {++common;} + } + 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; + bool any = false; + double dev2 = std::numeric_limits::infinity(); + for (const uint32_t t : fan) { + if (std::find(et.begin(), et.end(), t) != et.end()) {continue;} + std::array q; + for (int k = 0; k < 3; ++k) { + q[k] = T_[t][k] == u ? P_[v] : P_[T_[t][k]]; + } + const Vec3d n = (q[1] - q[0]).cross(q[2] - q[0]); + const double a2 = n.norm(); + const double sum = (q[1] - q[0]).squaredNorm() + (q[2] - q[1]).squaredNorm() + + (q[0] - q[2]).squaredNorm(); + if (a2 <= 1e-12 * sum) {return false;} + if (n.dot(N0_[t]) / a2 < cos_normal_) {return false;} + if (2.0 * std::sqrt(3.0) * a2 / sum < p_.min_quality) {return false;} + for (int k = 0; k < 3; ++k) { + if (T_[t][k] != u && (P_[T_[t][k]] - P_[v]).squaredNorm() > max_edge2) {return false;} + } + dev2 = std::min( + dev2, static_cast(point_triangle_squared_distance( + P_[u].cast(), q[0].cast(), q[1].cast(), q[2].cast()))); + any = true; + } + 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_; + } + + void push_best(uint32_t u) + { + ++stamp_[u]; + if (dead_[u] || locked_[u]) {return;} + double best = std::numeric_limits::infinity(); + uint32_t to = kNone; + for (const uint32_t v : neighbors(u)) { + double c; + if (valid(u, v, c) && c < best) {best = c; to = v;} + } + if (to != kNone) {pq_.push({best, u, to, stamp_[u]});} + } + + void collapse(uint32_t u, uint32_t v) + { + for (const uint32_t t : edge_tris(u, v)) { + alive_[t] = 0; + for (const uint32_t x : T_[t]) { + auto & l = VT_[x]; + l.erase(std::remove(l.begin(), l.end(), t), l.end()); + } + } + for (const uint32_t t : VT_[u]) { + for (auto & x : T_[t]) { + if (x == u) {x = v;} + } + VT_[v].push_back(t); + } + VT_[u].clear(); + Q_[v] += Q_[u]; + dead_[u] = 1; + } + + void compact() + { + const std::size_t nt = T_.size(); + std::vector vmap(P_.size(), kNone), tmap(nt, kNone); + std::vector kept; + Positions pos; + std::optional col; + const bool has_col = nm_.colors && nm_.colors->r.size() == P_.size(); + if (has_col) {col.emplace();} + std::vector cels; + for (uint32_t t = 0; t < nt; ++t) { + if (!alive_[t]) {continue;} + NavCel c; + for (int k = 0; k < 3; ++k) { + const uint32_t x = T_[t][k]; + if (vmap[x] == kNone) { + vmap[x] = static_cast(pos.size()); + pos.x.push_back(static_cast(P_[x].x())); + pos.y.push_back(static_cast(P_[x].y())); + pos.z.push_back(static_cast(P_[x].z())); + if (has_col) { + col->r.push_back(nm_.colors->r[x]); + col->g.push_back(nm_.colors->g[x]); + col->b.push_back(nm_.colors->b[x]); + col->a.push_back(nm_.colors->a[x]); + } + } + c.v[k] = vmap[x]; + } + tmap[t] = static_cast(cels.size()); + kept.push_back(t); + cels.push_back(c); + } + for (auto & s : nm_.surfaces) { + std::vector ids; + for (const NavCelId c : s.navcels) { + if (c < nt && tmap[c] != kNone) {ids.push_back(tmap[c]);} + } + s.navcels = std::move(ids); + } + for (const auto & name : nm_.list_layers()) { + const auto base = nm_.layers.get(name); + with_view( + base, [&](auto & v) { + auto & d = v.data(); + std::remove_reference_t out(kept.size()); + for (std::size_t i = 0; i < kept.size(); ++i) { + if (kept[i] < d.size()) {out[i] = d[kept[i]];} + } + d.swap(out); + v.mark_all_changed(); + }); + } + const auto links = nm_.links(); + nm_.positions = std::move(pos); + nm_.colors = std::move(col); + nm_.navcels = std::move(cels); + nm_.clear_links(); + for (const auto & l : links) { + if (l.from < nt && l.to < nt && tmap[l.from] != kNone && tmap[l.to] != kNone) { + nm_.add_link(tmap[l.from], tmap[l.to], l.type, l.length, l.bidirectional); + } + } + nm_.rebuild_geometry_accels(); + } + + NavMap & nm_; + const SimplifyParams & p_; + const double cos_normal_, max_err2_; + std::vector P_; + std::vector> T_; + std::vector N0_; + std::vector alive_, dead_, locked_; + std::vector> VT_; + std::vector Q_; + std::vector cls_, stamp_; + std::vector surf_; + std::priority_queue pq_; +}; + +} // namespace + +SimplifyStats simplify(NavMap & nm, const SimplifyParams & params) +{ + return Simplifier(nm, params).run(); +} + +} // namespace navmap diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index c869955..a23e218 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -82,3 +82,8 @@ ament_add_gtest(test_combine test_combine.cpp) target_link_libraries(test_combine ${PROJECT_NAME} ) + +ament_add_gtest(test_simplify test_simplify.cpp) +target_link_libraries(test_simplify + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_simplify.cpp b/navmap_core/tests/test_simplify.cpp new file mode 100644 index 0000000..f4ee8bb --- /dev/null +++ b/navmap_core/tests/test_simplify.cpp @@ -0,0 +1,410 @@ +// 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 +#include + +#include "navmap_core/NavMap.hpp" +#include "navmap_core/Simplify.hpp" + +using navmap::NavCelId; +using navmap::NavMap; +using navmap::SimplifyParams; + +namespace +{ + +using Height = std::function; +using Keep = std::function; + +// nx x ny cells of side res (two NavCels each) at offset z, cells where keep(i, j); one surface +void add_grid( + NavMap & nm, int nx, int ny, float res, const Height & h, const Keep & keep = 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, h(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 (keep && !keep(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))); + } + } +} + +float flat(float, float) {return 0.0f;} + +float area(const NavMap & nm) +{ + double a = 0.0; + for (const auto & c : nm.navcels) { + a += c.area; + } + return static_cast(a); +} + +// V - E + F over the mesh (1 per disc, 0 per annulus...) +int64_t euler(const NavMap & nm) +{ + std::set> edges; + std::set verts; + for (const auto & c : nm.navcels) { + for (int k = 0; k < 3; ++k) { + const uint32_t a = c.v[k], b = c.v[(k + 1) % 3]; + edges.insert({std::min(a, b), std::max(a, b)}); + verts.insert(a); + } + } + return static_cast(verts.size()) - static_cast(edges.size()) + + static_cast(nm.navcels.size()); +} + +std::size_t open_edges(const NavMap & nm) +{ + std::size_t n = 0; + for (const auto & c : nm.navcels) { + for (const auto x : c.neighbor) { + if (x == std::numeric_limits::max()) {++n;} + } + } + return n; +} + +float max_edge(const NavMap & nm) +{ + float m = 0.0f; + for (const auto & c : nm.navcels) { + for (int k = 0; k < 3; ++k) { + m = std::max(m, (nm.positions.at(c.v[k]) - nm.positions.at(c.v[(k + 1) % 3])).norm()); + } + } + return m; +} + +// Value of `layer` under every probe point, before and after, must match +template +void expect_same_values( + const NavMap & a, const NavMap & b, const std::string & layer, + const std::vector & probes) +{ + std::size_t s; + NavCelId ca, cb; + Eigen::Vector3f bary; + for (const auto & p : probes) { + ASSERT_TRUE(a.locate_navcel(p, s, ca, bary, nullptr)) << p.transpose(); + ASSERT_TRUE(b.locate_navcel(p, s, cb, bary, nullptr)) << p.transpose(); + ASSERT_EQ(a.layer_get(layer, ca), b.layer_get(layer, cb)) << layer << " at " + << p.transpose(); + } +} + +std::vector centroids(const NavMap & nm, float dz = 0.05f) +{ + std::vector out; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + out.push_back(nm.navcel_centroid(c) + Eigen::Vector3f(0, 0, dz)); + } + return out; +} + +// A 4 x 4 m floor at 0.1 m: free, a wall, an unknown corner, and a uniform float layer +NavMap occupancy_floor() +{ + NavMap nm; + add_grid(nm, 40, 40, 0.1f, flat); + nm.rebuild_geometry_accels(); + auto occ = nm.add_layer("occupancy", "", "", 0); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.navcel_centroid(c); + if (p.y() > 2.0f && p.y() < 2.1f && p.x() < 3.0f) {occ->data()[c] = 254;} + if (p.x() > 3.0f && p.y() > 3.0f) {occ->data()[c] = 255;} + } + nm.set_layer_unknown("occupancy", 255); + nm.add_layer("friction", "", "", 0.7f); + return nm; +} + +} // namespace + +TEST(Simplify, FlatFloorShrinksAndEveryPointKeepsItsLayerValues) +{ + const NavMap before = occupancy_floor(); + NavMap nm = before; + const auto st = navmap::simplify(nm); + EXPECT_EQ(st.navcels_before, 3200u); + EXPECT_EQ(st.navcels_after, nm.navcels.size()); + EXPECT_EQ(st.vertices_after, nm.positions.size()); + EXPECT_LT(nm.navcels.size(), before.navcels.size() / 5); + EXPECT_NEAR(area(nm), 16.0f, 1e-3f); + EXPECT_EQ(euler(nm), 1); + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + ASSERT_EQ(nm.positions.z[i], 0.0f); + } + const auto probes = centroids(before); + expect_same_values(before, nm, "occupancy", probes); + expect_same_values(before, nm, "friction", probes); + // Metadata, sizes and change tracking follow; the geometry is another one + EXPECT_EQ(nm.layer_size("occupancy"), nm.navcels.size()); + EXPECT_EQ(nm.get_layer_meta("occupancy")->unknown, 255.0); + EXPECT_TRUE(nm.layers.get("occupancy")->all_changed()); + EXPECT_NE(nm.geometry_id(), before.geometry_id()); + EXPECT_EQ(nm.surfaces.size(), 1u); + EXPECT_EQ(nm.surfaces[0].navcels.size(), nm.navcels.size()); + // Simplifying again finds (almost) nothing more to do + const std::size_t once = nm.navcels.size(); + navmap::simplify(nm); + EXPECT_GE(nm.navcels.size(), once * 9 / 10); + expect_same_values(before, nm, "occupancy", probes); +} + +TEST(Simplify, WithoutLayersAFlatSquareBecomesAFewNavCels) +{ + NavMap nm; + add_grid(nm, 30, 30, 0.1f, flat); + nm.rebuild_geometry_accels(); + navmap::simplify(nm); + EXPECT_LE(nm.navcels.size(), 16u); + EXPECT_NEAR(area(nm), 9.0f, 1e-3f); + EXPECT_EQ(open_edges(nm), open_edges(nm)); // well formed + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + const float x = nm.positions.x[i], y = nm.positions.y[i]; + // The square's outline stays: every vertex on its border (no interior vertex needed) + EXPECT_TRUE(std::abs(x) < 1e-4f || std::abs(x - 3.0f) < 1e-4f || + std::abs(y) < 1e-4f || std::abs(y - 3.0f) < 1e-4f) << x << " " << y; + } +} + +TEST(Simplify, MaxEdgeLengthBoundsTheNavCels) +{ + NavMap coarse, bounded; + add_grid(coarse, 30, 30, 0.1f, flat); + coarse.rebuild_geometry_accels(); + bounded = coarse; + navmap::simplify(coarse); + SimplifyParams p; + p.max_edge_length = 0.5f; + navmap::simplify(bounded, p); + EXPECT_LE(max_edge(bounded), 0.5f + 1e-5f); + EXPECT_GT(max_edge(coarse), 1.0f); + EXPECT_GT(bounded.navcels.size(), coarse.navcels.size()); + EXPECT_LT(bounded.navcels.size(), 1800u / 2); + EXPECT_NEAR(area(bounded), 9.0f, 1e-3f); +} + +TEST(Simplify, AnInclineStaysOnItsPlane) +{ + const float k = std::tan(20.0f * static_cast(M_PI) / 180.0f); + NavMap nm; + add_grid(nm, 30, 20, 0.1f, [k](float x, float) {return k * x;}); + nm.rebuild_geometry_accels(); + const float a0 = area(nm); + navmap::simplify(nm); + EXPECT_LT(nm.navcels.size(), 60u); + EXPECT_NEAR(area(nm), a0, 1e-3f * a0); + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + EXPECT_NEAR(nm.positions.z[i], k * nm.positions.x[i], 1e-5f); + } + for (const auto & c : nm.navcels) { + EXPECT_NEAR(std::acos(c.normal.z()) * 180.0f / static_cast(M_PI), 20.0f, 0.01f); + } +} + +TEST(Simplify, CurvedTerrainStaysWithinTheErrorAndZeroErrorKeepsIt) +{ + const Height bumps = [](float x, float y) {return 0.3f * std::sin(x) * std::cos(0.7f * y);}; + NavMap orig; + add_grid(orig, 50, 50, 0.1f, bumps); + orig.rebuild_geometry_accels(); + for (const float e : {0.01f, 0.05f}) { + NavMap nm = orig; + SimplifyParams p; + p.max_error = e; + navmap::simplify(nm, p); + EXPECT_LT(nm.navcels.size(), orig.navcels.size()) << e; + // Original NavCel centroids (on the original surface), measured vertically to the new one + float worst = 0.0f; + for (const auto & q : centroids(orig, 0.0f)) { + NavCelId c; + float t; + Eigen::Vector3f hit; + ASSERT_TRUE(nm.raycast(q + Eigen::Vector3f(0, 0, 1.0f), {0, 0, -1.0f}, c, t, hit)) << + q.transpose(); + worst = std::max(worst, std::abs(hit.z() - q.z())); + } + EXPECT_LE(worst, 1.5f * e) << e; + std::printf("max_error %.3f: worst %.4f m, %zu -> %zu NavCels\n", e, worst, + orig.navcels.size(), nm.navcels.size()); + } + NavMap nm = orig; + SimplifyParams p; + p.max_error = 0.0f; + navmap::simplify(nm, p); + EXPECT_GT(nm.navcels.size(), orig.navcels.size() * 9 / 10); +} + +TEST(Simplify, HolesAndStackedFloorsAreKept) +{ + NavMap orig; + // Ground floor with a 1 x 1 m hole (a stairwell), and a full floor 3 m above + add_grid(orig, 40, 40, 0.1f, flat, [](int i, int j) { + return !(i >= 15 && i < 25 && j >= 15 && + j < 25); + }); + add_grid(orig, 40, 40, 0.1f, [](float, float) {return 3.0f;}); + orig.rebuild_geometry_accels(); + const int64_t chi = euler(orig); + NavMap nm = orig; + navmap::simplify(nm); + EXPECT_LT(nm.navcels.size(), orig.navcels.size() / 5); + EXPECT_EQ(euler(nm), chi); // annulus + disc + EXPECT_NEAR(area(nm), 15.0f + 16.0f, 1e-3f); + ASSERT_EQ(nm.surfaces.size(), 2u); + for (std::size_t s = 0; s < 2; ++s) { + for (const NavCelId c : nm.surfaces[s].navcels) { + ASSERT_NEAR(nm.navcel_centroid(c).z(), s == 0 ? 0.0f : 3.0f, 1e-5f); + } + } + std::size_t s; + NavCelId c; + Eigen::Vector3f bary; + EXPECT_TRUE(nm.locate_navcel(Eigen::Vector3f(2.0f, 2.0f, 3.1f), s, c, bary, nullptr)); + EXPECT_EQ(s, 1u); + EXPECT_TRUE(nm.locate_navcel(Eigen::Vector3f(0.5f, 0.5f, 0.1f), s, c, bary, nullptr)); + EXPECT_EQ(s, 0u); + // In the stairwell the ground floor is still missing + Eigen::Vector3f hit; + float t; + ASSERT_TRUE(nm.raycast({2.0f, 2.0f, 2.0f}, {0, 0, -1.0f}, c, t, hit) == false); +} + +TEST(Simplify, LinkEndsSurviveAndAreRemapped) +{ + NavMap nm; + add_grid(nm, 20, 20, 0.1f, flat); + add_grid(nm, 20, 20, 0.1f, [](float, float) {return 3.0f;}); + nm.rebuild_geometry_accels(); + std::size_t s; + NavCelId a, b; + Eigen::Vector3f bary; + ASSERT_TRUE(nm.locate_navcel(Eigen::Vector3f(1.03f, 1.02f, 0.1f), s, a, bary, nullptr)); + ASSERT_TRUE(nm.locate_navcel(Eigen::Vector3f(1.03f, 1.02f, 3.1f), s, b, bary, nullptr)); + const auto ca = nm.navcel_centroid(a), cb = nm.navcel_centroid(b); + nm.add_link(a, b, navmap::LinkType::ELEVATOR, 3.0f, false); + navmap::simplify(nm); + EXPECT_LT(nm.navcels.size(), 1600u / 4); + ASSERT_EQ(nm.links().size(), 1u); + const auto & l = nm.links()[0]; + EXPECT_TRUE(nm.navcel_centroid(l.from).isApprox(ca, 1e-5f)); + EXPECT_TRUE(nm.navcel_centroid(l.to).isApprox(cb, 1e-5f)); + EXPECT_EQ(l.type, navmap::LinkType::ELEVATOR); + EXPECT_FLOAT_EQ(l.length, 3.0f); + EXPECT_FALSE(l.bidirectional); + EXPECT_EQ(nm.links_from(l.from).size(), 1u); +} + +TEST(Simplify, OnlyTheChosenLayersAreKept) +{ + NavMap orig = occupancy_floor(); + auto noise = orig.add_layer("noise", "", "", 0.0f); + for (NavCelId c = 0; c < orig.navcels.size(); ++c) { + noise->data()[c] = static_cast(c % 7); + } + NavMap all = orig, chosen = orig; + navmap::simplify(all); + SimplifyParams p; + p.layers = {"occupancy"}; + navmap::simplify(chosen, p); + EXPECT_GT(all.navcels.size(), orig.navcels.size() * 9 / 10); // noise everywhere: nothing + EXPECT_LT(chosen.navcels.size(), orig.navcels.size() / 5); + expect_same_values(orig, chosen, "occupancy", centroids(orig)); + EXPECT_EQ(chosen.layer_size("noise"), chosen.navcels.size()); // carried, values not kept + // The same, by leaving "noise" out of all the layers + NavMap ignored = orig; + SimplifyParams q; + q.ignore_layers = {"noise"}; + navmap::simplify(ignored, q); + EXPECT_EQ(ignored.navcels.size(), chosen.navcels.size()); + expect_same_values(orig, ignored, "occupancy", centroids(orig)); + expect_same_values(orig, ignored, "friction", centroids(orig)); +} + +TEST(Simplify, ColorsFollowTheVertices) +{ + NavMap nm; + add_grid(nm, 10, 10, 0.1f, flat); + nm.colors.emplace(); + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + const auto x = static_cast(std::lround(nm.positions.x[i] * 100.0f)); + nm.colors->r.push_back(x); + nm.colors->g.push_back(0); + nm.colors->b.push_back(0); + nm.colors->a.push_back(255); + } + nm.rebuild_geometry_accels(); + navmap::simplify(nm); + ASSERT_TRUE(nm.colors.has_value()); + ASSERT_EQ(nm.colors->r.size(), nm.positions.size()); + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + EXPECT_EQ(nm.colors->r[i], std::lround(nm.positions.x[i] * 100.0f)); + } +} + +TEST(Simplify, DegenerateInputsAreSafe) +{ + NavMap empty; + EXPECT_EQ(navmap::simplify(empty).navcels_after, 0u); + // One NavCel: nothing to remove + NavMap one; + const auto s = one.create_surface("map"); + one.add_navcel_to_surface( + s, one.add_navcel(one.add_vertex({0, 0, 0}), one.add_vertex({1, 0, 0}), + one.add_vertex({0, 1, 0}))); + one.rebuild_geometry_accels(); + navmap::simplify(one); + EXPECT_EQ(one.navcels.size(), 1u); + // A non-manifold edge (three NavCels on it) and an unused vertex + NavMap fin; + add_grid(fin, 6, 6, 0.1f, flat); + const auto s2 = fin.create_surface("map"); + const uint32_t top = fin.add_vertex({0.3f, 0.3f, 0.5f}); + fin.add_vertex({9, 9, 9}); + fin.add_navcel_to_surface(s2, fin.add_navcel(3 * 7 + 3, 3 * 7 + 4, top)); + fin.rebuild_geometry_accels(); + ASSERT_GT(fin.nonmanifold_edges(), 0u); + const float a0 = area(fin); + EXPECT_NO_THROW(navmap::simplify(fin)); + EXPECT_NEAR(area(fin), a0, 1e-4f); + EXPECT_GT(fin.nonmanifold_edges(), 0u); // the fin is still there + for (const auto & c : fin.navcels) { + for (const auto v : c.v) { + ASSERT_LT(v, fin.positions.size()); + } + } +} diff --git a/navmap_tools/CMakeLists.txt b/navmap_tools/CMakeLists.txt index 602ceb5..bebce57 100644 --- a/navmap_tools/CMakeLists.txt +++ b/navmap_tools/CMakeLists.txt @@ -45,9 +45,16 @@ target_link_libraries(navmap_publisher ${navmap_ros_interfaces_TARGETS} ) +add_executable(navmap_simplify src/navmap_simplify.cpp) +target_link_libraries(navmap_simplify + navmap_core::navmap_core + navmap_ros::navmap_ros +) + install(TARGETS pointcloud_to_navmap navmap_publisher + navmap_simplify DESTINATION lib/${PROJECT_NAME} ) diff --git a/navmap_tools/src/navmap_simplify.cpp b/navmap_tools/src/navmap_simplify.cpp new file mode 100644 index 0000000..6d942b0 --- /dev/null +++ b/navmap_tools/src/navmap_simplify.cpp @@ -0,0 +1,106 @@ +// 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. + +// Simplifies a .navmap keeping its layers (navmap_core/Simplify.hpp): +// navmap_simplify in.navmap out.navmap [--max-error m] [--max-edge m] [--min-quality q] +// [--max-normal-change deg] [--layer name]... [--ignore-layer name]... + +#include +#include +#include +#include +#include +#include + +#include "navmap_core/Simplify.hpp" +#include "navmap_ros/navmap_io.hpp" + +namespace +{ + +int usage() +{ + std::fprintf( + stderr, + "usage: navmap_simplify in.navmap out.navmap [--max-error m] [--max-edge m]\n" + " [--min-quality q] [--max-normal-change deg] [--layer name]...\n" + " [--ignore-layer name]...\n" + " Merges NavCels where the surface is flat (within --max-error) and every layer (or the\n" + " --layer ones, minus the --ignore-layer ones) has the same value. Borders between\n" + " values and open edges are kept. Ignored layers keep the value of the surviving NavCel,\n" + " except \"elevation\", recomputed as the mean height of each NavCel.\n"); + return 2; +} + +} // namespace + +int main(int argc, char ** argv) +{ + if (argc < 3) {return usage();} + const std::string in = argv[1], out = argv[2]; + navmap::SimplifyParams p; + for (int i = 3; i < argc; ++i) { + const std::string a = argv[i]; + if (i + 1 >= argc) {return usage();} + char * end = nullptr; + const char * v = argv[++i]; + const float f = std::strtof(v, &end); + const bool num = end != v && *end == '\0'; + if (a == "--max-error" && num && f >= 0.0f) { + p.max_error = f; + } else if (a == "--max-edge" && num && f > 0.0f) { + p.max_edge_length = f; + } else if (a == "--min-quality" && num && f >= 0.0f && f <= 1.0f) { + p.min_quality = f; + } else if (a == "--max-normal-change" && num && f >= 0.0f && f <= 180.0f) { + p.max_normal_change = f; + } else if (a == "--layer") { + p.layers.emplace_back(v); + } else if (a == "--ignore-layer") { + p.ignore_layers.emplace_back(v); + } else { + return usage(); + } + } + navmap::NavMap nm; + std::error_code ec; + if (!navmap_ros::io::load_from_file(in, nm, &ec)) { + std::fprintf(stderr, "cannot load %s: %s\n", in.c_str(), ec.message().c_str()); + return 1; + } + const auto t0 = std::chrono::steady_clock::now(); + const auto st = navmap::simplify(nm, p); + const double ms = std::chrono::duration( + std::chrono::steady_clock::now() - t0).count(); + // from_points' "elevation" is the mean height of the NavCel: recompute it if it was ignored + if (nm.has_layer("elevation") && nm.layer_type_name("elevation") == "float" && + std::find(p.ignore_layers.begin(), p.ignore_layers.end(), "elevation") != p.ignore_layers.end()) + { + for (navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + nm.layer_set("elevation", c, nm.navcel_centroid(c).z()); + } + } + if (!navmap_ros::io::save_to_file(nm, out, {}, &ec)) { + std::fprintf(stderr, "cannot save %s: %s\n", out.c_str(), ec.message().c_str()); + return 1; + } + std::printf( + "NavCels %zu -> %zu (%.1f %%), vertices %zu -> %zu, %.0f ms; file %s: %ju bytes\n", + st.navcels_before, st.navcels_after, + st.navcels_before ? 100.0 * st.navcels_after / st.navcels_before : 0.0, + st.vertices_before, st.vertices_after, ms, out.c_str(), + static_cast(std::filesystem::file_size(out))); + return 0; +}