navmap_tools: DEM files, Gazebo terrain, big point clouds; RViz colours by jmguerreroh · Pull Request #43 · EasyNavigation/NavMap

jmguerreroh · 2026-11-03T22:43:24Z

navmap_tools: DEM files, Gazebo terrain, big point clouds; RViz colours

What the BigMap maps manager (easynav_bigmap_maps_manager, easynav_plugins) needs to get its map
outdoors, and the tools used to make its maps.

navmap_gis_tool

--dem-out FILE: the DEM of a place as a binary grid file (geo/dem_file.py: header with the
origin, the reference altitude, the resolution and the source), which the maps manager reads.
--dem-data-resolution samples the source at its own spacing and interpolates it bicubically;
--dem-source ign | copernicus30 | mesh | synthetic (geo/ign.py: IGN MDT, Spain, 5 m;
geo/mesh_dem.py: a DEM rasterised from a mesh, e.g. the collision mesh of a Gazebo world). A DEM
made with the same configuration is reused.
Gazebo terrain (geo/terrain_export.py): --terrain-out + --world-template /
--world-out write the terrain mesh (collision STL, textured OBJ) and a world from a template
(@latitude@, @longitude@, @elevation@, @TERRAIN_DIR@).
Vectorised sampling (sample_array) of the Copernicus grid, the tile mosaic and the PNOA
orthophoto, which the DEM and terrain export sample over whole grids.

pointcloud_to_navmap

Streams ascii/binary PCD files block by block and reduces clouds above 5 M points by voxel
(pcd_stream, voxel_cloud), so memory follows the extent of the map and not the number of points;
several --input; --fill-holes closes the holes a SLAM cloud leaves in its floors
(floor_holes); --min-surface-area, --max-surfaces; colour from the cloud's rgb; it says
which resolution the data holds. The meshing is navmap_ros::from_points.

navmap_rviz_plugin

NavMapDisplay packed the vertex colours as ARGB whatever the render system: with OpenGL (ABGR)
red and blue were swapped. It now uses the render system's own colour format.

Tests

colcon test --packages-select navmap_tools: new tests for the DEM file (round trip, a golden
file, the headers the C++ reader of the maps manager refuses), the IGN and mesh sources, the reuse
of a DEM made with the same configuration, the terrain export and the C++ cloud tools: 562 tests,
no failures.

Independent of the navmap_core / navmap_ros PR (DistanceField, set_area, operator=, navmap_io);
neither needs the other.

fmrico

Thanks, the new tools look useful and the RViz colour fix is right. Requesting changes mainly for the first four comments: an out-of-bounds read with mixed coloured/uncoloured inputs, a Gazebo terrain shifted against the DEM and the NavMap, a change of the default behaviour of pointcloud_to_navmap for plain clouds, and options silently ignored on organised clouds. The rest are smaller.

fmrico · 2026-11-05T17:47:40Z

+            } else {
+              raw.xyz.insert(raw.xyz.end(), {x, y, z});
+              raw.count.push_back(1);
+              if (col) {raw.rgb.insert(raw.rgb.end(), {rgb[0], rgb[1], rgb[2]});}


With several --input files, some with rgb and some without, raw.rgb only grows for the coloured points, so it no longer lines up with raw.xyz. E.g. a.pcd with rgb and b.pcd without (under 5 M points in total): pts.coloured() is true, pts.rgb.size() is 3*|a| but pts.size() is |a|+|b|, and colour_vertices reads pts.rgb[3*k] past the end. With the uncoloured file first, colours are shifted onto the wrong points.

The voxel path has a milder form: VoxelCloud divides the colour sums by a.n, which also counts uncoloured points, so mixed voxels come out darker. A default colour (or a separate colour count) for points without rgb would fix both.

Fixed both paths. Raw path: once a point has colour, the points without one (before and after it, from any file) get a default grey (kNoColour), so rgb always lines up with xyz. Voxel path: VoxelCloud counts the coloured points per voxel separately and averages the colour over them only; a voxel with no coloured point gets the default grey. Tests: two files, one with rgb and one without, in both orders, raw and voxelised (each half keeps its colour), and a voxel unit test with mixed points.

fmrico · 2026-11-05T17:47:40Z

+    n = z.shape[0]
+    res = dem.resolution * stride
+    n_tex = dem.side
+    coords = (np.arange(n) - (n - 1) // 2) * res


When (side - 1) / stride is odd, the origin node is not among the subsampled ones, and centring with (n - 1) // 2 shifts the whole mesh by half a stride. With side = 251, resolution 2 m and stride 2, the nodes picked are at -250, -246, …, 250 but the coords come out as -248, …, 252: the collision and visual mesh are 2 m off the DEM and the NavMap in x and y. The tests use radius=8, where the ratio is always even.

Suggested: coords = (np.arange(n) * stride - (dem.side - 1) // 2) * dem.resolution, and a test with an odd ratio.

Fixed with your formula: coords = (np.arange(n) * stride - (dem.side - 1) // 2) * dem.resolution. New test with an odd ratio (side 11, stride 2): the mesh spans -5..5 m and every vertex has the DEM's height there. With the old formula it fails (-4 instead of -5).

fmrico · 2026-11-05T17:47:40Z

+  if (const int rc = parse_args(argc, argv, args)) {
+    return rc == 1 ? 0 : 2;
+  }
+  const bool whole = !args.colors.empty() ||


Every unorganised cloud now goes through run_streaming, even without any of the new options. There, Options::fill_holes defaults to 2.0, so pointcloud_to_navmap --input scan.pcd --output map.navmap now adds synthetic floor over gaps up to 2 m (e.g. at the edge of a stair well). It also always writes <map>.yaml with trunc, overwriting e.g. a Nav2 map.yaml with the same base name.

Could fill_holes default to 0 (opt-in), and the yaml be written only when asked for or when there is georeferencing?

Both opt-in now. fill_holes defaults to 0, so no synthetic floor unless --fill-holes is given. <map>.yaml is written only with --origin (georeferencing) or --recenter (the offset is needed to place the map back), and the tool prints its path when it does. A plain --input scan.pcd --output map.navmap writes only the map. Usage text updated.

fmrico · 2026-11-05T17:47:40Z

+
+  // An organised cloud (a grid, as navmap_tools' python side writes) has its connectivity: its
+  // own mesher, no reduction
+  if (infos.size() == 1 && infos[0].height > 1) {


main sends an organised cloud to the streaming path exactly when one of the streaming options is given (--crop, --recenter, --zmin/--zmax, --fill-holes, …), but here a single organised input goes to from_regular_grid, which ignores all of them. --crop … --recenter on a grid PCD reports success with an uncropped, uncentred map, and the yaml records offset 0. Either apply the options to the grid or refuse them with an error.

The options are applied now. options_the_grid_mesher_ignores(opt) lists the options from_regular_grid does not take (crop, recenter, zmin/zmax, fill_holes, voxel, the mesher parameters...). If any is set, the organised cloud is meshed as points, so they apply, and the log says why. Without them it still goes to the grid mesher. Test: an organised grid with --crop and --recenter comes out cropped, centred, and with the offset in the report.

fmrico · 2026-11-05T17:47:40Z

+      c.xyz.insert(c.xyz.end(), {x, y, z});
+      if (packed) {
+        std::uint32_t u;
+        if (h.type[ic] == 'F') {


For ASCII files with a packed rgb of TYPE F, the parsed number is taken as the float's bits. As far as I know, PCL's ASCII writer stores rgb as its uint32 integer value (pure red as 16711680), which this decodes as r=0x7F, g=0, b=0. Taking an integral value as the uint32 directly (and the bits only for a non-integral one) would read both kinds of file.

Done as you suggest: an integral value in the uint32 range is taken as the packed colour itself (PCL's ASCII writer), and only a non-integral one as the float's bits. Test with 16711680 (red), 4278255360 (green with alpha 255) and a printed float (0x0000ffff). The existing float-bits round trip still passes.

fmrico · 2026-11-05T17:47:40Z

+    return 0
+
+
+def _is_the_same(old, args, lat, lon, radius, reference, data_res) -> bool:


_is_the_same compares the geometry, the source id, whether there are holes and the zoom, but not which mesh file, holes mesh, --mesh-world-elevation, --mesh-cell or imagery source made the file. Rebuilding with --mesh-file site_v2.stl or --dem-holes-mesh buildings_v2.stl (same centre, size and resolution), or switching --imagery-source from esri to pnoa, prints "same configuration" and keeps the stale DEM. Recording a hash of those inputs in the header (or at least the file names and mtimes) would avoid it.

The DEM now records what made it, in <dem>.inputs.json next to it: the name and SHA-256 of the mesh file and of the holes mesh, --mesh-cell, --mesh-world-elevation, and the imagery source, zoom, URL or resolution. _is_the_same compares it, and a DEM without that record is rebuilt. I kept it out of the binary header because that format is also read by BigMap's C++ DemGrid, which refuses trailing data other than the cells block. Test: same centre, size and resolution with another mesh under the same name, another --mesh-world-elevation, or a missing record: all rebuilt.

fmrico · 2026-11-05T17:47:40Z

+        dem, lat, lon, radius=radius, resolution=args.resolution,
+        source=_source_id(args), reference_altitude=reference, data_resolution=data_res)
+    if imagery is not None:
+        add_imagery(grid, imagery, getattr(imagery, 'zoom', 1))


Next to --gazebo/--navmap, _write_dem always stores the imagery, even without --dem-imagery, and records the zoom as getattr(imagery, 'zoom', 1), while the DEM-only path compares with args.zoom. With PNOA imagery the stored zoom is 1, so a later DEM-only run with --dem-imagery sees a mismatch and rebuilds.

_write_dem now stores the imagery only with --dem-imagery, and records args.zoom, the value the DEM-only path compares with. Test: without --dem-imagery there are no colours; with it the zoom is the requested one, and a later DEM-only run with the same options reuses the file.

fmrico · 2026-11-05T17:47:40Z

+
+std::uint64_t pack2(int a, int b)
+{
+  return (static_cast<std::uint64_t>(a + kBias) << 21) | static_cast<std::uint64_t>(b + kBias);


Minor: pack2 does not check that the indices fit in 21 bits. A cloud far from the origin without --recenter (UTM, x ≈ 440 000 m at 0.25 m) gives indices past 2^20 that collide, and filled floor ends up at wrong places. Related: VoxelCloud drops points out of its range, but rep.dropped is never printed. A range check with a clear error (or a hint to use --recenter) would help.

floor_hole_cells now checks the 21-bit range and throws a clear error that suggests --recenter. VoxelCloud now indexes relative to the first point read, so a cloud in UTM is no longer dropped (the hole filling runs after --recenter, the voxel reduction before it, so --recenter alone would not have saved it). Points still out of range are reported as a warning, which pointcloud_to_navmap prints. Tests: a map 440 km from the origin gives the --recenter error; a voxel cloud at UTM coordinates keeps every point.

fmrico · 2026-11-05T17:47:40Z

+    needs_grid = args.gazebo or args.navmap
    n_per_side = round(args.size / args.resolution) + 1
-    if n_per_side * n_per_side > _MAX_GRID_POINTS:
+    if needs_grid and n_per_side * n_per_side > _MAX_GRID_POINTS:


Minor: --dem-out alone skips _MAX_GRID_POINTS, and the only limit left is side <= 20000. Near that, build_local_dem allocates several float64 arrays of ~400 M nodes (~25 GB) before failing, so the process is OOM-killed instead of getting a parser error. A limit on side_d² for this path too?

--dem-out is now checked before building: the memory it needs (about 44 bytes per node of the data and output grids, measured; 64 used for margin) against the memory available (MemAvailable). Too much is a parser error naming both figures. Test with the available memory patched.

fmrico · 2026-11-05T17:47:40Z

+    reference = args.reference_altitude
+    mesh = None
+    if args.dem_source == 'mesh':
+        mesh = MeshHeightfield.load_stl(args.mesh_file, args.mesh_cell)


Minor (performance): with --dem-source mesh and --navmap --dem-out, the STL is parsed and rasterised twice, here and in _load_dem, and the per-triangle Python loop dominates. Building the MeshHeightfield once and passing it to both would halve it.

The MeshHeightfield is now built once per run (cached and cleared at the start of each run(), so a changed file is read again) and shared by _load_dem and _dem_geometry. Test: both paths, one parse.

…in hole filling and yaml, organised clouds with options, ASCII rgb, DEM provenance, imagery zoom, index ranges, DEM memory check, STL parsed once)

navmap_tools: DEM files, Gazebo terrain, big point clouds; RViz colours

ebc7adf

fmrico requested changes Nov 5, 2026

View reviewed changes

navmap_tools: review fixes (mixed colour inputs, terrain offset, opt-…

0ee3386

…in hole filling and yaml, organised clouds with options, ASCII rgb, DEM provenance, imagery zoom, index ranges, DEM memory check, STL parsed once)

fmrico merged commit 4c23723 into EasyNavigation:rolling Nov 10, 2026
1 check passed

		return 0


		def _is_the_same(old, args, lat, lon, radius, reference, data_res) -> bool:

Conversation

jmguerreroh commented Nov 3, 2026

navmap_tools: DEM files, Gazebo terrain, big point clouds; RViz colours

navmap_gis_tool

pointcloud_to_navmap

navmap_rviz_plugin

Tests

Uh oh!

fmrico left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Uh oh!

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants