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

navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp

@@ -14,6 +14,8 @@
 // limitations under the License.
 
 
+#include <OgreRoot.h>
+#include <OgreRenderSystem.h>
 #include <algorithm>
 #include <cmath>
 
@@ -55,6 +57,12 @@ inline void hsv2rgb(float H, float S, float V, float & R, float & G, float & B)
   R = r1 + m; G = g1 + m; B = b1 + m;
 }
 
+inline Ogre::VertexElementType nativeColourType_()
+{
+  const auto * rs = Ogre::Root::getSingleton().getRenderSystem();
+  return rs ? rs->getColourVertexElementType() : Ogre::VET_COLOUR_ARGB;
+}
+
 inline Ogre::ColourValue colorFromRainbow(float value, float max_value, float alpha)
 {
   if (max_value <= 1e-9f) {
@@ -639,7 +647,9 @@ void NavMapDisplay::ensureMeshBuilt_()
   while (bind->isBufferBound(COLOR_SRC)) {++COLOR_SRC;}
 
   // Create the dynamic colour buffer (one 32-bit colour per vertex)
-  const Ogre::VertexElementType col_type = Ogre::VET_COLOUR_ARGB;  // we'll pack ARGB
+  // the render system's own packing (ABGR with OpenGL, ARGB with D3D): a fixed ARGB swaps red and
+  // blue where it is not the native one
+  const Ogre::VertexElementType col_type = nativeColourType_();
   decl->addElement(COLOR_SRC, /*offset=*/0, col_type, Ogre::VES_DIFFUSE);
 
   Ogre::HardwareVertexBufferSharedPtr colour_vbuf =
@@ -748,8 +758,8 @@ void NavMapDisplay::updateColorsOnly_()
   uint32_t * p = reinterpret_cast<uint32_t *>(base);
 
   auto packARGB = [&](const Ogre::ColourValue & c) -> uint32_t {
-    // Pack into ARGB to match VET_COLOUR_ARGB used at creation
-      return Ogre::VertexElement::convertColourValue(c, Ogre::VET_COLOUR_ARGB);
+    // Pack in the render system's colour format, the one the buffer was created with
+      return Ogre::VertexElement::convertColourValue(c, nativeColourType_());
     };
 
   if (vertex_color_mode) {

navmap_tools/CMakeLists.txt

@@ -28,15 +28,26 @@ install(PROGRAMS
 )
 
 # ---------------- C++ tools ----------------
-add_executable(pointcloud_to_navmap src/pointcloud_to_navmap.cpp)
-target_include_directories(pointcloud_to_navmap PRIVATE ${PCL_INCLUDE_DIRS})
-target_link_libraries(pointcloud_to_navmap
+# Streaming point cloud -> NavMap mesher (several floors, voxel reduction, hole closing)
+add_library(navmap_tools_cloud STATIC
+  src/pcd_stream.cpp
+  src/voxel_cloud.cpp
+  src/floor_holes.cpp
+  src/cloud_to_navmap.cpp)
+set_target_properties(navmap_tools_cloud PROPERTIES POSITION_INDEPENDENT_CODE ON)
+target_include_directories(navmap_tools_cloud PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  ${PCL_INCLUDE_DIRS})
+target_link_libraries(navmap_tools_cloud PUBLIC
   navmap_core::navmap_core
   navmap_ros::navmap_ros
   ${navmap_ros_interfaces_TARGETS}
   ${PCL_LIBRARIES}
 )
 
+add_executable(pointcloud_to_navmap src/pointcloud_to_navmap.cpp)
+target_link_libraries(pointcloud_to_navmap navmap_tools_cloud)
+
 add_executable(navmap_publisher src/navmap_publisher.cpp)
 target_link_libraries(navmap_publisher
   rclcpp::rclcpp
@@ -66,6 +77,12 @@ if(BUILD_TESTING)
   endif()
   ament_lint_auto_find_test_dependencies()
 
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_cloud_to_navmap test/test_cloud_to_navmap.cpp)
+  target_link_libraries(test_cloud_to_navmap navmap_tools_cloud)
+  ament_add_gtest(test_floor_holes test/test_floor_holes.cpp)
+  target_link_libraries(test_floor_holes navmap_tools_cloud)
+
   find_package(ament_cmake_pytest REQUIRED)
   ament_add_pytest_test(navmap_tools_pytest test
     WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}

navmap_tools/include/navmap_tools/cloud_to_navmap.hpp

@@ -0,0 +1,109 @@
+// Copyright 2026 José Miguel Guerrero
+//
+// 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.
+
+#ifndef NAVMAP_TOOLS__CLOUD_TO_NAVMAP_HPP_
+#define NAVMAP_TOOLS__CLOUD_TO_NAVMAP_HPP_
+
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "navmap_tools/voxel_cloud.hpp"
+#include "navmap_ros_interfaces/msg/nav_map.hpp"
+
+namespace navmap_tools
+{
+
+struct Options
+{
+  /// The finest NavCel the map will have: the maximum resolution a consumer works from
+  /// (for bigmap, lengths.cell_size.min). Finer is wasted for the grid engine, which rasterises at
+  /// cell_size.min; coarser throws detail away for good.
+  float resolution{1.0f};
+  float max_slope_deg{30.0f};   ///< steeper triangles are not created (walls stay as gaps)
+  float max_dz{0.25f};          ///< vertical step of one surface / spread that means clutter
+  float min_clearance{1.0f};    ///< free height that separates two floors in a cell: the robot's
+  float max_hole_area{1.0f};    ///< enclosed empty regions up to this (m^2) are interpolated
+  /// A SLAM cloud has no points where it did not see the floor, and the mesher cannot invent them:
+  /// a map of holes, and ground the robot could drive over is not connected. A stretch of empty
+  /// cells up to this wide (m) between two bits of the same floor, in a row or a column, gets floor
+  /// points at the height in between and the cloud is meshed again (0: leave the holes)
+  float fill_holes{2.0f};
+  float min_surface_area{0.0f};  ///< pieces of the map smaller than this (m^2) are dropped
+  int max_surfaces{0};          ///< keep only this many pieces, the largest (0: all)
+
+  /// Voxel size (m) of the pre-reduction: < 0 automatic (none up to `raw_below` points, else
+  /// resolution / 4: 16 voxels per cell, so only a cloud denser than that is reduced and each
+  /// cell keeps enough points for the mesher's surface fit), 0 none.
+  float voxel{-1.0f};
+  std::size_t raw_below{5000000};
+  std::uint32_t min_voxel_points{1};   ///< 2 or more also drops isolated noise points
+
+  /// Only what is inside this window (x, y) and these heights is used
+  bool crop{false};
+  double xmin{0}, ymin{0}, xmax{0}, ymax{0};
+  double zmin{-std::numeric_limits<double>::infinity()};
+  double zmax{std::numeric_limits<double>::infinity()};
+
+  /// Shift x, y so that the centre of the cloud's bounding box is (0, 0): a consumer that
+  /// rasterises a map around its origin needs a grid centred on it, so a map far from it would
+  /// waste memory. The shift is in the report.
+  bool recenter{false};
+
+  std::string frame_id{"map"};
+  std::function<void(const std::string &)> log;   ///< progress; empty: silent
+};
+
+struct Report
+{
+  std::size_t points_in_files{0}, points_used{0};
+  bool voxelized{false};
+  float voxel{0.0f};
+  std::size_t voxels{0}, points_meshed{0}, dropped{0};
+  std::size_t filled_cells{0};   ///< cells of floor added over holes (fill_holes)
+  /// Points per occupied cell of `resolution` (median) and the finest resolution the data
+  /// supports with at least 3 points per cell
+  double median_points_per_cell{0.0};
+  double supported_resolution{0.0};
+  std::size_t vertices{0}, triangles{0};
+  bool coloured{false};
+  bool organised{false};
+  double offset_x{0.0}, offset_y{0.0};   ///< what --recenter added to x, y (subtract it to go back)
+  double seconds{0.0};
+  /// Heights (m) with surface in the map: the floors, from the lowest (vertices per 0.5 m band)
+  std::vector<std::pair<float, std::size_t>> levels;
+  std::vector<std::string> warnings;
+};
+
+/// The NavMap of one or several .pcd files (their points are joined) at `opt.resolution`.
+/// Streams the files, reduces by voxel when they are big, meshes with navmap_ros::from_points
+/// (several floors, slopes, clutter) and gives the mesh the colour of the points. Throws
+/// std::runtime_error with the reason when nothing can be built.
+Report cloud_to_navmap(
+  const std::vector<std::string> & inputs, const Options & opt,
+  navmap_ros_interfaces::msg::NavMap & out);
+
+/// Colours the vertices of `msg` from the coloured points (nearest in 3D among the ones of the
+/// vertex's cell and its neighbours, within a height tolerance; the rest take the nearest height).
+void colour_vertices(
+  navmap_ros_interfaces::msg::NavMap & msg, const PointSet & points, float resolution,
+  float height_tolerance);
+
+}  // namespace navmap_tools
+
+#endif  // NAVMAP_TOOLS__CLOUD_TO_NAVMAP_HPP_

navmap_tools/include/navmap_tools/floor_holes.hpp

@@ -0,0 +1,40 @@
+// Copyright 2026 José Miguel Guerrero
+//
+// 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.
+
+#ifndef NAVMAP_TOOLS__FLOOR_HOLES_HPP_
+#define NAVMAP_TOOLS__FLOOR_HOLES_HPP_
+
+#include <vector>
+
+#include "navmap_core/NavMap.hpp"
+
+namespace navmap_tools
+{
+
+/// A flat cell of floor: its centre and height
+struct FloorCell
+{
+  float x, y, z;
+};
+
+/// The cells (of `cell` m) that close the holes of a NavMap's floors: an empty stretch up to
+/// `max_width` m wide between two bits of the same floor, in a row or a column, with the height
+/// in between (a ramp across the hole stays a ramp). A straight border does not grow: a bridge
+/// needs floor at both ends. Floors are told apart by height, so the hole of the floor below
+/// another one is closed too.
+std::vector<FloorCell> floor_hole_cells(const ::navmap::NavMap & map, float cell, float max_width);
+
+}  // namespace navmap_tools
+
+#endif  // NAVMAP_TOOLS__FLOOR_HOLES_HPP_

navmap_tools/include/navmap_tools/pcd_stream.hpp

@@ -0,0 +1,59 @@
+// Copyright 2026 José Miguel Guerrero
+//
+// 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.
+
+#ifndef NAVMAP_TOOLS__PCD_STREAM_HPP_
+#define NAVMAP_TOOLS__PCD_STREAM_HPP_
+
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <string>
+#include <vector>
+
+namespace navmap_tools
+{
+
+/// What the header of a .pcd says
+struct PcdInfo
+{
+  std::size_t points{0};
+  std::size_t width{0}, height{1};
+  std::string data;          ///< ascii | binary | binary_compressed
+  bool has_color{false};     ///< an rgb / rgba field, or separate r, g, b
+  bool streamed{false};      ///< read in chunks (ascii, binary); compressed files are loaded whole
+};
+
+/// A block of points. `rgb` has 3 bytes per point, or is empty when the file has no colour.
+struct PcdChunk
+{
+  std::vector<float> xyz;
+  std::vector<std::uint8_t> rgb;
+  std::size_t size() const {return xyz.size() / 3;}
+};
+
+/// Reads the header only.
+PcdInfo pcd_info(const std::string & path);
+
+/// Streams the points of a .pcd to `sink` in blocks of at most `chunk_points`. ascii and binary
+/// files are never held whole in memory (a 100 million point file costs one block); the
+/// binary_compressed format (LZF over the whole table) cannot be streamed and is loaded by PCL.
+/// Points with a NaN coordinate are skipped. Throws std::runtime_error on an unreadable file or
+/// one without x, y, z.
+PcdInfo read_pcd(
+  const std::string & path, const std::function<void(const PcdChunk &)> & sink,
+  std::size_t chunk_points = 1u << 20);
+
+}  // namespace navmap_tools
+
+#endif  // NAVMAP_TOOLS__PCD_STREAM_HPP_

navmap_tools/include/navmap_tools/voxel_cloud.hpp

@@ -0,0 +1,69 @@
+// Copyright 2026 José Miguel Guerrero
+//
+// 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.
+
+#ifndef NAVMAP_TOOLS__VOXEL_CLOUD_HPP_
+#define NAVMAP_TOOLS__VOXEL_CLOUD_HPP_
+
+#include <cstddef>
+#include <cstdint>
+#include <unordered_map>
+#include <vector>
+
+namespace navmap_tools
+{
+
+/// Points with the colour (3 bytes each, or empty) and how many raw points each one stands for.
+struct PointSet
+{
+  std::vector<float> xyz;
+  std::vector<std::uint8_t> rgb;
+  std::vector<std::uint32_t> count;
+  std::size_t size() const {return xyz.size() / 3;}
+  bool coloured() const {return !rgb.empty();}
+};
+
+/// Streaming voxel grid: a hash of occupied voxels, each holding the sum of its points. Memory
+/// grows with the number of occupied voxels (the extent of the map over the voxel size), not with
+/// the number of points: a hundred million points over a 1 km^2 floor are a few million voxels.
+/// A voxel is replaced by the centroid of its points, which keeps planes planes and the vertical
+/// spread of clutter (what the mesher fits and tests), unlike keeping an arbitrary point.
+class VoxelCloud
+{
+public:
+  VoxelCloud(float voxel_xy, float voxel_z);
+
+  /// `rgb`: 3 bytes, or nullptr for a point without colour. False if the voxel index overflows.
+  bool add(float x, float y, float z, const std::uint8_t * rgb);
+
+  std::size_t voxels() const {return grid_.size();}
+  std::size_t dropped() const {return dropped_;}
+
+  /// Centroids of the voxels with at least `min_points` points
+  PointSet centroids(std::uint32_t min_points = 1) const;
+
+private:
+  struct Acc
+  {
+    float sx{0}, sy{0}, sz{0};            // offsets from the voxel corner, summed
+    std::uint32_t n{0}, r{0}, g{0}, b{0};
+  };
+  float vx_, vz_;
+  bool coloured_{false};
+  std::size_t dropped_{0};
+  std::unordered_map<std::uint64_t, Acc> grid_;
+};
+
+}  // namespace navmap_tools
+
+#endif  // NAVMAP_TOOLS__VOXEL_CLOUD_HPP_