[jmguerreroh]

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]

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.

[fmrico]

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.

[fmrico]

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?

[fmrico]

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.

[fmrico]

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.

[fmrico]

_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.

[fmrico]

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.

[fmrico]

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.

[fmrico]

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?

[fmrico]

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.