diff --git a/README.md b/README.md index c7d6f39..7df9c32 100644 --- a/README.md +++ b/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(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. + +--- + +## 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`). + +--- + +## 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. + +--- + +## 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::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 +--- -// 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::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: diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index f4f9dd8..0b3ae95 100644 --- a/navmap_core/CMakeLists.txt +++ b/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 $ diff --git a/navmap_core/bench/navmap_bench.cpp b/navmap_core/bench/navmap_bench.cpp index 00d3052..706565e 100644 --- a/navmap_core/bench/navmap_bench.cpp +++ b/navmap_core/bench/navmap_bench.cpp @@ -25,6 +25,8 @@ #include #include +#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 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"); + } + + // 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/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/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/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..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; @@ -139,6 +141,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/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/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 5c611c6..bbf94b6 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. @@ -140,9 +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, 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 }; /** @@ -188,8 +234,13 @@ 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 + { + return std::make_shared>(*this); + } ///@} }; @@ -237,6 +288,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. * @@ -310,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; } } @@ -340,6 +417,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 +514,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. @@ -460,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: @@ -503,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. * @@ -699,6 +809,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. * @@ -746,11 +880,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_all_changed(); } - layer_meta[name] = LayerMeta{description, unit, true}; + auto & meta = layer_meta[name]; + meta.description = description; + meta.unit = unit; + meta.per_cell = true; return v; } @@ -776,7 +915,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); } } @@ -789,7 +928,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 +961,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 +975,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 +991,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. @@ -880,7 +1032,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(); } /** @@ -965,7 +1117,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 +1138,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 +1174,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 @@ -1151,6 +1306,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/include/navmap_core/Occupancy.hpp b/navmap_core/include/navmap_core/Occupancy.hpp new file mode 100644 index 0000000..e0048a6 --- /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_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_changed(cid);} + } + + 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/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/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/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/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 85efaac..0286140 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; @@ -168,11 +170,21 @@ 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; } } layer_meta = other.layer_meta; + links_ = other.links_; + link_index_dirty_ = true; return *this; } @@ -185,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; @@ -203,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_; @@ -423,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]); @@ -932,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()); @@ -940,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); @@ -1101,17 +1120,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 { @@ -1173,6 +1181,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 { @@ -1192,6 +1241,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 +1255,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/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 e87418a..a23e218 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -52,3 +52,38 @@ 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} +) + +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} +) + +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} +) + +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_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_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); +} 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; 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_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) {})); + } + } +} 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_core/tests/test_links.cpp b/navmap_core/tests/test_links.cpp new file mode 100644 index 0000000..a793163 --- /dev/null +++ b/navmap_core/tests/test_links.cpp @@ -0,0 +1,155 @@ +// 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); +} + +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_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); +} 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_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/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/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index c0b6519..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 --------- @@ -150,6 +138,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 +163,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/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/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 0ae1cf4..dae0448 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" @@ -71,6 +72,114 @@ 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()); + lmsg.geometry_id = nm.geometry_id(); + 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_all_changed(); +} + +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) + { + 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,26 +226,24 @@ 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)); } + out.geometry_id = nm.geometry_id(); + + // 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; } @@ -194,20 +301,14 @@ 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); + } + + // 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(); @@ -233,30 +334,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; } @@ -267,41 +345,62 @@ 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, 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 +470,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..b1fc74c 100644 --- a/navmap_ros/src/navmap_ros/navmap_io.cpp +++ b/navmap_ros/src/navmap_ros/navmap_io.cpp @@ -16,6 +16,8 @@ #include +#include +#include #include #include #include @@ -26,15 +28,23 @@ #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" +#include "zlib.h" namespace navmap_ros::io { namespace { -// Magic header "NAVMAP\0" (7 chars + NUL), version uint32_t = 1, payload_size uint64_t (LE) +// 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'}; -static constexpr uint32_t kVersion = 1; +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) { @@ -46,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 = {};} @@ -82,12 +103,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,37 +163,75 @@ bool load_msg_from_file( ifs.read(magic, sizeof(magic)); if (!ifs) {set_error(ec); return false;} - uint32_t version = 0; - uint64_t payload_size = 0; - bool has_header = std::memcmp(magic, kMagic, sizeof(kMagic)) == 0; - if (has_header) { + // Files without header are the oldest format: version 1 + uint32_t version = 1; + 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 != kVersion) {set_error(ec); return false;} + 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); + } - // 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..12d3b09 --- /dev/null +++ b/navmap_ros/tests/test_layer_metadata_and_migration.cpp @@ -0,0 +1,537 @@ +// 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::NavCelId; +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); +} + +// ------------------------------------------------------------------ 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 + 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)); +} + +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"); +} + +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", 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) +{ + 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/CMakeLists.txt b/navmap_ros_interfaces/CMakeLists.txt index 5573f7c..c2f70a4 100644 --- a/navmap_ros_interfaces/CMakeLists.txt +++ b/navmap_ros_interfaces/CMakeLists.txt @@ -18,6 +18,9 @@ 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..dcbd5df 100644 --- a/navmap_ros_interfaces/msg/NavMap.msg +++ b/navmap_ros_interfaces/msg/NavMap.msg @@ -23,3 +23,9 @@ 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 20e7e50..61546ad 100644 --- a/navmap_ros_interfaces/msg/NavMapLayer.msg +++ b/navmap_ros_interfaces/msg/NavMapLayer.msg @@ -4,12 +4,27 @@ 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 + +# 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 +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/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_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/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 559eaeb..9f78c3e 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);} @@ -111,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.", @@ -129,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_); @@ -157,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); @@ -183,6 +210,7 @@ void NavMapDisplay::reset() destroyMesh_(); updateNormals_(); + updateLinks_(); } void NavMapDisplay::onEnable() @@ -246,6 +274,7 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg) ensureMeshBuilt_(); updateColorsOnly_(); updateNormals_(); + updateLinks_(); context_->queueRender(); } @@ -317,18 +346,67 @@ 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(); 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 +422,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 +701,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 +790,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 +811,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)); @@ -777,6 +888,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(); @@ -787,7 +949,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/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/navmap_tools/resample.py b/navmap_tools/navmap_tools/resample.py index d42a8f9..66bf7a3 100644 --- a/navmap_tools/navmap_tools/resample.py +++ b/navmap_tools/navmap_tools/resample.py @@ -34,32 +34,72 @@ import os import struct import sys +import zlib -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, int16 layers, links, geometry ids; compressed payload +CODEC_NONE, CODEC_ZLIB = 0, 1 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 +233,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 +272,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/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; +} diff --git a/navmap_tools/test/test_resample.py b/navmap_tools/test/test_resample.py index 1bd6d69..b03e85d 100644 --- a/navmap_tools/test/test_resample.py +++ b/navmap_tools/test/test_resample.py @@ -14,15 +14,17 @@ """Unit tests for navmap_tools.resample (grid-based NavMap resampling).""" +import struct 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): @@ -111,6 +113,39 @@ def test_the_file_header_is_the_navmap_one(tmp_path): assert path.read_bytes()[:8] == b'NAVMAP\x00\x00' +@pytest.mark.parametrize('compress', [True, False]) +def test_files_are_zlib_compressed_or_not_and_read_back(tmp_path, compress): + g = np.zeros((40, 40), np.uint8) + g[20, :] = 254 + path = tmp_path / 'm.navmap' + write_navmap(build_navmap(occ_layers(g), (0, 0, 0), 0.1, 'map'), str(path), compress) + data = path.read_bytes() + version, codec, raw, stored = struct.unpack('