Representation improvements: unknown space, incremental updates, descriptors, links, compact maps

README.md

@@ -28,6 +28,8 @@ 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.
 
 ---
 
@@ -227,6 +229,124 @@ nm.set_area<uint8_t>(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_core/Iterators.hpp>
+
+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<uint8_t>("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.
+
+---
+
+## 7c. Unknown values and log-odds occupancy (`Occupancy.hpp`)
+
+```cpp
+#include <navmap_core/Occupancy.hpp>
+
+nm.add_layer<float>("elevation", "terrain height", "m", 0.0f);
+nm.set_layer_unknown("elevation", std::numeric_limits<double>::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`).
+
+---
+
+## 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_core/DistanceField.hpp>
+
+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.
+
+---
+
+## 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.
+
+---
+
+## 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]; /* ... */}
+```
+
+---
+
+## 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_core/Combine.hpp>
+
+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);
+```
+
+---
+
+## 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`:
@@ -244,22 +364,26 @@ navmap::NavMap nm2 = navmap_ros::from_msg(msg);
 
 ---
 
-## 9. Save and load from disk
+### Layer updates
 
-NavMap supports saving and loading using YAML + mesh files:
+`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.
 
-```cpp
-#include <navmap_ros/map_io.hpp>
+---
 
-// Save NavMap to disk
-navmap_ros::saveMapToFile(nm, "/tmp/navmap.yaml");
+## 9. Save and load from disk
 
-// Load NavMap from disk
-navmap::NavMap nm3 = navmap_ros::loadMapFromYaml("/tmp/navmap.yaml");
+```cpp
+#include <navmap_ros/navmap_io.hpp>
+
+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: 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
 
 For advanced control you can still access internal data directly:

navmap_core/CMakeLists.txt

@@ -15,6 +15,10 @@ find_package(Eigen3 REQUIRED)
 
 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
+  src/${PROJECT_NAME}/Simplify.cpp
 )
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

navmap_core/bench/navmap_bench.cpp

@@ -25,6 +25,8 @@
 #include <string>
 #include <vector>
 
+#include "navmap_core/DistanceField.hpp"
+#include "navmap_core/Iterators.hpp"
 #include "navmap_core/NavMap.hpp"
 
 using navmap::NavCelId;
@@ -157,5 +159,121 @@ 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<float> U(2.5f, std::max(2.6f, extent - 2.5f));
+    const int M = 2000;
+    std::vector<Eigen::Vector3f> pts(M);
+    std::vector<NavCelId> 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");
+  }
+
+  // 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<NavCelId> C(0, static_cast<NavCelId>(nm.navcels.size() - 1));
+    std::vector<NavCelId> 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<double, std::milli>(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<NavCelId> 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;
 }

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 <cstddef>
+#include <cstdint>
+#include <limits>
+#include <string>
+#include <vector>
+
+#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<NavCelId> 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<double>::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_