NavMap

NavMap is an open-source C++ and ROS 2 library for representing navigable surfaces for mobile robot navigation and localization. Unlike classic grid-based maps, NavMap stores the environment as triangular meshes (NavCels), enabling efficient queries and multi-surface environments (e.g., multi-floor buildings).

---

Funded by the European Union through the Horizon Europe programme under Grant Agreement No. 101070254 (CoreSense), and by MICIU/AEI/10.13039/501100011033 and ERDF/EU under grants PERMAP PID2024-161761OB-C21 and PLANNAV PID2024-161761OB-C22 (AURORAS).

---

✨ Features

• Triangular cell mesh representation with adjacency relations.
• Dynamic runtime layers: per-cell or per-vertex attributes (occupancy, elevation, cost, traversability, etc.).
• 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.

---

📂 Repository structure

This repository is organized into several ROS 2 packages:

• navmap_core/
  Core C++ library implementing NavMap. Minimal dependencies (Eigen3).
• navmap_ros/
  ROS 2 conversions and message definitions:
  
  • navmap::NavMap ↔ navmap_ros_interfaces::msg::NavMap
    
  • nav_msgs::msg::OccupancyGrid ↔ navmap::NavMap
• navmap_rviz_plugin/
  RViz2 plugin for visualization of NavMap messages:
  
  • Displays surfaces and layers.
    
  • Optional per-cell normal rendering.
    
  • Layer-based coloring.
• navmap_tools/
  Tools and utilities for building and testing NavMaps (mesh import/export, conversions, etc.)
• navmap_examples/
  Practical examples demonstrating the usage of NavMap, both core C++ API and ROS 2 integrations.

---

⚙️ Build instructions

NavMap can be built as a standalone C++ library or within a ROS 2 workspace.

ROS 2 colcon build

# Clone into your ROS 2 workspace
cd ~/ros2_ws/src
git clone https://github.com/<your-org>/NavMap.git
 
# Build
cd ~/ros2_ws
colcon build --packages-up-to navmap_core navmap_ros navmap_rviz_plugin navmap_tools navmap_examples
 
# Source workspace
source install/setup.bash

Dependencies

• C++23 compiler
• Eigen3
• ROS 2 (tested with Humble, Iron, Jazzy)
• RViz2 (for the visualization plugin)
• PCL (for mesh construction utilities)

---

🚀 Usage (C++ API)

C++ API

This section shows small, self-contained snippets that demonstrate how to create a NavMap, add geometry, attach layers, query values, and locate the triangle (NavCel) corresponding to a 3D position.

Note: After modifying geometry (vertices, triangles, or surfaces), always call rebuild_geometry_accels() before performing queries such as locate_navcel() or raycast().

---

1. Create a minimal NavMap (single square floor with two triangles)

#include <navmap_core/NavMap.hpp>
#include <Eigen/Core>
 
using navmap::NavMap;
using navmap::NavCelId;
 
NavMap nm;
 
// Create a surface
std::size_t surf_idx = nm.create_surface("map");
 
// Add 4 vertices of a unit square (z=0)
uint32_t v0 = nm.add_vertex({0.f, 0.f, 0.f});
uint32_t v1 = nm.add_vertex({1.f, 0.f, 0.f});
uint32_t v2 = nm.add_vertex({1.f, 1.f, 0.f});
uint32_t v3 = nm.add_vertex({0.f, 1.f, 0.f});
 
// Add 2 triangles
NavCelId c0 = nm.add_navcel(v0, v1, v2);
NavCelId c1 = nm.add_navcel(v0, v2, v3);
 
// Assign them to the surface
nm.add_navcel_to_surface(surf_idx, c0);
nm.add_navcel_to_surface(surf_idx, c1);
 
// Build normals, adjacency, BVH, etc.
nm.rebuild_geometry_accels();

---

2. Add a per-NavCel layer (cost or occupancy)

// Add a layer of type float called "cost"
auto cost = nm.add_layer<float>("cost", "Traversal cost", "" , 0.0f);
 
// Assign values to each triangle
nm.layer_set<float>("cost", c0, 1.0f);
nm.layer_set<float>("cost", c1, 5.0f);

---

3. Read a layer value for a given NavCel

double v = nm.layer_get<double>("cost", c0);  // → 1.0

---

4. Locate the NavCel corresponding to a 3D position

size_t surf_idx;
NavCelId cid;
Eigen::Vector3f bary, hit;
 
bool ok = nm.locate_navcel(Eigen::Vector3f(0.5f, 0.5f, 0.1f),
                           surf_idx, cid, bary, &hit);
 
if (ok) {
  std::cout << "Point belongs to surface " << surf_idx
            << ", NavCel " << cid
            << " with barycentric coords " << bary.transpose() << std::endl;
}

---

5. Sample a layer at a world position

double val = nm.sample_layer_at("cost", Eigen::Vector3f(0.2f, 0.8f, 1.0f), -1.0);
if (!std::isnan(val)) {
  std::cout << "Cost at (0.2,0.8) is " << val << std::endl;
}

If the layer does not exist or the position cannot be located, the fallback value (-1.0 here) is returned.

---

6. Raycasting

NavCelId hit_cid;
float t;
Eigen::Vector3f hit_pt;
 
bool hit = nm.raycast(
  Eigen::Vector3f(0.5f, 0.5f, 2.0f),   // origin
  Eigen::Vector3f(0.0f, 0.0f, -1.0f),  // direction
  hit_cid, t, hit_pt);
 
if (hit) {
  std::cout << "Ray hit NavCel " << hit_cid
            << " at " << hit_pt.transpose() << std::endl;
}

---

7. Marking areas (set_area)

You can set values over regions of the map using shapes such as circular or rectangular areas:

// Add an occupancy layer
nm.add_layer<uint8_t>("occupancy", "Occupancy", "%", 0);
 
// Mark a circular area at (0.5,0.5) with radius 0.3
nm.set_area<uint8_t>(Eigen::Vector3f(0.5f, 0.5f, 0.0f),
                     (uint8_t)254,
                     "occupancy",
                     navmap::AreaShape::CIRCULAR,
                     0.3f);
 
// Mark a rectangular area centered at (0.8,0.2)
nm.set_area<uint8_t>(Eigen::Vector3f(0.8f, 0.2f, 0.0f),
                     (uint8_t)200,
                     "occupancy",
                     navmap::AreaShape::RECTANGULAR,
                     0.35f);

---

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.

#include <navmap_core/Iterators.hpp>
 
navmap::TraversalScratch scratch;
const Eigen::Vector3f robot(2.0f, 1.0f, 0.0f);
 
// NavCels within 0.5 m of the robot (3D distance to the centroid)
navmap::for_each_in_disc(nm, robot, 0.5f, [&](navmap::NavCelId c) {/* ... */},
  navmap::Metric::EUCLIDEAN, &scratch);
 
// NavCels crossed by a sensor ray projected on the surface, with the entry/exit parameters
navmap::for_each_on_segment(nm, robot, Eigen::Vector3f(6.0f, 3.0f, 0.0f),
  [&](navmap::NavCelId c, float t_in, float t_out) {/* clear c */});
 
// Nearest NavCel along the surface (geodesic order) with a free value
navmap::for_each_by_distance(nm, seed, robot, 3.0f, [&](navmap::NavCelId c, float d) {
    return nm.layer_get<uint8_t>("occupancy", c) != 0;  // false stops at the first free one
  });

Also for_each_in_aabb (every surface, via the BVH), for_each_in_ring and for_each_in_polygon (XY polygon on the seed's floor). Metric::GEODESIC measures along the surface and bends around borders; it is about 80 times costlier than EUCLIDEAN, so use it for occasional queries.

---

7c. Unknown values and log-odds occupancy (Occupancy.hpp)

#include <navmap_core/Occupancy.hpp>
 
nm.add_layer<float>("elevation", "terrain height", "m", 0.0f);
nm.set_layer_unknown("elevation", std::numeric_limits<double>::quiet_NaN());  // NaN = not surveyed
bool unseen = nm.is_unknown("elevation", cid);
 
navmap::OccupancyLayer occ(nm);   // int16 layer "occupancy_logodds", all unknown
occ.miss(crossed_cid);            // a ray crossed it
occ.hit(end_cid);                 // a ray ended on it
if (occ.state(cid) == navmap::OccupancyState::OCCUPIED) {/* ... */}

The unknown value, description and unit of each layer travel in the message and in the file. Which NavCels a ray hits or crosses is up to the algorithm using it (see for_each_on_segment). ros2 launch navmap_examples unknown_occupancy.launch.py shows both in RViz2 (build with -DBUILD_ROS_EXAMPLES=ON).

---

7d. Incremental updates (change tracking, DistanceField.hpp)

Layers record which NavCels changed since the last clear_changes() (layer_set, OccupancyLayer; bulk writes mark the whole layer), so a consumer only reprocesses what changed. DistanceField keeps a distance-to-seed field up to date from the seed cells added and removed:

#include <navmap_core/DistanceField.hpp>
 
navmap::DistanceField df(nm, /*from_boundary=*/true, /*max_distance=*/0.7f);
df.compute(lethal_cells);                       // once
df.update(newly_lethal, no_longer_lethal);      // every cycle: cost of the area that changes
for (navmap::NavCelId c : df.changed()) {/* recompute the cost of c from df[c] */}

With a finite max_distance the field is exact (straight distance to the nearest seed reaching the NavCel over the surface), and identical after any sequence of updates to a full recomputation.

---

7e. Geometric descriptors (Descriptors.hpp)

navmap::compute_descriptors(nm) adds F32 layers per NavCel: slope (deg), roughness (RMS distance to the plane fitted in a disc, m), step (height span in a small disc, m) and clearance (free height to the next surface of the map above, m; +inf if none). Discs grow over the surface, so floors never mix. They only describe the surface: weighing them into a cost is up to the planner.

---

7f. Links between surfaces (SurfaceLink)

Stairs, elevators or doors between NavCels that do not share an edge. The map only records them; whether a robot can take one, and at which cost, is up to the planner. They travel in the message and the file, and RViz2 draws them (Draw Links).

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.

#include <navmap_core/Combine.hpp>
 
navmap::CombineOptions o;
o.provenance = "source";
o.source_id = 1;
navmap::combine_layer(nm, "cost", "prior", navmap::CombineOp::TRUE_OVERWRITE, o);
o.source_id = 2;
o.cells = nm.layers.get("obstacles")->changed();
navmap::combine_layer(nm, "cost", "obstacles", navmap::CombineOp::MAX, o);

---

7h. Simplification that keeps the layers (Simplify.hpp)

navmap::simplify(nm, params) merges NavCels where the surface is flat (within max_error) and the layers have the same value, and keeps the original ones where they change: every layer keeps its value at every point, open edges keep their shape and the topology does not change (floors, holes, links). max_edge_length bounds the NavCel size. Layers that are per-cell geometry (elevation, support from from_points) go in ignore_layers.

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

---

7i. Clouds with sensor positions (from_points with viewpoints)

A cloud whose points keep the position of the sensor that saw them (pcl::PointWithViewpoint: vp_x, vp_y, vp_z) tells free space from occupied space, which a bare cloud cannot. from_points takes it and cleans the cloud before meshing (filter_with_viewpoints):

• Free-space carving (carve_free_space): points in voxels that rays went through more often than they hit them are dropped: people walking during the mapping, noise, spikes. Rays grazing a floor or running along a wall do not carve it.
• Faces seen from below (drop_seen_from_below): a ceiling or the underside of a slab is never a floor. Undersides over a floor (a table) stay, as they still take free height.

pcl::PointCloud<pcl::PointWithViewpoint> cloud;  // each scan: its points and the sensor position
navmap_ros::BuildParams p;
p.resolution = 0.2;
navmap_ros_interfaces::msg::NavMap msg;
navmap::NavMap nm = navmap_ros::from_points(cloud, msg, p);

The ray traversal runs in parallel with OpenMP when it is available. On simulated lidar over the playgrounds' parking garage (0.2 m), the area of the map that is not floor (ceilings seen from below) goes from 15.5 % to 1.0 %; on the scans of a real two-floor building, the route the robot drove lies on one surface, against six from the bare cloud.

---

8. Serialize to / from ROS messages

Conversion functions are provided in navmap_ros:

#include <navmap_ros/conversions.hpp>
#include <navmap_ros_interfaces/msg/nav_map.hpp>
 
// Convert to ROS message
navmap_ros_interfaces::msg::NavMap msg = navmap_ros::to_msg(nm);
 
// Convert back to core structure
navmap::NavMap nm2 = navmap_ros::from_msg(msg);

---

Layer updates

navmap_ros::to_layer_update_msg(nm, "obstacles") publishes only what changed since the layer's last clear_changes(): a sparse NavMapLayer (indices + values) when few NavCels changed, the full layer otherwise. Every NavMap and NavMapLayer message carries the map's geometry_id, so from_msg (and RViz2) reject updates meant for another geometry.

---

9. Save and load from disk

#include <navmap_ros/navmap_io.hpp>
 
navmap_ros::io::save_to_file(nm, "/tmp/map.navmap");     // zlib-compressed by default
navmap::NavMap nm3;
navmap_ros::io::load_from_file("/tmp/map.navmap", nm3);

---

File format. .navmap files are version 2: the CDR-serialized NavMap message (layer metadata, int16 layers, links, geometry id), zlib-compressed by default (SaveOptions::compression, level). zlib is available on every ROS distro and in the Python standard library. Version 1 files, and files without header, still load: they are migrated with a deprecation warning and rewritten in place when the location is writable.

10. Classic low-level API

For advanced control you can still access internal data directly:

// Access vertex positions
Eigen::Vector3f v = nm.positions.at(0);
 
// Iterate over NavCels
for (const auto & cel : nm.navcels) {
  Eigen::Vector3f centroid = nm.navcel_centroid(&cel - &nm.navcels[0]);
  std::cout << "NavCel area: " << cel.area
            << " centroid: " << centroid.transpose() << std::endl;
}

---

📦 Usage (Examples package)

In addition to the snippets above, the repository provides the package navmap_examples with ready-to-run executables.

Build

colcon build --packages-select navmap_examples

If your navmap_core / navmap_ros do not yet export CMake targets, you can provide include paths:

colcon build --packages-select navmap_examples   --cmake-args -DNAVMAP_CORE_INCLUDE_DIR=~/ros2_ws/src/NavMap/navmap_core/include                -DNAVMAP_ROS_INCLUDE_DIR=~/ros2_ws/src/NavMap/navmap_ros/include

Disable ROS 2 examples:

colcon build --packages-select navmap_examples --cmake-args -DBUILD_ROS_EXAMPLES=OFF

Run core examples

ros2 run navmap_examples 01_flat_plane
ros2 run navmap_examples 02_two_floors
ros2 run navmap_examples 03_slope_surface
ros2 run navmap_examples 04_layers
ros2 run navmap_examples 05_neighbors_and_centroids
ros2 run navmap_examples 06_area_marking
ros2 run navmap_examples 07_raycast
ros2 run navmap_examples 08_copy_and_assign

Each demonstrates a different feature: geometry creation, multi-surface, U8/F32 layers, centroids and neighbors, area marking, raycasting, and copy/assign semantics.

Run ROS 2 examples

If BUILD_ROS_EXAMPLES=ON:

ros2 run navmap_examples 01_from_occgrid
ros2 run navmap_examples 02_to_occgrid
ros2 run navmap_examples 03_save_load

Or via launch:

ros2 launch navmap_examples navmap_to_occgrid.launch.py

---

🧪 Testing

NavMap provides unit tests with GTest. To run them:

colcon test --packages-select navmap_core navmap_ros
colcon test-result --verbose

---

🤝 Contributing

Contributions are welcome! Please open issues and pull requests on GitHub.
Before submitting code, run the linters and tests:

colcon test
ament_lint_auto

---

📜 License

This project is licensed under the Apache 2 License.
See the [LICENSE](LICENSE) file for details.

---

🙏 Acknowledgements

Developed at the Intelligent Robotics Lab (Universidad Rey Juan Carlos).
Part of the Easy Navigation (EasyNav) project.