From ebc7adf2b433afb989bde781b36dbb06e6b06050 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 3 Nov 2026 23:37:38 +0100 Subject: [PATCH] navmap_tools: DEM files, Gazebo terrain, big point clouds; RViz colours --- .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 16 +- navmap_tools/CMakeLists.txt | 23 +- .../include/navmap_tools/cloud_to_navmap.hpp | 109 +++++ .../include/navmap_tools/floor_holes.hpp | 40 ++ .../include/navmap_tools/pcd_stream.hpp | 59 +++ .../include/navmap_tools/voxel_cloud.hpp | 69 +++ navmap_tools/navmap_tools/cli.py | 337 +++++++++++++-- navmap_tools/navmap_tools/geo/dem.py | 15 + navmap_tools/navmap_tools/geo/dem_file.py | 390 +++++++++++++++++ navmap_tools/navmap_tools/geo/ign.py | 203 +++++++++ navmap_tools/navmap_tools/geo/imagery.py | 56 ++- navmap_tools/navmap_tools/geo/mesh_dem.py | 225 ++++++++++ navmap_tools/navmap_tools/geo/pnoa.py | 8 + .../navmap_tools/geo/terrain_export.py | 133 ++++++ navmap_tools/src/cloud_to_navmap.cpp | 314 ++++++++++++++ navmap_tools/src/floor_holes.cpp | 134 ++++++ navmap_tools/src/pcd_stream.cpp | 315 ++++++++++++++ navmap_tools/src/pointcloud_to_navmap.cpp | 275 +++++++++--- navmap_tools/src/voxel_cloud.cpp | 83 ++++ navmap_tools/test/data/dem_golden.bin | Bin 0 -> 400 bytes navmap_tools/test/test_cli.py | 200 +++++++++ navmap_tools/test/test_cloud_to_navmap.cpp | 397 ++++++++++++++++++ navmap_tools/test/test_dem.py | 10 + navmap_tools/test/test_dem_file.py | 269 ++++++++++++ navmap_tools/test/test_floor_holes.cpp | 80 ++++ navmap_tools/test/test_ign.py | 146 +++++++ navmap_tools/test/test_imagery.py | 72 ++++ navmap_tools/test/test_mesh_dem.py | 151 +++++++ navmap_tools/test/test_terrain_export.py | 129 ++++++ 29 files changed, 4148 insertions(+), 110 deletions(-) create mode 100644 navmap_tools/include/navmap_tools/cloud_to_navmap.hpp create mode 100644 navmap_tools/include/navmap_tools/floor_holes.hpp create mode 100644 navmap_tools/include/navmap_tools/pcd_stream.hpp create mode 100644 navmap_tools/include/navmap_tools/voxel_cloud.hpp create mode 100644 navmap_tools/navmap_tools/geo/dem_file.py create mode 100644 navmap_tools/navmap_tools/geo/ign.py create mode 100644 navmap_tools/navmap_tools/geo/mesh_dem.py create mode 100644 navmap_tools/navmap_tools/geo/terrain_export.py create mode 100644 navmap_tools/src/cloud_to_navmap.cpp create mode 100644 navmap_tools/src/floor_holes.cpp create mode 100644 navmap_tools/src/pcd_stream.cpp create mode 100644 navmap_tools/src/voxel_cloud.cpp create mode 100644 navmap_tools/test/data/dem_golden.bin create mode 100644 navmap_tools/test/test_cloud_to_navmap.cpp create mode 100644 navmap_tools/test/test_dem_file.py create mode 100644 navmap_tools/test/test_floor_holes.cpp create mode 100644 navmap_tools/test/test_ign.py create mode 100644 navmap_tools/test/test_mesh_dem.py create mode 100644 navmap_tools/test/test_terrain_export.py diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index 9f78c3e..4d3ef6d 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -14,6 +14,8 @@ // limitations under the License. +#include +#include #include #include @@ -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(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) { diff --git a/navmap_tools/CMakeLists.txt b/navmap_tools/CMakeLists.txt index bebce57..b6bfe06 100644 --- a/navmap_tools/CMakeLists.txt +++ b/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 + $ + ${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} diff --git a/navmap_tools/include/navmap_tools/cloud_to_navmap.hpp b/navmap_tools/include/navmap_tools/cloud_to_navmap.hpp new file mode 100644 index 0000000..d68b801 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#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::infinity()}; + double zmax{std::numeric_limits::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 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> levels; + std::vector 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 & 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_ diff --git a/navmap_tools/include/navmap_tools/floor_holes.hpp b/navmap_tools/include/navmap_tools/floor_holes.hpp new file mode 100644 index 0000000..f0339dc --- /dev/null +++ b/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 + +#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 floor_hole_cells(const ::navmap::NavMap & map, float cell, float max_width); + +} // namespace navmap_tools + +#endif // NAVMAP_TOOLS__FLOOR_HOLES_HPP_ diff --git a/navmap_tools/include/navmap_tools/pcd_stream.hpp b/navmap_tools/include/navmap_tools/pcd_stream.hpp new file mode 100644 index 0000000..8068031 --- /dev/null +++ b/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 +#include +#include +#include +#include + +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 xyz; + std::vector 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 & sink, + std::size_t chunk_points = 1u << 20); + +} // namespace navmap_tools + +#endif // NAVMAP_TOOLS__PCD_STREAM_HPP_ diff --git a/navmap_tools/include/navmap_tools/voxel_cloud.hpp b/navmap_tools/include/navmap_tools/voxel_cloud.hpp new file mode 100644 index 0000000..0532aba --- /dev/null +++ b/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 +#include +#include +#include + +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 xyz; + std::vector rgb; + std::vector 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 grid_; +}; + +} // namespace navmap_tools + +#endif // NAVMAP_TOOLS__VOXEL_CLOUD_HPP_ diff --git a/navmap_tools/navmap_tools/cli.py b/navmap_tools/navmap_tools/cli.py index 712bb41..76e20fe 100644 --- a/navmap_tools/navmap_tools/cli.py +++ b/navmap_tools/navmap_tools/cli.py @@ -23,6 +23,8 @@ """ import argparse +import math +from pathlib import Path import subprocess import sys @@ -30,13 +32,20 @@ from .geo.cache import default_cache_dir, DiskCache from .geo.dem import load_dem_grid +from .geo.dem_file import ( + add_imagery, build_local_dem, DemSource, read_dem_file, SyntheticDem, SyntheticImagery, + write_dem_file, +) from .geo.google import ( API_KEY_ENV_VAR as GOOGLE_API_KEY_ENV_VAR, get_api_key as get_google_api_key, load_google_mosaic, ) +from .geo.ign import load_ign_dem from .geo.imagery import DEFAULT_ZOOM, load_imagery_mosaic, native_resolution_m_per_px +from .geo.mesh_dem import hole_cells, load_stl_triangles, MeshDem, MeshHeightfield from .geo.pnoa import DEFAULT_RESOLUTION_M as PNOA_DEFAULT_RESOLUTION_M, load_pnoa_mosaic from .geo.projection import LocalProjection +from .geo.terrain_export import write_gazebo_terrain, write_world_from_template from .mesh_export import write_dae, write_stl, write_texture_png from .pcd_writer import write_colors_csv, write_pcd_xyz from .scaffold import scaffold_world_package @@ -107,6 +116,43 @@ def build_arg_parser() -> argparse.ArgumentParser: help='side length (meters) of the square area centered on --center') parser.add_argument('--gazebo', action='store_true', help='emit a Gazebo world + models') parser.add_argument('--navmap', action='store_true', help='emit a .navmap file') + parser.add_argument( + '--dem-out', default=None, metavar='FILE', + help=( + 'write the elevation as the local height-grid file bigmap_maps_manager reads ' + "(BigMap's DemGrid: terrain.dem_file). Can be the only output; the grid is " + '--resolution metres, --size wide, centred on --center' + )) + parser.add_argument( + '--dem-data-resolution', type=float, default=0.0, metavar='METERS', + help=( + '--dem-out: spacing at which the source is sampled, bicubically interpolated to ' + '--resolution when coarser (0: what the source supports: 5 ign, 30 copernicus30)' + )) + parser.add_argument( + '--reference-altitude', type=float, default=None, metavar='METERS', + help='--dem-out: altitude above sea level that is z = 0 (default: the height at --center)') + parser.add_argument( + '--dem-imagery', action='store_true', + help='--dem-out: also store the imagery colour of every node') + parser.add_argument( + '--terrain-out', default=None, metavar='DIR', + help=( + '--dem-out: also write the grid as a Gazebo terrain in DIR (terrain.stl for the ' + 'collision, terrain.obj/.mtl/.png for the visual), in the map frame: x east, y north, ' + 'z from the reference altitude' + )) + parser.add_argument( + '--terrain-resolution', type=float, default=4.0, metavar='METERS', + help='--terrain-out: spacing of the collision mesh, coarser than the grid (default: 4)') + parser.add_argument( + '--world-template', default=None, metavar='FILE', + help=( + '--terrain-out: a world with @LATITUDE@, @LONGITUDE@, @ELEVATION@ (m above sea level ' + 'of z = 0) and @TERRAIN_DIR@ placeholders, filled in and written to --world-out' + )) + parser.add_argument( + '--world-out', default=None, metavar='FILE', help='--world-template: where the world goes') parser.add_argument( '--resolution', type=float, default=1.0, metavar='METERS', help=( @@ -130,12 +176,38 @@ def build_arg_parser() -> argparse.ArgumentParser: '--cache-dir', default=None, help='GIS download cache directory (default: platformdirs cache dir)') parser.add_argument( - '--dem-source', choices=['copernicus30'], default='copernicus30', - help='elevation data source (default: %(default)s)') + '--dem-source', choices=['copernicus30', 'ign', 'mesh', 'synthetic'], + default='copernicus30', + help=( + 'elevation data source: copernicus30 (worldwide, 30 m, keyless), ign (Spain only, ' + "5 m, keyless: IGN's national terrain model), mesh (an STL of your own survey, " + 'see --mesh-file) or synthetic (an offline test terrain) (default: %(default)s)' + )) + parser.add_argument( + '--mesh-file', default=None, metavar='STL', + help=( + '--dem-source mesh: STL in the local frame of --center (x east, y north, metres); ' + 'its top surface is the terrain' + )) parser.add_argument( - '--imagery-source', choices=['esri', 'pnoa', 'google'], default='esri', + '--dem-holes-mesh', default=None, metavar='STL', help=( - 'imagery data source: esri (worldwide, keyless), pnoa (Spain ' + '--dem-out: STL in the local frame of --center (buildings, walls); the cells its ' + 'triangles touch (seen from above) have no ground in the DEM: holes, as the walls of ' + 'a NavMap. Use it with a --mesh-file of the ground alone' + )) + parser.add_argument( + '--mesh-world-elevation', type=float, default=None, metavar='METERS', + help='--dem-source mesh: altitude above sea level of the mesh z = 0 (default: 0)') + parser.add_argument( + '--mesh-cell', type=float, default=0.5, metavar='METERS', + help='--dem-source mesh: cell of the height grid the mesh is rasterised to (0.5)') + parser.add_argument( + '--imagery-source', choices=['esri', 'pnoa', 'google', 'xyz', 'synthetic'], + default='esri', + help=( + 'imagery data source: esri (worldwide, keyless), xyz (tiles of any server, see ' + '--imagery-url), synthetic (an offline chessboard), pnoa (Spain ' "only -- IGN's national aerial survey, keyless, CC BY 4.0, up to " '25 cm/px native and often sharper than esri in rural Spain), or ' 'google (worldwide, paid -- requires a Google Maps Platform API ' @@ -144,6 +216,15 @@ def build_arg_parser() -> argparse.ArgumentParser: 'caching/licensing caveat specific to this source) ' '(default: %(default)s)' )) + parser.add_argument( + '--imagery-url', default=None, metavar='TEMPLATE', + help=( + '--imagery-source xyz: tile URL with {z}, {x} and {y}, e.g. ' + "'https://tile.example.org/{z}/{x}/{y}.jpg' (Web Mercator tiles, as OSM and Esri)" + )) + parser.add_argument( + '--imagery-resolution', type=float, default=PNOA_DEFAULT_RESOLUTION_M, metavar='M_PER_PX', + help='pnoa only: metres per pixel requested (default: %(default)s, its native best)') parser.add_argument( '--zoom', type=int, default=DEFAULT_ZOOM, help=( @@ -169,8 +250,8 @@ def run(argv=None) -> int: parser = build_arg_parser() args = parser.parse_args(argv) - if not args.gazebo and not args.navmap: - parser.error('at least one of --gazebo/--navmap is required') + if not args.gazebo and not args.navmap and not args.dem_out: + parser.error('at least one of --gazebo/--navmap/--dem-out is required') if args.size <= 0: parser.error('--size must be positive') if args.resolution <= 0: @@ -184,9 +265,29 @@ def run(argv=None) -> int: get_google_api_key() except RuntimeError as e: parser.error(str(e)) - + if args.imagery_source == 'xyz' and not args.imagery_url: + parser.error('--imagery-source xyz needs --imagery-url') + if args.dem_source == 'mesh' and not args.mesh_file: + parser.error('--dem-source mesh needs --mesh-file') + if args.dem_holes_mesh and not args.dem_out: + parser.error('--dem-holes-mesh needs --dem-out') + if args.dem_source == 'mesh' and args.mesh_cell <= 0: + parser.error('--mesh-cell must be positive') + if args.dem_data_resolution < 0: + parser.error('--dem-data-resolution must be >= 0') + if (args.terrain_out or args.world_template) and not args.dem_out: + parser.error('--terrain-out and --world-template need --dem-out') + if args.world_template and not (args.terrain_out and args.world_out): + parser.error('--world-template needs --terrain-out and --world-out') + if args.terrain_resolution <= 0: + parser.error('--terrain-resolution must be positive') + if args.imagery_resolution <= 0: + parser.error('--imagery-resolution must be positive') + + # the world / NavMap sampling loop is pure Python; --dem-out alone is vectorised + 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: parser.error( f'--size {args.size} / --resolution {args.resolution} would sample ' f'{n_per_side * n_per_side} grid points (a {n_per_side}x{n_per_side} grid), ' @@ -206,41 +307,14 @@ def run(argv=None) -> int: projection = LocalProjection(lat, lon) bbox = projection.square_bbox(args.size) + if not needs_grid: + return _run_dem_only(args, bbox, cache) + print(f'[navmap_gis_tool] fetching DEM ({args.dem_source}) over {bbox} ...') - dem = load_dem_grid(bbox, cache, force=args.force_refresh) + dem = _load_dem(args, bbox, cache) - if args.imagery_source == 'esri': - print(f'[navmap_gis_tool] fetching imagery (esri, zoom={args.zoom}) ...') - imagery = load_imagery_mosaic(bbox, cache, zoom=args.zoom, force=args.force_refresh) - resolution_m_per_px = native_resolution_m_per_px(imagery.zoom, lat) - imagery_desc = ( - f'Esri World Imagery, zoom {imagery.zoom} ' - f'(~{resolution_m_per_px:.2f} m/px at this latitude)' - ) - imagery_attribution = ( - 'Imagery (c) Esri, Maxar, Earthstar Geographics, and the GIS User Community.' - ) - elif args.imagery_source == 'pnoa': - print( - f'[navmap_gis_tool] fetching imagery (pnoa, ' - f'{PNOA_DEFAULT_RESOLUTION_M} m/px) ...') - imagery = load_pnoa_mosaic( - bbox, cache, resolution_m_per_px=PNOA_DEFAULT_RESOLUTION_M, force=args.force_refresh) - resolution_m_per_px = imagery.pixel_size_m - imagery_desc = f'PNOA (IGN Spain), {resolution_m_per_px:.2f} m/px' - imagery_attribution = ( - 'Contains PNOA orthoimagery (c) Instituto Geografico Nacional de Espana, ' - 'CC BY 4.0.' - ) - else: - print(f'[navmap_gis_tool] fetching imagery (google, zoom={args.zoom}) ...') - imagery = load_google_mosaic(bbox, cache, zoom=args.zoom, force=args.force_refresh) - resolution_m_per_px = native_resolution_m_per_px(imagery.zoom, lat) - imagery_desc = ( - f'Google Maps Platform satellite, zoom {imagery.zoom} ' - f'(~{resolution_m_per_px:.2f} m/px at this latitude)' - ) - imagery_attribution = 'Imagery (c) Google.' + imagery, resolution_m_per_px, imagery_desc, imagery_attribution = _load_imagery( + args, bbox, cache, lat) dem_native_m = dem.pixel_size_lat * _METERS_PER_DEGREE if args.resolution < dem_native_m / 2: @@ -248,6 +322,9 @@ def run(argv=None) -> int: f'[navmap_gis_tool] warning: --resolution {args.resolution} m is much finer ' f"than the DEM's native ~{dem_native_m:.1f} m resolution; elevation detail " 'below that will be smoothly interpolated, not real', file=sys.stderr) + + if args.dem_out: + _write_dem(args, dem, imagery, lat, lon) grid = build_terrain_grid(projection, dem, imagery, args.size, args.resolution) paths = scaffold_world_package( @@ -296,6 +373,184 @@ def run(argv=None) -> int: return 0 +def _load_dem(args, bbox, cache): + """Load the terrain model of --dem-source: something with sample(lon, lat) -> metres.""" + if args.dem_source == 'ign': + return load_ign_dem(bbox, cache, force=args.force_refresh) + if args.dem_source == 'mesh': + field = MeshHeightfield.load_stl(args.mesh_file, args.mesh_cell) + print( + f'[navmap_gis_tool] mesh {args.mesh_file}: {field.coverage:.0%} of its footprint ' + 'is covered by triangles (the rest is filled from the neighbours)') + return MeshDem(field, args.center[0], args.center[1], args.mesh_world_elevation or 0.0) + if args.dem_source == 'synthetic': + return SyntheticDem(args.center[0], args.center[1]) + return load_dem_grid(bbox, cache, force=args.force_refresh) + + +def _load_imagery(args, bbox, cache, lat): + """Load the --imagery-source mosaic; also its metres per pixel, a description, a credit.""" + if args.imagery_source in ('esri', 'xyz'): + url = args.imagery_url if args.imagery_source == 'xyz' else None + print(f'[navmap_gis_tool] fetching imagery ({args.imagery_source}, zoom={args.zoom}) ...') + imagery = load_imagery_mosaic( + bbox, cache, zoom=args.zoom, force=args.force_refresh, url_template=url) + resolution_m_per_px = native_resolution_m_per_px(imagery.zoom, lat) + if url is None: + desc = ( + f'Esri World Imagery, zoom {imagery.zoom} ' + f'(~{resolution_m_per_px:.2f} m/px at this latitude)' + ) + attribution = ( + 'Imagery (c) Esri, Maxar, Earthstar Geographics, and the GIS User Community.' + ) + else: + desc = ( + f'XYZ tiles {url}, zoom {imagery.zoom} ' + f'(~{resolution_m_per_px:.2f} m/px at this latitude)' + ) + attribution = 'Imagery: see the terms of the tile server that was used.' + return imagery, resolution_m_per_px, desc, attribution + if args.imagery_source == 'synthetic': + return ( + SyntheticImagery(args.center[0], args.center[1]), 1.0, + 'synthetic chessboard', 'Offline test imagery.') + if args.imagery_source == 'pnoa': + print( + f'[navmap_gis_tool] fetching imagery (pnoa, ' + f'{args.imagery_resolution} m/px) ...') + imagery = load_pnoa_mosaic( + bbox, cache, resolution_m_per_px=args.imagery_resolution, force=args.force_refresh) + resolution_m_per_px = imagery.pixel_size_m + return ( + imagery, resolution_m_per_px, + f'PNOA (IGN Spain), {resolution_m_per_px:.2f} m/px', + 'Contains PNOA orthoimagery (c) Instituto Geografico Nacional de Espana, ' + 'CC BY 4.0.') + print(f'[navmap_gis_tool] fetching imagery (google, zoom={args.zoom}) ...') + imagery = load_google_mosaic(bbox, cache, zoom=args.zoom, force=args.force_refresh) + resolution_m_per_px = native_resolution_m_per_px(imagery.zoom, lat) + return ( + imagery, resolution_m_per_px, + f'Google Maps Platform satellite, zoom {imagery.zoom} ' + f'(~{resolution_m_per_px:.2f} m/px at this latitude)', + 'Imagery (c) Google.') + + +_DEM_SOURCE_IDS = { + 'copernicus30': DemSource.COPERNICUS, 'ign': DemSource.IGN, 'mesh': DemSource.MESH, + 'synthetic': DemSource.SYNTHETIC, +} +_DEM_NATIVE_M = {'copernicus30': 30.0, 'ign': 5.0, 'mesh': 0.0, 'synthetic': 0.0} + + +def _source_id(args): + """Return the id of --dem-source that the DEM file records.""" + return _DEM_SOURCE_IDS[args.dem_source] + + +def _dem_geometry(args): + """Return the radius, requested reference altitude and data resolution of --dem-out.""" + radius = args.size / 2.0 + reference = args.reference_altitude + mesh = None + if args.dem_source == 'mesh': + mesh = MeshHeightfield.load_stl(args.mesh_file, args.mesh_cell) + # the DEM cannot show more than the mesh: cap the area to its footprint + reach = math.ceil(max(abs(mesh.x_min), abs(mesh.x_max), abs(mesh.y_min), abs(mesh.y_max))) + if reach < radius: + print( + f'[navmap_gis_tool] mesh {args.mesh_file} covers up to {reach:.0f} m from the ' + f'origin: radius {radius:.0f} -> {reach:.0f}') + radius = float(reach) + if reference is None and args.mesh_world_elevation is not None: + reference = args.mesh_world_elevation # z = 0 is the world's elevation + data_res = args.dem_data_resolution or _DEM_NATIVE_M[args.dem_source] + return radius, reference, data_res, mesh + + +def _run_dem_only(args, bbox, cache) -> int: + """--dem-out without a world or a NavMap: reuse the file if it is the same, else build it.""" + lat, lon = args.center + radius, reference, data_res, mesh = _dem_geometry(args) + grid = None + if not args.force_refresh and Path(args.dem_out).is_file(): + try: + old = read_dem_file(args.dem_out) + except (OSError, ValueError): + old = None + if old is not None and _is_the_same(old, args, lat, lon, radius, reference, data_res): + print(f'[navmap_gis_tool] using the DEM {args.dem_out} (same configuration)') + grid = old + if grid is None: + print(f'[navmap_gis_tool] fetching DEM ({args.dem_source}) over {bbox} ...') + dem = (MeshDem(mesh, lat, lon, args.mesh_world_elevation or 0.0) if mesh is not None + else _load_dem(args, bbox, cache)) + imagery = None + if args.dem_imagery: + try: + imagery = _load_imagery(args, bbox, cache, lat)[0] + except Exception as e: # the terrain is still usable without colours + print(f'[navmap_gis_tool] imagery unavailable ({e}): no colours', file=sys.stderr) + grid = build_local_dem( + 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, args.zoom) + _add_holes(grid, args) + write_dem_file(grid, args.dem_out) + print( + f'[navmap_gis_tool] wrote {args.dem_out} ({grid.side}x{grid.side} nodes of ' + f'{grid.resolution} m, sampled every {grid.data_resolution} m, ' + f'reference altitude {grid.reference_altitude:.1f} m)') + if args.terrain_out: + write_gazebo_terrain(grid, args.terrain_out, args.terrain_resolution) + print(f'[navmap_gis_tool] wrote the Gazebo terrain in {args.terrain_out}') + if args.world_template: + write_world_from_template(grid, args.world_template, args.world_out, args.terrain_out) + print(f'[navmap_gis_tool] wrote the world {args.world_out}') + return 0 + + +def _is_the_same(old, args, lat, lon, radius, reference, data_res) -> bool: + """Whether the DEM file on disk is what these arguments would make.""" + if not old.matches(lat, lon, radius, args.resolution, _source_id(args), reference, data_res): + return False + if bool(args.dem_holes_mesh) != (old.cell_valid is not None): + return False + return not args.dem_imagery or (old.rgb is not None and old.imagery_zoom == args.zoom) + + +def _add_holes(grid, args) -> None: + """--dem-holes-mesh: the cells its triangles touch have no ground.""" + if not args.dem_holes_mesh: + return + holes = hole_cells(load_stl_triangles(args.dem_holes_mesh), grid.side, grid.resolution) + grid.cell_valid = (~holes).astype(np.uint8) + print(f'[navmap_gis_tool] holes from {args.dem_holes_mesh}: {holes.sum()} of {holes.size} ' + f'cells without ground') + + +def _write_dem(args, dem, imagery, lat, lon) -> None: + """--dem-out next to a world / NavMap: the local height grid that bigmap_maps_manager reads.""" + radius, reference, data_res, _ = _dem_geometry(args) + grid = build_local_dem( + 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)) + _add_holes(grid, args) + write_dem_file(grid, args.dem_out) + print( + f'[navmap_gis_tool] wrote {args.dem_out} ({grid.side}x{grid.side} nodes of ' + f'{grid.resolution} m, sampled every {grid.data_resolution} m, ' + f'reference altitude {grid.reference_altitude:.1f} m)') + if args.terrain_out: + write_gazebo_terrain(grid, args.terrain_out, args.terrain_resolution) + if args.world_template: + write_world_from_template(grid, args.world_template, args.world_out, args.terrain_out) + + def _find_tool_executable(name: str) -> str: from ament_index_python.packages import get_package_prefix, PackageNotFoundError diff --git a/navmap_tools/navmap_tools/geo/dem.py b/navmap_tools/navmap_tools/geo/dem.py index 110933b..8951828 100644 --- a/navmap_tools/navmap_tools/geo/dem.py +++ b/navmap_tools/navmap_tools/geo/dem.py @@ -99,6 +99,21 @@ def sample(self, lon: float, lat: float) -> float: bot = v10 * (1 - fc) + v11 * fc return float(top * (1 - fr) + bot * fr) + def sample_array(self, lon, lat) -> np.ndarray: + """Bilinearly sample elevations (m) at arrays of lon, lat; clamps to the grid edge.""" + rows, cols = self.elevation.shape + col = np.clip((np.asarray(lon, dtype=np.float64) - self.west) / self.pixel_size_lon, + 0.0, cols - 1.0) + row = np.clip((self.north - np.asarray(lat, dtype=np.float64)) / self.pixel_size_lat, + 0.0, rows - 1.0) + c0, r0 = np.floor(col).astype(int), np.floor(row).astype(int) + c1, r1 = np.minimum(c0 + 1, cols - 1), np.minimum(r0 + 1, rows - 1) + fc, fr = col - c0, row - r0 + z = self.elevation + top = z[r0, c0] * (1 - fc) + z[r0, c1] * fc + bot = z[r1, c0] * (1 - fc) + z[r1, c1] * fc + return top * (1 - fr) + bot * fr + def _read_tile_array(path) -> Tuple[np.ndarray, float, float, float, float]: # Pillow decodes via libtiff, which handles Copernicus' floating-point predictor diff --git a/navmap_tools/navmap_tools/geo/dem_file.py b/navmap_tools/navmap_tools/geo/dem_file.py new file mode 100644 index 0000000..cbaca26 --- /dev/null +++ b/navmap_tools/navmap_tools/geo/dem_file.py @@ -0,0 +1,390 @@ +# 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. + +""" +The local height-grid file that bigmap_maps_manager reads (BigMap's `DemGrid`). + +A square grid of heights in the local ENU frame of a GPS origin (x east, y north, +the origin an exact node), with z relative to a reference altitude, and +optionally an imagery colour per node. This module builds it from anything with +a `sample_array(lon, lat)` (or `sample(lon, lat)`) and writes the binary format +of BigMap's `DemGrid::save` (bigmap_maps_manager), so the C++ side loads it unchanged: + + char magic[8] = "LMDEM006" + int32 side (odd) + int32 source (DemSource) + double resolution, radius, origin_lat, origin_lon, reference_altitude, + requested_reference (NaN: automatic), data_resolution + float32 z[side * side] (row-major, index = j * side + i) + int32 imagery_zoom (0: no colours) + uint8 rgb[side * side * 3] (only if imagery_zoom != 0) + optional, to the end of the file (older files end above): + char tag[8] = "CELLS" and three zero bytes + uint8 cell_valid[(side - 1) * (side - 1)] (cell (i, j) between nodes i..i+1, j..j+1, + row-major; 1 = ground, 0 = a hole: a building, a wall) + +Native little endian, as written by the C++ side on x86 and arm. +""" + +from dataclasses import dataclass, field +import enum +import math +from pathlib import Path +import struct +from typing import Callable, Optional + +import numpy as np + +from pyproj import Transformer + +MAGIC = b'LMDEM006' +CELLS_TAG = b'CELLS\0\0\0' +_MAX_SIDE = 20000 # what DemGrid::load accepts + + +class DemSource(enum.IntEnum): + """Where the heights come from (BigMap's DemGrid::Source).""" + + SYNTHETIC = 1 + IGN = 2 + MESH = 3 + TERRAIN_TILES = 4 + COPERNICUS = 5 + + +_GEODETIC = 'EPSG:4979' # lon, lat, height on WGS84 +_ECEF = 'EPSG:4978' + + +class EnuProjection: + """Local tangent plane (east, north, up) at a geodetic origin, on WGS84.""" + + def __init__(self, lat0: float, lon0: float, alt0: float = 0.0): + """Fix the origin (degrees, degrees, metres).""" + self.lat0, self.lon0, self.alt0 = lat0, lon0, alt0 + self._to_ecef = Transformer.from_crs(_GEODETIC, _ECEF, always_xy=True) + self._to_geodetic = Transformer.from_crs(_ECEF, _GEODETIC, always_xy=True) + lat, lon = math.radians(lat0), math.radians(lon0) + sl, cl, so, co = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon) + # rows: east, north, up (in ECEF) + self._rot = np.array([ + [-so, co, 0.0], + [-sl * co, -sl * so, cl], + [cl * co, cl * so, sl], + ]) + self._origin = np.array(self._to_ecef.transform(lon0, lat0, alt0)) + + def to_geodetic(self, east, north, up=0.0): + """Arrays of east, north, up (m) -> (lat, lon, alt) arrays.""" + east = np.asarray(east, dtype=np.float64) + north = np.asarray(north, dtype=np.float64) + up = np.broadcast_to(np.asarray(up, dtype=np.float64), east.shape) + enu = np.stack([east, north, up]) + ecef = self._origin[:, None] + self._rot.T @ enu.reshape(3, -1) + lon, lat, alt = self._to_geodetic.transform(ecef[0], ecef[1], ecef[2]) + shape = east.shape + return (np.asarray(lat).reshape(shape), np.asarray(lon).reshape(shape), + np.asarray(alt).reshape(shape)) + + def to_enu(self, lat, lon, alt=0.0): + """Arrays of lat, lon (degrees) and alt (m) -> east, north, up arrays.""" + lat = np.asarray(lat, dtype=np.float64) + lon = np.asarray(lon, dtype=np.float64) + alt = np.broadcast_to(np.asarray(alt, dtype=np.float64), lat.shape) + x, y, z = self._to_ecef.transform(lon, lat, alt) + ecef = np.stack([np.ravel(x), np.ravel(y), np.ravel(z)]) + enu = self._rot @ (ecef - self._origin[:, None]) + return tuple(enu[k].reshape(lat.shape) for k in range(3)) + + +@dataclass +class LocalDem: + """A square height grid in the local ENU frame of a GPS origin.""" + + side: int + resolution: float + data_resolution: float + radius: float + origin_lat: float + origin_lon: float + reference_altitude: float + requested_reference: float # NaN: automatic + source: int + z: np.ndarray # (side, side) float32, [j, i] + imagery_zoom: int = 0 + rgb: Optional[np.ndarray] = field(default=None) # (side, side, 3) uint8 + # (side - 1, side - 1) uint8 [j, i], 1 = ground; None: every cell has ground + cell_valid: Optional[np.ndarray] = field(default=None) + + def x(self, i): + """East coordinate (m) of node column(s) `i`.""" + return (np.asarray(i) - (self.side - 1) // 2) * self.resolution + + def y(self, j): + """North coordinate (m) of node row(s) `j`.""" + return (np.asarray(j) - (self.side - 1) // 2) * self.resolution + + def height_at(self, x: float, y: float) -> float: + """Return the height (m, from the reference altitude), bilinear, clamped to the grid.""" + c = (self.side - 1) / 2.0 + u = min(max(x / self.resolution + c, 0.0), self.side - 1.0) + v = min(max(y / self.resolution + c, 0.0), self.side - 1.0) + i = min(int(u), self.side - 2) + j = min(int(v), self.side - 2) + tu, tv = u - i, v - j + z = self.z + return float((1 - tv) * ((1 - tu) * z[j, i] + tu * z[j, i + 1]) + + tv * ((1 - tu) * z[j + 1, i] + tu * z[j + 1, i + 1])) + + def matches( + self, lat: float, lon: float, radius: float, resolution: float, source: int, + requested_reference: Optional[float] = None, data_resolution: float = 0.0, + ) -> bool: + """Whether this grid was made with this geometry and source (cache validity).""" + if requested_reference is None: + ref_ok = math.isnan(self.requested_reference) + else: + ref_ok = abs(requested_reference - self.requested_reference) < 1e-6 + mine = self.data_resolution if self.data_resolution > 0 else self.resolution + want = data_resolution if data_resolution > 0 else resolution + return (abs(lat - self.origin_lat) < 1e-9 and abs(lon - self.origin_lon) < 1e-9 and + abs(radius - self.radius) < 1e-6 and abs(resolution - self.resolution) < 1e-9 and + ref_ok and int(source) == self.source and abs(want - mine) < 1e-9) + + +def _catmull_rom(p0, p1, p2, p3, t): + return 0.5 * ((2.0 * p1) + (-p0 + p2) * t + (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3) * t * t + + (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t * t * t) + + +def _bicubic(z: np.ndarray, u: np.ndarray, v: np.ndarray) -> np.ndarray: + """Catmull-Rom sample of a (side, side) grid at fractional node coordinates, clamped.""" + side = z.shape[0] + u = np.clip(u, 0.0, side - 1.0) + v = np.clip(v, 0.0, side - 1.0) + i = np.minimum(u.astype(int), side - 2) + j = np.minimum(v.astype(int), side - 2) + tu, tv = u - i, v - j + + def at(x, y): + return z[np.clip(y, 0, side - 1), np.clip(x, 0, side - 1)].astype(np.float64) + + cols = [] + for k in range(4): + y = j - 1 + k + cols.append(_catmull_rom(at(i - 1, y), at(i, y), at(i + 1, y), at(i + 2, y), tu)) + return _catmull_rom(cols[0], cols[1], cols[2], cols[3], tv) + + +def _sample(sampler, lon: np.ndarray, lat: np.ndarray) -> np.ndarray: + if hasattr(sampler, 'sample_array'): + return np.asarray(sampler.sample_array(lon, lat), dtype=np.float64) + return np.array( + [sampler.sample(float(a), float(b)) for a, b in zip(lon.ravel(), lat.ravel())], + dtype=np.float64).reshape(lon.shape) + + +def build_local_dem( + sampler, lat0: float, lon0: float, radius: float, resolution: float, source: int, + reference_altitude: Optional[float] = None, data_resolution: float = 0.0, + progress: Optional[Callable[[int, int], None]] = None, +) -> LocalDem: + """ + Sample `sampler` (heights above sea level at lon, lat) into a local grid. + + As DemGrid::build: `radius` is the half side; the grid has an odd number of nodes + with the origin on a node. If `data_resolution` is above `resolution` the source is + queried only every `data_resolution` metres (what its data supports) and bicubically + interpolated to the finer grid. z is relative to `reference_altitude`, by default + the height at the origin. + """ + if radius <= 0.0 or resolution <= 0.0: + raise ValueError('radius and resolution must be positive') + dres = data_resolution if data_resolution > resolution else resolution + half = int(math.ceil(radius / resolution)) + side = 2 * half + 1 + if side > _MAX_SIDE: + raise ValueError(f'a grid of {side} x {side} nodes is over the limit of {_MAX_SIDE}') + half_d = int(math.ceil(radius / dres)) + side_d = 2 * half_d + 1 + + proj = EnuProjection(lat0, lon0) + idx = (np.arange(side_d) - half_d) * dres + east, north = np.meshgrid(idx, idx) + lat, lon, _ = proj.to_geodetic(east, north, 0.0) + elev = np.empty((side_d, side_d), dtype=np.float64) + rows_per_chunk = max(1, 4096 // side_d) + for j in range(0, side_d, rows_per_chunk): + e = min(side_d, j + rows_per_chunk) + elev[j:e] = _sample(sampler, lon[j:e], lat[j:e]) + if progress: + progress(e * side_d, side_d * side_d) + + ref = float(elev[half_d, half_d]) if reference_altitude is None else reference_altitude + if side_d == side and dres == resolution: + z = (elev - ref).astype(np.float32) + else: + coords = (np.arange(side) - half) * resolution / dres + half_d + u, v = np.meshgrid(coords, coords) + z = (_bicubic(elev, u, v) - ref).astype(np.float32) + return LocalDem( + side=side, resolution=resolution, data_resolution=dres, radius=radius, + origin_lat=lat0, origin_lon=lon0, reference_altitude=ref, + requested_reference=float('nan') if reference_altitude is None else reference_altitude, + source=int(source), z=z) + + +def add_imagery(dem: LocalDem, imagery, zoom: int) -> None: + """ + Colour every node from the imagery; `zoom` is recorded in the file. + + `imagery` has `sample_array(lon, lat) -> (..., 3)` (fast) or `sample(lon, lat) -> (r, g, b)`. + """ + proj = EnuProjection(dem.origin_lat, dem.origin_lon) + idx = (np.arange(dem.side) - (dem.side - 1) // 2) * dem.resolution + east, north = np.meshgrid(idx, idx) + lat, lon, _ = proj.to_geodetic(east, north, 0.0) + if hasattr(imagery, 'sample_array'): + rgb = np.clip(np.rint(imagery.sample_array(lon, lat)), 0, 255).astype(np.uint8) + else: + rgb = np.empty((dem.side, dem.side, 3), dtype=np.uint8) + for j in range(dem.side): + for i in range(dem.side): + rgb[j, i] = imagery.sample(float(lon[j, i]), float(lat[j, i])) + dem.rgb = rgb + dem.imagery_zoom = max(int(zoom), 1) # 0 would mean "no colours" to the reader + + +def write_dem_file(dem: LocalDem, path) -> None: + """Write `dem` in the format of BigMap's DemGrid::save (atomically).""" + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + tmp = path.with_name(path.name + '.tmp') + zoom = dem.imagery_zoom if dem.rgb is not None else 0 + header = (dem.resolution, dem.radius, dem.origin_lat, dem.origin_lon, + dem.reference_altitude, dem.requested_reference, dem.data_resolution) + with open(tmp, 'wb') as f: + f.write(MAGIC) + f.write(struct.pack(' LocalDem: + """Read a file written by this module or by DemGrid::save.""" + data = Path(path).read_bytes() + if data[:8] != MAGIC: + raise ValueError(f'{path} is not a DEM file (bad magic)') + if len(data) < 72: + raise ValueError(f'{path} is truncated') + side, source, *header = struct.unpack_from(' _MAX_SIDE: + raise ValueError(f'bad side {side}') + if len(data) < 72 + 4 * side * side + 4: + raise ValueError(f'{path} is truncated') + resolution, radius, lat0, lon0, reference, requested, data_res = header + if not (resolution > 0.0 and math.isfinite(resolution)) or not all( + math.isfinite(v) for v in (radius, lat0, lon0, reference)): + raise ValueError(f'{path} has a bad header (resolution {resolution}, radius {radius}, ' + f'origin {lat0}, {lon0}, reference {reference})') + offset = 8 + 8 + 56 + n = side * side + z = np.frombuffer(data, dtype=' offset: # the optional cells block (DemGrid::load refuses it broken too) + nc = (side - 1) * (side - 1) + if data[offset:offset + 8] != CELLS_TAG or len(data) < offset + 8 + nc: + raise ValueError(f'{path} has a broken cells block') + cells = (np.frombuffer(data, dtype=np.uint8, count=nc, offset=offset + 8) + .reshape(side - 1, side - 1) != 0).astype(np.uint8) + return LocalDem( + side=side, resolution=resolution, data_resolution=data_res, radius=radius, + origin_lat=lat0, origin_lon=lon0, reference_altitude=reference, + requested_reference=requested, source=source, z=z, imagery_zoom=zoom, rgb=rgb, + cell_valid=cells) + + +class SyntheticDem: + """Offline deterministic terrain (hills and roughness) around an origin, for demos.""" + + pixel_size_lat = 0.0 + + def __init__(self, lat0: float, lon0: float): + """Place the terrain at the origin (the same one the DEM is centred on).""" + self.lat0, self.lon0 = lat0, lon0 + + @staticmethod + def height(x, y): + """Height (m) at local metres (x east, y north).""" + x = np.asarray(x, dtype=np.float64) + y = np.asarray(y, dtype=np.float64) + hill = 20.0 * np.exp(-((x - 80) ** 2 + (y + 60) ** 2) / (2 * 40.0 * 40.0)) + swell = 12.0 * np.sin(x / 60.0) * np.cos(y / 45.0) + rough = 1.5 * np.sin(x / 7.0 + y / 5.0) + return 650.0 + hill + swell + rough + + def _local(self, lon, lat): + m_lat = 111132.0 + m_lon = 111320.0 * math.cos(math.radians(self.lat0)) + return (np.asarray(lon) - self.lon0) * m_lon, (np.asarray(lat) - self.lat0) * m_lat + + def sample_array(self, lon, lat) -> np.ndarray: + """Heights at lon, lat arrays.""" + return self.height(*self._local(lon, lat)) + + def sample(self, lon: float, lat: float) -> float: + """Height at one point.""" + return float(self.sample_array(np.array([lon]), np.array([lat]))[0]) + + +class SyntheticImagery: + """Offline imagery: a red / blue chessboard of 25 m squares with white axes.""" + + def __init__(self, lat0: float, lon0: float): + """Place the chessboard at the origin.""" + self._dem = SyntheticDem(lat0, lon0) + + def sample_array(self, lon, lat) -> np.ndarray: + """Colours (..., 3) at lon, lat arrays.""" + x, y = self._dem._local(lon, lat) + even = ((np.floor(x / 25.0) + np.floor(y / 25.0)) % 2) == 0 + rgb = np.stack([np.where(even, 200, 60), np.full(x.shape, 60), + np.where(even, 60, 200)], axis=-1).astype(np.float64) + axes = (np.abs(x) < 2.0) | (np.abs(y) < 2.0) + rgb[axes] = 255.0 + return rgb + + def sample(self, lon: float, lat: float): + """Colour at one point.""" + r, g, b = self.sample_array(np.array([lon]), np.array([lat]))[0] + return int(r), int(g), int(b) diff --git a/navmap_tools/navmap_tools/geo/ign.py b/navmap_tools/navmap_tools/geo/ign.py new file mode 100644 index 0000000..2c50666 --- /dev/null +++ b/navmap_tools/navmap_tools/geo/ign.py @@ -0,0 +1,203 @@ +# 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. + +""" +IGN Spain terrain model (MDT, from PNOA-LiDAR) through its WCS service -- no API key. + +5 m grid, orthometric heights, CC BY 4.0, Spain only (the coverage is ETRS89 / +UTM 30N for the whole peninsula, so the zone is forced for every point). +Roughly six times finer than Copernicus 30 m, which is why it is worth having +next to it for Spanish sites. + +The area is requested in square blocks of at most `max_block_m` metres (one +WCS GetCoverage each, as an ArcGrid / ASC body) and every block is cached on +disk. `IgnDem.sample` has the interface of `geo.dem.DemGrid.sample` so both +can feed the same pipeline. +""" + +from dataclasses import dataclass, field +import math +from typing import List, Tuple +from urllib.parse import quote + +import numpy as np + +from pyproj import Transformer + +from .cache import DiskCache +from .net import get_to_file +from .projection import BBox + +DEFAULT_ENDPOINT = 'https://servicios.idee.es/wcs-inspire/mdt' +DEFAULT_COVERAGE = 'Elevacion25830_5' +DEFAULT_CELL_M = 5.0 +_MAX_BLOCK_M = 2000.0 +_TIMEOUT_S = 120 +_METERS_PER_DEGREE = 111320.0 + +_TO_UTM30 = Transformer.from_crs('EPSG:4326', 'EPSG:25830', always_xy=True) + + +@dataclass +class IgnBlock: + """One ArcGrid block: row 0 is the northern one, values are at the cell centres.""" + + x_min: float + y_min: float + cell: float + values: np.ndarray # (nrows, ncols) float32, NaN where the service has no data + + def sample_array(self, x: np.ndarray, y: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """Bilinear heights at UTM (x, y) arrays, and which of them this block could answer.""" + nrows, ncols = self.values.shape + fx = (x - self.x_min) / self.cell - 0.5 + fy = ((self.y_min + nrows * self.cell) - y) / self.cell - 0.5 + inside = (fx >= -0.5) & (fy >= -0.5) & (fx <= ncols - 0.5) & (fy <= nrows - 0.5) + cx = np.clip(fx, 0.0, ncols - 1.0) + cy = np.clip(fy, 0.0, nrows - 1.0) + i0 = np.minimum(cx.astype(int), max(ncols - 2, 0)) + j0 = np.minimum(cy.astype(int), max(nrows - 2, 0)) + i1 = np.minimum(i0 + 1, ncols - 1) + j1 = np.minimum(j0 + 1, nrows - 1) + tx, ty = cx - i0, cy - j0 + corners = ( + (self.values[j0, i0], (1 - tx) * (1 - ty)), + (self.values[j0, i1], tx * (1 - ty)), + (self.values[j1, i0], (1 - tx) * ty), + (self.values[j1, i1], tx * ty), + ) + acc = np.zeros(x.shape, dtype=np.float64) + ok = inside.copy() + for value, weight in corners: + matters = weight >= 1e-9 # a cell without data only matters if it contributes + ok &= ~(matters & np.isnan(value)) + acc += np.where(matters, weight * np.nan_to_num(value), 0.0) + return acc, ok + + +def parse_asc(body: str) -> IgnBlock: + """Parse the (possibly multipart) body of a GetCoverage response in ArcGrid format.""" + start = body.find('ncols') + if start < 0: + raise ValueError('not an ArcGrid body: no "ncols"') + tokens = body[start:].split() + header = {} + pos = 0 + while pos + 1 < len(tokens): + key = tokens[pos].lower() + if key not in ( + 'ncols', 'nrows', 'xllcorner', 'xllcenter', 'yllcorner', 'yllcenter', + 'cellsize', 'nodata_value', + ): + break + header[key] = float(tokens[pos + 1]) + pos += 2 + ncols, nrows = int(header.get('ncols', 0)), int(header.get('nrows', 0)) + cell = header.get('cellsize', 0.0) + has_x = 'xllcorner' in header or 'xllcenter' in header + has_y = 'yllcorner' in header or 'yllcenter' in header + if ncols < 1 or nrows < 1 or cell <= 0 or not (has_x and has_y): + raise ValueError('bad ArcGrid header') + x_min = header['xllcorner'] if 'xllcorner' in header else header['xllcenter'] - cell / 2 + y_min = header['yllcorner'] if 'yllcorner' in header else header['yllcenter'] - cell / 2 + nodata = header.get('nodata_value', -9999.0) + count = ncols * nrows + raw = np.array(tokens[pos:pos + count], dtype=np.float64) + if raw.size != count: + raise ValueError(f'ArcGrid body holds {raw.size} values, header says {count}') + values = raw.astype(np.float32).reshape(nrows, ncols) + values[np.abs(raw.reshape(nrows, ncols) - nodata) < 0.5] = np.nan + return IgnBlock(x_min=x_min, y_min=y_min, cell=cell, values=values) + + +@dataclass +class IgnDem: + """The IGN terrain model over an area, as a set of blocks.""" + + blocks: List[IgnBlock] = field(default_factory=list) + cell_m: float = DEFAULT_CELL_M + + @property + def pixel_size_lat(self) -> float: + """Spacing of the data in degrees of latitude (what the CLI compares with).""" + return self.cell_m / _METERS_PER_DEGREE + + def sample_array(self, lon, lat) -> np.ndarray: + """Heights (m above sea level) at arrays of lon, lat; it raises where there is no data.""" + lon = np.asarray(lon, dtype=np.float64) + lat = np.asarray(lat, dtype=np.float64) + x, y = _TO_UTM30.transform(lon, lat) + x = np.asarray(x, dtype=np.float64) + y = np.asarray(y, dtype=np.float64) + out = np.zeros(x.shape, dtype=np.float64) + todo = np.ones(x.shape, dtype=bool) + for block in self.blocks: + if not todo.any(): + break + value, ok = block.sample_array(x, y) + take = todo & ok + out[take] = value[take] + todo &= ~take + if todo.any(): + k = int(np.argmax(todo)) + raise RuntimeError( + f'IGN MDT has no data at lat {lat.flat[k]:.6f}, lon {lon.flat[k]:.6f} ' + '(the service only covers Spain)') + return out + + def sample(self, lon: float, lat: float) -> float: + """Height at one point (the interface of `geo.dem.DemGrid.sample`).""" + return float(self.sample_array(np.array([lon]), np.array([lat]))[0]) + + +def _wcs_url(endpoint: str, coverage: str, x0: float, y0: float, x1: float, y1: float) -> str: + return ( + f'{endpoint}?service=WCS&version=2.0.1&request=GetCoverage' + f'&coverageId={quote(coverage, safe="")}' + f'&subset={quote(f"x({x0:.0f},{x1:.0f})", safe="")}' + f'&subset={quote(f"y({y0:.0f},{y1:.0f})", safe="")}' + f'&format={quote("application/asc", safe="")}' + ) + + +def load_ign_dem( + bbox: BBox, cache: DiskCache, force: bool = False, + endpoint: str = DEFAULT_ENDPOINT, coverage: str = DEFAULT_COVERAGE, + cell_m: float = DEFAULT_CELL_M, max_block_m: float = _MAX_BLOCK_M, +) -> IgnDem: + """Load (downloading and caching each block as needed) the IGN MDT covering `bbox`.""" + xs, ys = _TO_UTM30.transform( + [bbox.west, bbox.west, bbox.east, bbox.east], + [bbox.south, bbox.north, bbox.south, bbox.north]) + margin = 4.0 * cell_m + xmin = math.floor((min(xs) - margin) / cell_m) * cell_m + ymin = math.floor((min(ys) - margin) / cell_m) * cell_m + xmax = math.ceil((max(xs) + margin) / cell_m) * cell_m + ymax = math.ceil((max(ys) + margin) / cell_m) * cell_m + + dem = IgnDem(cell_m=cell_m) + y = ymin + while y < ymax: + x = xmin + while x < xmax: + x1, y1 = min(x + max_block_m, xmax), min(y + max_block_m, ymax) + url = _wcs_url(endpoint, coverage, x, y, x1, y1) + key = f'dem/ign/{coverage}/{x:.0f}_{y:.0f}_{x1:.0f}_{y1:.0f}' + path = cache.get_or_fetch( + key, '.asc', lambda dest, url=url: get_to_file(url, dest, _TIMEOUT_S), + force=force) + dem.blocks.append(parse_asc(path.read_text(errors='replace'))) + x += max_block_m + y += max_block_m + return dem diff --git a/navmap_tools/navmap_tools/geo/imagery.py b/navmap_tools/navmap_tools/geo/imagery.py index 20ae574..fd6acb8 100644 --- a/navmap_tools/navmap_tools/geo/imagery.py +++ b/navmap_tools/navmap_tools/geo/imagery.py @@ -27,7 +27,7 @@ import hashlib import math import sys -from typing import Tuple +from typing import Optional, Tuple import numpy as np @@ -130,9 +130,26 @@ def lonlat_to_tile(lon: float, lat: float, zoom: int) -> Tuple[float, float]: return x, y -def _fetch_tile_path(z: int, x: int, y: int, cache: DiskCache, force: bool = False): - url = _URL_TEMPLATE.format(z=z, y=y, x=x) - key = f'imagery/esri/{z}/{x}/{y}' +def _lonlat_to_tile_array(lon, lat, zoom: int): + lat = np.clip(np.asarray(lat, dtype=np.float64), -85.05112878, 85.05112878) + lat_rad = np.radians(lat) + n = 2.0 ** zoom + x = (np.asarray(lon, dtype=np.float64) + 180.0) / 360.0 * n + y = (1.0 - np.log(np.tan(lat_rad) + 1.0 / np.cos(lat_rad)) / math.pi) / 2.0 * n + return x, y + + +def _fetch_tile_path( + z: int, x: int, y: int, cache: DiskCache, force: bool = False, + url_template: Optional[str] = None, +): + if url_template is None: + url = _URL_TEMPLATE.format(z=z, y=y, x=x) + key = f'imagery/esri/{z}/{x}/{y}' + else: + url = url_template.replace('{z}', str(z)).replace('{x}', str(x)).replace('{y}', str(y)) + digest = hashlib.sha1(url_template.encode('utf-8')).hexdigest()[:12] + key = f'imagery/xyz/{digest}/{z}/{x}/{y}' return cache.get_or_fetch( key, '.jpg', lambda dest: get_to_file(url, dest, _TIMEOUT_S), force=force) @@ -167,10 +184,30 @@ def sample(self, lon: float, lat: float) -> Tuple[int, int, int]: r, g, b = (int(round(v)) for v in rgb) return r, g, b + def sample_array(self, lon, lat) -> np.ndarray: + """Bilinear colours (..., 3) at lon, lat arrays (float, 0-255); clamps to the edge.""" + gx, gy = _lonlat_to_tile_array(lon, lat, self.zoom) + return _bilinear_rgb( + self.image, (gx - self.tile_x0) * _TILE_SIZE, (gy - self.tile_y0) * _TILE_SIZE) + + +def _bilinear_rgb(image: np.ndarray, px: np.ndarray, py: np.ndarray) -> np.ndarray: + """Bilinear colours (..., 3) of a (H, W, 3) image at fractional pixels, clamped to it.""" + h, w, _ = image.shape + px = np.clip(px, 0.0, w - 1.0) + py = np.clip(py, 0.0, h - 1.0) + x0, y0 = np.floor(px).astype(int), np.floor(py).astype(int) + x1, y1 = np.minimum(x0 + 1, w - 1), np.minimum(y0 + 1, h - 1) + fx, fy = (px - x0)[..., None], (py - y0)[..., None] + img = image.astype(np.float32) + top = img[y0, x0] * (1 - fx) + img[y0, x1] * fx + bot = img[y1, x0] * (1 - fx) + img[y1, x1] * fx + return top * (1 - fy) + bot * fy + def load_imagery_mosaic( bbox: BBox, cache: DiskCache, zoom: int = DEFAULT_ZOOM, force: bool = False, - auto_fallback: bool = True, + auto_fallback: bool = True, url_template: Optional[str] = None, ) -> ImageryMosaic: """ Fetch/mosaic (caching each tile) the imagery covering `bbox` at `zoom`. @@ -179,6 +216,10 @@ def load_imagery_mosaic( `zoom` down until it finds real imagery instead of Esri's "not yet available" placeholder (see `find_available_zoom`) -- the mosaic is then fetched at that adjusted zoom, and `ImageryMosaic.zoom` reflects it. + + `url_template` (with `{z}`, `{x}` and `{y}`) takes the tiles from any XYZ + server instead of Esri (the Web Mercator tile scheme is the same). The + placeholder probe is Esri's, so it does not run for a custom template. """ if not 0 <= zoom <= 23: raise ValueError(f'zoom out of range [0, 23]: {zoom}') @@ -203,7 +244,7 @@ def _tile_range(z): 'reduce --size or --zoom' ) - if auto_fallback: + if auto_fallback and url_template is None: center_lon = (bbox.west + bbox.east) / 2.0 center_lat = (bbox.south + bbox.north) / 2.0 effective_zoom = find_available_zoom(center_lon, center_lat, cache, zoom, force=force) @@ -220,7 +261,8 @@ def _tile_range(z): mosaic = np.zeros((rows, cols, 3), dtype=np.uint8) def _fetch_and_place(tx: int, ty: int) -> None: - path = _fetch_tile_path(zoom, tx, ty, cache, force=force) + extra = {} if url_template is None else {'url_template': url_template} + path = _fetch_tile_path(zoom, tx, ty, cache, force=force, **extra) with Image.open(path) as im: tile_rgb = np.asarray(im.convert('RGB')) row_off = (ty - ty0) * _TILE_SIZE diff --git a/navmap_tools/navmap_tools/geo/mesh_dem.py b/navmap_tools/navmap_tools/geo/mesh_dem.py new file mode 100644 index 0000000..5883da3 --- /dev/null +++ b/navmap_tools/navmap_tools/geo/mesh_dem.py @@ -0,0 +1,225 @@ +# 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. + +""" +A triangle mesh (STL) as a terrain model: the survey of a site you measured yourself. + +The top surface of the mesh is rasterised to a regular height grid (vertical walls +collapse to their upper edge, overhangs keep the highest surface; cells the mesh does +not cover are filled from their neighbours, so a sample is defined everywhere). The +mesh is in the local frame of a GPS origin (x east, y north); the "elevation" of a +lon, lat is `world_elevation` plus the mesh height at its east / north offset. +`MeshDem.sample` has the interface of `geo.dem.DemGrid.sample`. +""" + +from dataclasses import dataclass +import math +from pathlib import Path + +import numpy as np + +from .dem_file import EnuProjection + +_METERS_PER_DEGREE = 111320.0 + + +def load_stl_triangles(path) -> np.ndarray: + """Triangles (N, 9) float32 of a binary or ASCII STL: x0 y0 z0 x1 y1 z1 x2 y2 z2.""" + raw = Path(path).read_bytes() + if len(raw) >= 84: + n = int(np.frombuffer(raw, dtype=' float: + """East edge (m) of the grid.""" + return self.x_min + (self.z.shape[1] - 1) * self.cell + + @property + def y_max(self) -> float: + """North edge (m) of the grid.""" + return self.y_min + (self.z.shape[0] - 1) * self.cell + + @classmethod + def from_triangles(cls, tris: np.ndarray, cell: float = 0.5) -> 'MeshHeightfield': + """Rasterise (N, 9) triangles.""" + if len(tris) == 0: + raise ValueError('empty mesh') + if cell <= 0.0: + raise ValueError('cell must be positive') + tris = np.asarray(tris, dtype=np.float64) + xs, ys = tris[:, 0::3], tris[:, 1::3] + x_min, y_min = float(xs.min()), float(ys.min()) + nx = int(math.floor((float(xs.max()) - x_min) / cell)) + 1 + ny = int(math.floor((float(ys.max()) - y_min) / cell)) + 1 + z = np.full((ny, nx), np.nan, dtype=np.float32) + + for t in tris: + x0, y0, z0, x1, y1, z1, x2, y2, z2 = t + det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2) + if abs(det) < 1e-9: + continue # vertical wall: no area seen from above + i0 = max(0, int(math.ceil((min(x0, x1, x2) - x_min) / cell - 1e-9))) + i1 = min(nx - 1, int(math.floor((max(x0, x1, x2) - x_min) / cell + 1e-9))) + j0 = max(0, int(math.ceil((min(y0, y1, y2) - y_min) / cell - 1e-9))) + j1 = min(ny - 1, int(math.floor((max(y0, y1, y2) - y_min) / cell + 1e-9))) + if i0 > i1 or j0 > j1: + continue + px = x_min + np.arange(i0, i1 + 1) * cell + py = y_min + np.arange(j0, j1 + 1) * cell + gx, gy = np.meshgrid(px, py) + l0 = ((y1 - y2) * (gx - x2) + (x2 - x1) * (gy - y2)) / det + l1 = ((y2 - y0) * (gx - x2) + (x0 - x2) * (gy - y2)) / det + l2 = 1.0 - l0 - l1 + inside = (l0 >= -1e-9) & (l1 >= -1e-9) & (l2 >= -1e-9) + if not inside.any(): + continue + height = (l0 * z0 + l1 * z1 + l2 * z2).astype(np.float32) + dst = z[j0:j1 + 1, i0:i1 + 1] + better = inside & (np.isnan(dst) | (height > dst)) # the highest wins + dst[better] = height[better] + + covered = ~np.isnan(z) + coverage = float(covered.mean()) + zmin = float(np.nanmin(z)) if covered.any() else 0.0 + z = _fill_from_neighbours(z) + z[np.isnan(z)] = zmin + return cls(x_min=x_min, y_min=y_min, cell=cell, z=z, coverage=coverage) + + @classmethod + def load_stl(cls, path, cell: float = 0.5) -> 'MeshHeightfield': + """Rasterise a binary or ASCII STL.""" + return cls.from_triangles(load_stl_triangles(path), cell) + + def sample_array(self, x, y) -> np.ndarray: + """Bilinear heights at east / north arrays; clamped to the mesh's bounding box.""" + ny, nx = self.z.shape + u = np.clip((np.asarray(x, dtype=np.float64) - self.x_min) / self.cell, 0.0, nx - 1.0) + v = np.clip((np.asarray(y, dtype=np.float64) - self.y_min) / self.cell, 0.0, ny - 1.0) + i = np.minimum(u.astype(int), max(nx - 2, 0)) + j = np.minimum(v.astype(int), max(ny - 2, 0)) + i1, j1 = np.minimum(i + 1, nx - 1), np.minimum(j + 1, ny - 1) + tu, tv = u - i, v - j + z = self.z.astype(np.float64) + return ((1 - tv) * ((1 - tu) * z[j, i] + tu * z[j, i1]) + + tv * ((1 - tu) * z[j1, i] + tu * z[j1, i1])) + + +def _fill_from_neighbours(z: np.ndarray) -> np.ndarray: + """Fill NaN cells with the mean of their known 3x3 neighbours, one ring per pass.""" + z = z.copy() + ny, nx = z.shape + for _ in range(nx + ny): + missing = np.isnan(z) + if not missing.any(): + break + padded = np.pad(z, 1, constant_values=np.nan) + total = np.zeros_like(z, dtype=np.float64) + count = np.zeros_like(z, dtype=np.int32) + for dj in range(3): + for di in range(3): + window = padded[dj:dj + ny, di:di + nx] + known = ~np.isnan(window) + total += np.where(known, window, 0.0) + count += known + fill = missing & (count > 0) + if not fill.any(): + break + z[fill] = (total[fill] / count[fill]).astype(np.float32) + return z + + +class MeshDem: + """A mesh in the local frame of a GPS origin, as a terrain model over lon, lat.""" + + def __init__(self, field: MeshHeightfield, lat0: float, lon0: float, world_elevation: float): + """Place `field` at the origin; heights are `world_elevation` + the mesh height.""" + self.field = field + self.world_elevation = world_elevation + self._proj = EnuProjection(lat0, lon0) + + @property + def pixel_size_lat(self) -> float: + """Spacing of the data in degrees of latitude (what the CLI compares with).""" + return self.field.cell / _METERS_PER_DEGREE + + def sample_array(self, lon, lat) -> np.ndarray: + """Heights (m above sea level) at lon, lat arrays.""" + east, north, _ = self._proj.to_enu(np.asarray(lat), np.asarray(lon), 0.0) + return self.world_elevation + self.field.sample_array(east, north) + + def sample(self, lon: float, lat: float) -> float: + """Height at one point (the interface of `geo.dem.DemGrid.sample`).""" + return float(self.sample_array(np.array([lon]), np.array([lat]))[0]) + + +def hole_cells(triangles: np.ndarray, side: int, resolution: float) -> np.ndarray: + """ + Mark the cells of a DEM grid that the xy projection of any triangle touches. + + Returns (side - 1, side - 1) bool, [j, i]. + + The grid is the one of `dem_file.LocalDem` (nodes at (i - (side - 1) / 2) * resolution, cell + (i, j) between nodes i..i+1 and j..j+1, the mesh in the same local frame). A cell is touched + if the triangle and the cell overlap or touch (separating axes: x, y and the normals of the + triangle's edges), the rule of a collision map: a cell is blocked if any part of an obstacle is + in it. A vertical wall projects to a segment and still marks the cells it crosses. + """ + nc = side - 1 + out = np.zeros((nc, nc), dtype=bool) + if len(triangles) == 0: + return out + half = (side - 1) // 2 + tri = np.asarray(triangles, dtype=np.float64).reshape(-1, 3, 3)[:, :, :2] / resolution + half + lo = np.floor(tri.min(axis=1)).astype(np.int64) + hi = np.floor(tri.max(axis=1)).astype(np.int64) + keep = (hi[:, 0] >= 0) & (hi[:, 1] >= 0) & (lo[:, 0] < nc) & (lo[:, 1] < nc) + for t, (i0, j0), (i1, j1) in zip(tri[keep], lo[keep], hi[keep]): + i0, j0, i1, j1 = max(i0, 0), max(j0, 0), min(i1, nc - 1), min(j1, nc - 1) + cj, ci = np.mgrid[j0:j1 + 1, i0:i1 + 1] + hit = np.ones(ci.shape, dtype=bool) + # x and y axes: the bounding boxes overlap by construction; the edge normals + for k in range(3): + e = t[(k + 1) % 3] - t[k] + n = np.array([-e[1], e[0]]) + if not n.any(): + continue + proj = t @ n + # the cell's corners projected on n: centre +- half extent + c = (ci + 0.5) * n[0] + (cj + 0.5) * n[1] + r = 0.5 * (abs(n[0]) + abs(n[1])) + hit &= (c - r <= proj.max()) & (c + r >= proj.min()) + out[cj[hit], ci[hit]] = True + return out diff --git a/navmap_tools/navmap_tools/geo/pnoa.py b/navmap_tools/navmap_tools/geo/pnoa.py index 6bb1c9c..1270807 100644 --- a/navmap_tools/navmap_tools/geo/pnoa.py +++ b/navmap_tools/navmap_tools/geo/pnoa.py @@ -100,6 +100,14 @@ def sample(self, lon: float, lat: float) -> Tuple[int, int, int]: r, g, b = (int(round(v)) for v in rgb) return r, g, b + def sample_array(self, lon, lat) -> np.ndarray: + """Bilinear colours (..., 3) at lon, lat arrays (float, 0-255); clamps to the edge.""" + from .imagery import _bilinear_rgb + mx, my = _TO_WEB_MERCATOR.transform(np.asarray(lon), np.asarray(lat)) + return _bilinear_rgb( + self.image, (np.asarray(mx) - self.origin_x) / self.pixel_size_m, + (self.origin_y - np.asarray(my)) / self.pixel_size_m) + def _fetch_chunk( x0: float, y0: float, x1: float, y1: float, width_px: int, height_px: int, diff --git a/navmap_tools/navmap_tools/geo/terrain_export.py b/navmap_tools/navmap_tools/geo/terrain_export.py new file mode 100644 index 0000000..ffdb172 --- /dev/null +++ b/navmap_tools/navmap_tools/geo/terrain_export.py @@ -0,0 +1,133 @@ +# 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. + +""" +A local height grid (`geo.dem_file.LocalDem`) as a Gazebo terrain, and a world from a template. + +`write_gazebo_terrain` writes, in a directory: + terrain.stl binary STL, the COLLISION geometry + terrain.obj / .mtl the same mesh with UVs and smooth normals, the VISUAL geometry + terrain.png the imagery (one texel per node of the full grid), only if the grid has it +in the map frame (x east, y north, z relative to the grid's reference altitude), so a robot spawned +at (0, 0) stands on z = 0 and Gazebo's world is the NavMap's map frame. `terrain_resolution` +above the grid's makes the mesh coarser (every k-th node, k a divisor of side - 1): a collision +mesh of millions of triangles is too heavy for the simulator; the texture keeps the full grid. + +`write_world_from_template` fills @LATITUDE@, @LONGITUDE@, @ELEVATION@ (m above sea level of the +world origin, z = 0) and @TERRAIN_DIR@ in a world template. +""" + +import io +from pathlib import Path + +import numpy as np + +from PIL import Image + +from .dem_file import LocalDem + + +def _stride_for(dem: LocalDem, terrain_resolution: float) -> int: + """Largest divisor of side - 1 not above the requested coarsening.""" + if terrain_resolution <= dem.resolution: + return 1 + want = int(round(terrain_resolution / dem.resolution)) + for k in range(min(want, dem.side - 1), 0, -1): + if (dem.side - 1) % k == 0: + return k + return 1 + + +def _cell_triangles(n: int) -> np.ndarray: + """Node index triples (T, 3) as build_navmap does: (v00, v10, v11) and (v00, v11, v01).""" + j, i = np.mgrid[0:n - 1, 0:n - 1] + v00 = (j * n + i).ravel() + v10, v01, v11 = v00 + 1, v00 + n, v00 + n + 1 + tris = np.empty((v00.size * 2, 3), dtype=np.int64) + tris[0::2] = np.stack([v00, v10, v11], axis=1) + tris[1::2] = np.stack([v00, v11, v01], axis=1) + return tris + + +def write_gazebo_terrain(dem: LocalDem, directory, terrain_resolution: float = 0.0) -> None: + """Write terrain.stl, terrain.obj, terrain.mtl (and terrain.png) into `directory`.""" + out = Path(directory) + out.mkdir(parents=True, exist_ok=True) + stride = _stride_for(dem, terrain_resolution) + z = dem.z[::stride, ::stride] + n = z.shape[0] + res = dem.resolution * stride + n_tex = dem.side + coords = (np.arange(n) - (n - 1) // 2) * res + gx, gy = np.meshgrid(coords, coords) + verts = np.stack([gx.ravel(), gy.ravel(), z.ravel().astype(np.float64)], axis=1) + verts = verts.astype(np.float32) + tris = _cell_triangles(n) + + # collision: binary STL + p = verts[tris] # (T, 3, 3) + normal = np.cross(p[:, 1] - p[:, 0], p[:, 2] - p[:, 0]) + normal /= np.maximum(np.linalg.norm(normal, axis=1, keepdims=True), 1e-12) + rec = np.zeros(len(tris), dtype=np.dtype([('n', ' None: + """Instantiate a world template for `dem` (see the module docstring).""" + text = Path(template_path).read_text() + if '@TERRAIN_DIR@' not in text: + raise ValueError('world template has no @TERRAIN_DIR@ placeholder') + + def num(v): + return '%.10g' % v + + text = (text.replace('@LATITUDE@', num(dem.origin_lat)) + .replace('@LONGITUDE@', num(dem.origin_lon)) + .replace('@ELEVATION@', num(dem.reference_altitude)) + .replace('@TERRAIN_DIR@', str(Path(terrain_dir).resolve()))) + world = Path(world_path) + world.parent.mkdir(parents=True, exist_ok=True) + world.write_text(text) diff --git a/navmap_tools/src/cloud_to_navmap.cpp b/navmap_tools/src/cloud_to_navmap.cpp new file mode 100644 index 0000000..3abb036 --- /dev/null +++ b/navmap_tools/src/cloud_to_navmap.cpp @@ -0,0 +1,314 @@ +// 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. + +#include "navmap_tools/cloud_to_navmap.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_ros/conversions.hpp" +#include "navmap_tools/floor_holes.hpp" +#include "navmap_tools/pcd_stream.hpp" +#include "pcl/io/pcd_io.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" + +namespace navmap_tools +{ + +namespace +{ +std::uint64_t cell_key(std::int64_t i, std::int64_t j) +{ + return (static_cast(static_cast(i)) << 32) | + static_cast(j); +} + +std::string sformat(const char * fmt, double a = 0, double b = 0, double c = 0, double d = 0) +{ + char buf[512]; + std::snprintf(buf, sizeof(buf), fmt, a, b, c, d); + return buf; +} + +struct CellCount +{ + std::uint32_t raw{0}, kept{0}; +}; +} // namespace + +void colour_vertices( + navmap_ros_interfaces::msg::NavMap & msg, const PointSet & pts, float res, float tol) +{ + const std::size_t nv = msg.positions_x.size(); + if (!pts.coloured() || nv == 0 || pts.size() == 0) {return;} + struct P {float x, y, z; std::uint8_t r, g, b;}; + std::unordered_map> cells; + cells.reserve(pts.size() / 2 + 1); + for (std::size_t k = 0; k < pts.size(); ++k) { + const auto i = static_cast(std::floor(pts.xyz[3 * k] / res)); + const auto j = static_cast(std::floor(pts.xyz[3 * k + 1] / res)); + cells[cell_key(i, j)].push_back( + {pts.xyz[3 * k], pts.xyz[3 * k + 1], pts.xyz[3 * k + 2], pts.rgb[3 * k], pts.rgb[3 * k + 1], + pts.rgb[3 * k + 2]}); + } + msg.has_vertex_rgba = true; + msg.colors_r.assign(nv, 128); + msg.colors_g.assign(nv, 128); + msg.colors_b.assign(nv, 128); + msg.colors_a.assign(nv, 255); + for (std::size_t v = 0; v < nv; ++v) { + const float vx = msg.positions_x[v], vy = msg.positions_y[v], vz = msg.positions_z[v]; + const auto ci = static_cast(std::floor(vx / res)); + const auto cj = static_cast(std::floor(vy / res)); + const P * best = nullptr; + float best_d = 1e30f; + // the vertex sits on the mesher's lattice, which may fall on the border of our cells: look at + // the 3 x 3 around it, then at the 5 x 5 if no point of its height is there + for (int reach = 1; reach <= 2 && !best; ++reach) { + for (int di = -reach; di <= reach; ++di) { + for (int dj = -reach; dj <= reach; ++dj) { + const auto it = cells.find(cell_key(ci + di, cj + dj)); + if (it == cells.end()) {continue;} + for (const auto & p : it->second) { + const float dz = std::abs(p.z - vz); + if (dz > tol) {continue;} + const float d = (p.x - vx) * (p.x - vx) + (p.y - vy) * (p.y - vy) + dz * dz; + if (d < best_d) {best_d = d; best = &p;} + } + } + } + } + if (best) { + msg.colors_r[v] = best->r; + msg.colors_g[v] = best->g; + msg.colors_b[v] = best->b; + } + } +} + +Report cloud_to_navmap( + const std::vector & inputs, const Options & opt, + navmap_ros_interfaces::msg::NavMap & out) +{ + using clock = std::chrono::steady_clock; + const auto t0 = clock::now(); + auto say = [&](const std::string & s) {if (opt.log) {opt.log(s);}}; + if (inputs.empty()) {throw std::runtime_error("no input point cloud");} + if (!(opt.resolution > 0.0f)) {throw std::runtime_error("resolution must be positive");} + + Report rep; + std::vector infos; + for (const auto & f : inputs) { + infos.push_back(pcd_info(f)); + rep.points_in_files += infos.back().points; + } + say(sformat("%.1f million points in %.0f file(s)", rep.points_in_files / 1e6, inputs.size())); + + // 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) { + rep.organised = true; + pcl::PointCloud cloud; + if (pcl::io::loadPCDFile(inputs[0], cloud) != 0) { + throw std::runtime_error("PCL cannot load " + inputs[0]); + } + navmap_ros::BuildParams bp; + bp.resolution = opt.resolution; + bp.max_slope_deg = opt.max_slope_deg; + navmap_ros::from_regular_grid(cloud, out, bp); + rep.points_used = rep.points_meshed = cloud.size(); + rep.warnings.push_back("organised cloud: meshed as a regular grid, no colour"); + } else { + float voxel = opt.voxel; + if (voxel < 0.0f) { + voxel = rep.points_in_files <= opt.raw_below ? 0.0f : 0.25f * opt.resolution; + } + rep.voxelized = voxel > 0.0f; + rep.voxel = voxel; + const float vz = std::min(voxel, 0.5f * opt.max_dz); + VoxelCloud vox(std::max(voxel, 1e-3f), std::max(vz, 1e-3f)); + PointSet raw; + std::size_t seen = 0, next_report = 20000000; + for (std::size_t n = 0; n < inputs.size(); ++n) { + say("reading " + inputs[n]); + read_pcd(inputs[n], [&](const PcdChunk & c) { + const bool col = !c.rgb.empty(); + for (std::size_t k = 0; k < c.size(); ++k) { + const float x = c.xyz[3 * k], y = c.xyz[3 * k + 1], z = c.xyz[3 * k + 2]; + if (z < opt.zmin || z > opt.zmax) {continue;} + if (opt.crop && (x < opt.xmin || x > opt.xmax || y < opt.ymin || y > opt.ymax)) { + continue; + } + ++rep.points_used; + const std::uint8_t * rgb = col ? &c.rgb[3 * k] : nullptr; + if (rep.voxelized) { + vox.add(x, y, z, rgb); + } 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]});} + } + } + seen += c.size(); + if (seen >= next_report) { + say(sformat(" %.0f million points read, %.0f thousand voxels", seen / 1e6, + vox.voxels() / 1e3)); + next_report += 20000000; + } + }); + } + if (rep.points_used < 3) {throw std::runtime_error("fewer than 3 points inside the window");} + PointSet pts = rep.voxelized ? vox.centroids(opt.min_voxel_points) : std::move(raw); + rep.voxels = rep.voxelized ? vox.voxels() : 0; + rep.dropped = vox.dropped(); + rep.points_meshed = pts.size(); + if (rep.voxelized) { + say(sformat("voxels of %.3f m: %.1f million points -> %.2f million", voxel, + rep.points_used / 1e6, + pts.size() / 1e6)); + } + if (pts.size() < 3) {throw std::runtime_error("fewer than 3 points left after the reduction");} + if (opt.recenter) { + float x0 = 1e30f, x1 = -1e30f, y0 = 1e30f, y1 = -1e30f; + for (std::size_t k = 0; k < pts.size(); ++k) { + x0 = std::min(x0, pts.xyz[3 * k]); x1 = std::max(x1, pts.xyz[3 * k]); + y0 = std::min(y0, pts.xyz[3 * k + 1]); y1 = std::max(y1, pts.xyz[3 * k + 1]); + } + // a whole number of cells, so that the lattice of the map does not move + rep.offset_x = -std::round(0.5 * (x0 + x1) / opt.resolution) * opt.resolution; + rep.offset_y = -std::round(0.5 * (y0 + y1) / opt.resolution) * opt.resolution; + for (std::size_t k = 0; k < pts.size(); ++k) { + pts.xyz[3 * k] += static_cast(rep.offset_x); + pts.xyz[3 * k + 1] += static_cast(rep.offset_y); + } + say(sformat("recentred: shifted by (%.1f, %.1f) m", rep.offset_x, rep.offset_y)); + } + + // How many points each cell of the map gets, to say whether the data holds this resolution + { + std::unordered_map per_cell; + per_cell.reserve(pts.size() / 2 + 1); + for (std::size_t k = 0; k < pts.size(); ++k) { + const auto i = static_cast(std::floor(pts.xyz[3 * k] / opt.resolution)); + const auto j = static_cast(std::floor(pts.xyz[3 * k + 1] / opt.resolution)); + CellCount & c = per_cell[cell_key(i, j)]; + c.raw += pts.count[k]; + ++c.kept; + } + std::vector raw_n, kept_n; + raw_n.reserve(per_cell.size()); + kept_n.reserve(per_cell.size()); + for (const auto & kv : per_cell) { + raw_n.push_back(kv.second.raw); + kept_n.push_back(kv.second.kept); + } + std::nth_element(raw_n.begin(), raw_n.begin() + raw_n.size() / 2, raw_n.end()); + std::nth_element(kept_n.begin(), kept_n.begin() + kept_n.size() / 2, kept_n.end()); + rep.median_points_per_cell = raw_n[raw_n.size() / 2]; + const double density = rep.median_points_per_cell / + (static_cast(opt.resolution) * opt.resolution); + rep.supported_resolution = density > 0 ? std::sqrt(3.0 / density) : 0.0; + if (rep.median_points_per_cell < 3.0) { + rep.warnings.push_back(sformat( + "only %.1f points per cell at %.2f m: the mesher fits a surface from about 3, below " + "that a " + "height is a single point's. Fine if the cloud is already a grid of this resolution; " + "otherwise holes and rough ground follow. The data supports %.2f m", + rep.median_points_per_cell, opt.resolution, rep.supported_resolution)); + } else if (rep.voxelized && kept_n[kept_n.size() / 2] < 4) { + rep.warnings.push_back(sformat( + "the voxel reduction leaves %.0f points per cell: the surface fit near borders and " + "holes suffers. Use a smaller --voxel", + static_cast(kept_n[kept_n.size() / 2]))); + } + } + + pcl::PointCloud cloud; + cloud.reserve(pts.size()); + for (std::size_t k = 0; k < pts.size(); ++k) { + cloud.push_back(pcl::PointXYZ(pts.xyz[3 * k], pts.xyz[3 * k + 1], pts.xyz[3 * k + 2])); + } + navmap_ros::BuildParams bp; + bp.resolution = opt.resolution; + bp.max_slope_deg = opt.max_slope_deg; + bp.max_dz = opt.max_dz; + bp.min_clearance = opt.min_clearance; + bp.max_hole_area = opt.max_hole_area; + bp.min_surface_area = opt.min_surface_area; + bp.max_surfaces = opt.max_surfaces; + say(sformat("meshing %.2f million points at %.2f m", pts.size() / 1e6, opt.resolution)); + navmap_ros::from_points(cloud, out, bp); + if (opt.fill_holes > 0.0f && !out.positions_x.empty()) { + // the holes of the floors: floor points are added there (four per cell, at the height in + // between) and the cloud is meshed again, so that the new triangles join the old ones + const auto nm = navmap_ros::from_msg(out); + const auto cells = floor_hole_cells(nm, opt.resolution, opt.fill_holes); + rep.filled_cells = cells.size(); + if (!cells.empty()) { + const float q = 0.25f * opt.resolution; + for (const auto & c : cells) { + for (const auto & d : {std::pair{-q, -q}, std::pair{q, -q}, std::pair{q, q}, + std::pair{-q, q}}) + { + cloud.push_back(pcl::PointXYZ(c.x + d.first, c.y + d.second, c.z)); + pts.xyz.insert(pts.xyz.end(), {c.x + d.first, c.y + d.second, c.z}); + pts.count.push_back(1); + if (pts.coloured()) {pts.rgb.insert(pts.rgb.end(), {128, 128, 128});} + } + } + say(sformat("%.0f cells of floor over holes of the map: meshing again", + static_cast(cells.size()))); + out = navmap_ros_interfaces::msg::NavMap(); + navmap_ros::from_points(cloud, out, bp); + } + } + if (pts.coloured() && !out.positions_x.empty()) { + say("colouring the vertices"); + colour_vertices(out, pts, opt.resolution, std::max(0.4f, 2.0f * opt.max_dz)); + rep.coloured = true; + } + } + rep.vertices = out.positions_x.size(); + rep.triangles = out.navcels_v0.size(); + if (rep.vertices > 0) { + float lo = out.positions_z[0], hi = lo; + for (const float z : out.positions_z) { + lo = std::min(lo, z); hi = std::max(hi, z); + } + const int bands = static_cast((hi - lo) / 0.5f) + 1; + std::vector count(bands, 0); + for (const float z : out.positions_z) { + ++count[static_cast((z - lo) / 0.5f)]; + } + for (int b = 0; b < bands; ++b) { // bands holding >= 2 % of the vertices + if (count[b] * 50 >= rep.vertices) {rep.levels.emplace_back(lo + 0.5f * b, count[b]);} + } + } + if (rep.vertices == 0 || rep.triangles == 0) { + throw std::runtime_error( + "the cloud produced an empty NavMap: check the resolution, max_slope_deg and the " + "window against the data"); + } + out.header.frame_id = opt.frame_id; + rep.seconds = std::chrono::duration(clock::now() - t0).count(); + return rep; +} + +} // namespace navmap_tools diff --git a/navmap_tools/src/floor_holes.cpp b/navmap_tools/src/floor_holes.cpp new file mode 100644 index 0000000..237a8e2 --- /dev/null +++ b/navmap_tools/src/floor_holes.cpp @@ -0,0 +1,134 @@ +// 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. + +#include "navmap_tools/floor_holes.hpp" + +#include +#include +#include +#include +#include + +namespace navmap_tools +{ + +namespace +{ +constexpr int kBias = 1 << 20; // 21 bits per index, +-1 M cells + +std::uint64_t pack2(int a, int b) +{ + return (static_cast(a + kBias) << 21) | static_cast(b + kBias); +} + +std::pair split(std::uint64_t k) +{ + const std::uint64_t m = (1u << 21) - 1; + return {static_cast((k >> 21) & m) - kBias, static_cast(k & m) - kBias}; +} + +// the surfaces of the NavMap per column of the grid, sorted by height +using Columns = std::unordered_map>; + +Columns raster_floors(const ::navmap::NavMap & map, float size) +{ + Columns cols; + const auto & P = map.positions; + for (const auto & c : map.navcels) { + const double x0 = P.x[c.v[0]], y0 = P.y[c.v[0]], z0 = P.z[c.v[0]]; + const double x1 = P.x[c.v[1]], y1 = P.y[c.v[1]], z1 = P.z[c.v[1]]; + const double x2 = P.x[c.v[2]], y2 = P.y[c.v[2]], z2 = P.z[c.v[2]]; + const double det = (y1 - y2) * (x0 - x2) + (x2 - x1) * (y0 - y2); + if (std::abs(det) < 1e-12) {continue;} + const int i0 = static_cast(std::floor(std::min({x0, x1, x2}) / size)); + const int i1 = static_cast(std::floor(std::max({x0, x1, x2}) / size)); + const int j0 = static_cast(std::floor(std::min({y0, y1, y2}) / size)); + const int j1 = static_cast(std::floor(std::max({y0, y1, y2}) / size)); + for (int j = j0; j <= j1; ++j) { + for (int i = i0; i <= i1; ++i) { + const double x = (i + 0.5) * size, y = (j + 0.5) * size; + const double l0 = ((y1 - y2) * (x - x2) + (x2 - x1) * (y - y2)) / det; + const double l1 = ((y2 - y0) * (x - x2) + (x0 - x2) * (y - y2)) / det; + const double l2 = 1.0 - l0 - l1; + constexpr double kEps = -1e-9; + if (l0 < kEps || l1 < kEps || l2 < kEps) {continue;} + cols[pack2(i, j)].push_back(static_cast(l0 * z0 + l1 * z1 + l2 * z2)); + } + } + } + for (auto & kv : cols) { + std::sort(kv.second.begin(), kv.second.end()); + } + return cols; +} +} // namespace + +std::vector floor_hole_cells(const ::navmap::NavMap & map, float cell, float max_width) +{ + std::vector out; + const int span = static_cast(std::round(max_width / cell)); + if (span <= 0) {return out;} + const Columns cols = raster_floors(map, cell); + + const auto near_height = [&](std::uint64_t col, float h, float tol, float * found) { + const auto it = cols.find(col); + if (it == cols.end()) {return false;} + for (const float c : it->second) { + if (std::abs(c - h) <= tol) { + if (found) {*found = c;} + return true; + } + } + return false; + }; + + std::unordered_map> add; + const std::pair dirs[2] = {{1, 0}, {0, 1}}; + for (const auto & kv : cols) { + const auto [a0, b0] = split(kv.first); + for (const float h : kv.second) { + for (const auto & [da, db] : dirs) { + // is the next cell this floor? then there is no hole this way + if (near_height(pack2(a0 + da, b0 + db), h, 0.35f, nullptr)) {continue;} + for (int n = 2; n <= span + 1; ++n) { // the other end, n cells away (n - 1 empty) + float h2 = 0.0f; + const float tol = 0.2f + 0.6f * static_cast(n) * cell; // up to ~30 deg + if (!near_height(pack2(a0 + n * da, b0 + n * db), h, tol, &h2)) {continue;} + for (int i = 1; i < n; ++i) { // fill what the stretch between lacks + const float hi = h + (h2 - h) * static_cast(i) / static_cast(n); + const auto q = pack2(a0 + i * da, b0 + i * db); + if (near_height(q, hi, 0.25f, nullptr)) {continue;} + auto & list = add[q]; + if (std::none_of( + list.begin(), list.end(), [&](float c) {return std::abs(c - hi) < 0.25f;})) + { + list.push_back(hi); + } + } + break; // the nearest other end only + } + } + } + } + for (const auto & [q, levels] : add) { + const auto [a, b] = split(q); + for (const float h : levels) { + out.push_back( + {(static_cast(a) + 0.5f) * cell, (static_cast(b) + 0.5f) * cell, h}); + } + } + return out; +} + +} // namespace navmap_tools diff --git a/navmap_tools/src/pcd_stream.cpp b/navmap_tools/src/pcd_stream.cpp new file mode 100644 index 0000000..9d114e9 --- /dev/null +++ b/navmap_tools/src/pcd_stream.cpp @@ -0,0 +1,315 @@ +// 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. + +#include "navmap_tools/pcd_stream.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "pcl/PCLPointCloud2.h" +#include "pcl/io/pcd_io.h" + +namespace navmap_tools +{ + +namespace +{ +struct Header +{ + std::vector fields; + std::vector size, count; + std::vector type; + std::size_t width{0}, height{1}, points{0}; + std::string data; + std::streamoff data_start{0}; +}; + +std::vector words(const std::string & line) +{ + std::istringstream is(line); + std::vector w; + for (std::string s; is >> s; ) { + w.push_back(s); + } + return w; +} + +Header parse_header(std::ifstream & f, const std::string & path) +{ + Header h; + std::string line; + bool have_points = false; + while (std::getline(f, line)) { + if (!line.empty() && line.back() == '\r') {line.pop_back();} + const auto w = words(line); + if (w.empty() || w[0][0] == '#') {continue;} + if (w[0] == "FIELDS" || w[0] == "COLUMNS") { + h.fields.assign(w.begin() + 1, w.end()); + } else if (w[0] == "SIZE") { + for (std::size_t i = 1; i < w.size(); ++i) { + h.size.push_back(std::stoi(w[i])); + } + } else if (w[0] == "TYPE") { + for (std::size_t i = 1; i < w.size(); ++i) { + h.type.push_back(w[i][0]); + } + } else if (w[0] == "COUNT") { + for (std::size_t i = 1; i < w.size(); ++i) { + h.count.push_back(std::stoi(w[i])); + } + } else if (w[0] == "WIDTH" && w.size() > 1) { + h.width = std::stoull(w[1]); + } else if (w[0] == "HEIGHT" && w.size() > 1) { + h.height = std::stoull(w[1]); + } else if (w[0] == "POINTS" && w.size() > 1) { + h.points = std::stoull(w[1]); + have_points = true; + } else if (w[0] == "DATA" && w.size() > 1) { + h.data = w[1]; + h.data_start = f.tellg(); + break; + } + } + if (h.data.empty()) {throw std::runtime_error(path + ": not a PCD file (no DATA line)");} + if (!have_points) {h.points = h.width * h.height;} + if (h.count.empty()) {h.count.assign(h.fields.size(), 1);} + if (h.fields.size() != h.size.size() || h.fields.size() != h.type.size() || + h.fields.size() != h.count.size()) + { + throw std::runtime_error(path + ": inconsistent FIELDS/SIZE/TYPE/COUNT"); + } + return h; +} + +int index_of(const Header & h, const char * name) +{ + for (std::size_t i = 0; i < h.fields.size(); ++i) { + if (h.fields[i] == name) {return static_cast(i);} + } + return -1; +} + +/// Where x, y, z and the colour are inside one binary record +struct Layout +{ + std::size_t record{0}; + int ox{-1}, oy{-1}, oz{-1}; + bool dbl{false}; + int orgb{-1}; + int or_{-1}, og{-1}, ob{-1}; + bool has_color() const {return orgb >= 0 || (or_ >= 0 && og >= 0 && ob >= 0);} +}; + +Layout layout_of(const Header & h, const std::string & path) +{ + Layout l; + std::vector off(h.fields.size()); + std::size_t at = 0; + for (std::size_t i = 0; i < h.fields.size(); ++i) { + off[i] = at; + at += static_cast(h.size[i]) * h.count[i]; + } + l.record = at; + const int ix = index_of(h, "x"), iy = index_of(h, "y"), iz = index_of(h, "z"); + if (ix < 0 || iy < 0 || iz < 0) {throw std::runtime_error(path + ": no x, y, z fields");} + for (const int i : {ix, iy, iz}) { + if (h.type[i] != 'F' || (h.size[i] != 4 && h.size[i] != 8)) { + throw std::runtime_error(path + ": x, y, z must be float32 or float64"); + } + } + l.dbl = h.size[ix] == 8; + l.ox = static_cast(off[ix]); + l.oy = static_cast(off[iy]); + l.oz = static_cast(off[iz]); + int c = index_of(h, "rgb"); + if (c < 0) {c = index_of(h, "rgba");} + if (c >= 0 && h.size[c] == 4) {l.orgb = static_cast(off[c]);} + const int ir = index_of(h, "r"), ig = index_of(h, "g"), ib = index_of(h, "b"); + if (l.orgb < 0 && ir >= 0 && ig >= 0 && ib >= 0 && h.size[ir] == 1) { + l.or_ = static_cast(off[ir]); + l.og = static_cast(off[ig]); + l.ob = static_cast(off[ib]); + } + return l; +} + +template +T load(const std::uint8_t * p) +{ + T v; + std::memcpy(&v, p, sizeof(T)); + return v; +} + +void extract(const Layout & l, const std::uint8_t * data, std::size_t n, PcdChunk & out) +{ + const bool color = l.has_color(); + for (std::size_t k = 0; k < n; ++k) { + const std::uint8_t * r = data + k * l.record; + float x, y, z; + if (l.dbl) { + x = static_cast(load(r + l.ox)); + y = static_cast(load(r + l.oy)); + z = static_cast(load(r + l.oz)); + } else { + x = load(r + l.ox); + y = load(r + l.oy); + z = load(r + l.oz); + } + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {continue;} + out.xyz.insert(out.xyz.end(), {x, y, z}); + if (color) { + if (l.orgb >= 0) { // packed 0x00RRGGBB, little endian: bytes B, G, R, A + out.rgb.insert(out.rgb.end(), {r[l.orgb + 2], r[l.orgb + 1], r[l.orgb]}); + } else { + out.rgb.insert(out.rgb.end(), {r[l.or_], r[l.og], r[l.ob]}); + } + } + } +} +} // namespace + +PcdInfo pcd_info(const std::string & path) +{ + std::ifstream f(path, std::ios::binary); + if (!f) {throw std::runtime_error("cannot open " + path);} + const Header h = parse_header(f, path); + PcdInfo info; + info.points = h.points; + info.width = h.width; + info.height = h.height; + info.data = h.data; + info.has_color = layout_of(h, path).has_color(); + info.streamed = h.data == "ascii" || h.data == "binary"; + return info; +} + +PcdInfo read_pcd( + const std::string & path, const std::function & sink, + std::size_t chunk_points) +{ + std::ifstream f(path, std::ios::binary); + if (!f) {throw std::runtime_error("cannot open " + path);} + const Header h = parse_header(f, path); + const Layout l = layout_of(h, path); + PcdInfo info; + info.points = h.points; + info.width = h.width; + info.height = h.height; + info.data = h.data; + info.has_color = l.has_color(); + info.streamed = h.data == "ascii" || h.data == "binary"; + chunk_points = std::max(chunk_points, 1); + + if (h.data == "binary") { + std::vector buf(chunk_points * l.record); + std::size_t left = h.points; + while (left > 0) { + const std::size_t n = std::min(left, chunk_points); + f.read(reinterpret_cast(buf.data()), static_cast(n * l.record)); + const std::size_t got = static_cast(f.gcount()) / l.record; + if (got == 0) {break;} + PcdChunk c; + c.xyz.reserve(got * 3); + extract(l, buf.data(), got, c); + if (c.size() > 0) {sink(c);} + if (got < n) {break;} + left -= n; + } + } else if (h.data == "ascii") { + std::vector tok(h.fields.size()); // first token of each field + int at = 0; + for (std::size_t i = 0; i < h.fields.size(); ++i) { + tok[i] = at; at += h.count[i]; + } + const int ix = index_of(h, "x"), iy = index_of(h, "y"), iz = index_of(h, "z"); + int ic = index_of(h, "rgb"); + if (ic < 0) {ic = index_of(h, "rgba");} + const int ir = index_of(h, "r"), ig = index_of(h, "g"), ib = index_of(h, "b"); + const bool packed = ic >= 0 && l.orgb >= 0; + const bool separate = !packed && l.or_ >= 0; + std::vector v(static_cast(at)); + PcdChunk c; + std::string line; + std::size_t done = 0; + while (done < h.points && std::getline(f, line)) { + const char * p = line.c_str(); + char * end = nullptr; + int n = 0; + for (; n < at; ++n) { + v[static_cast(n)] = std::strtod(p, &end); + if (end == p) {break;} + p = end; + } + if (n < at) {continue;} + ++done; + const float x = static_cast(v[tok[ix]]), y = static_cast(v[tok[iy]]), + z = static_cast(v[tok[iz]]); + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {continue;} + c.xyz.insert(c.xyz.end(), {x, y, z}); + if (packed) { + std::uint32_t u; + if (h.type[ic] == 'F') { + const float fv = static_cast(v[tok[ic]]); + std::memcpy(&u, &fv, 4); + } else { + u = static_cast(v[tok[ic]]); + } + c.rgb.insert(c.rgb.end(), {static_cast(u >> 16), + static_cast(u >> 8), static_cast(u)}); + } else if (separate) { + c.rgb.insert(c.rgb.end(), {static_cast(v[tok[ir]]), + static_cast(v[tok[ig]]), static_cast(v[tok[ib]])}); + } + if (c.size() >= chunk_points) {sink(c); c = PcdChunk();} + } + if (c.size() > 0) {sink(c);} + } else { // binary_compressed: the whole table is one LZF block + pcl::PCLPointCloud2 blob; + if (pcl::PCDReader().read(path, blob) < 0) { + throw std::runtime_error("PCL cannot read " + path); + } + Layout lc; + lc.record = blob.point_step; + for (const auto & fl : blob.fields) { + if (fl.name == "x") { + lc.ox = static_cast(fl.offset); lc.dbl = fl.datatype == pcl::PCLPointField::FLOAT64; + } + if (fl.name == "y") {lc.oy = static_cast(fl.offset);} + if (fl.name == "z") {lc.oz = static_cast(fl.offset);} + if ((fl.name == "rgb" || fl.name == "rgba") && lc.orgb < 0) { + lc.orgb = static_cast(fl.offset); + } + } + if (lc.ox < 0 || lc.oy < 0 || lc.oz < 0) { + throw std::runtime_error(path + ": no x, y, z fields"); + } + info.has_color = lc.has_color(); + const std::size_t total = blob.width * blob.height; + for (std::size_t at = 0; at < total; at += chunk_points) { + const std::size_t n = std::min(chunk_points, total - at); + PcdChunk c; + extract(lc, blob.data.data() + at * lc.record, n, c); + if (c.size() > 0) {sink(c);} + } + } + return info; +} + +} // namespace navmap_tools diff --git a/navmap_tools/src/pointcloud_to_navmap.cpp b/navmap_tools/src/pointcloud_to_navmap.cpp index c4b637a..7bf2bcf 100644 --- a/navmap_tools/src/pointcloud_to_navmap.cpp +++ b/navmap_tools/src/pointcloud_to_navmap.cpp @@ -12,15 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Builds a .navmap file from a point cloud (.pcd) using the existing -// navmap_ros::from_points() mesher -- the resolution/max-slope-deg -// downsampling and filtering already implemented and tested there, not -// reimplemented here. Optionally colors the resulting mesh's vertices -// (the only real "texture/RGBD" hook NavMap has, see navmap_core/NavMap.hpp -// Colors) by nearest-neighbour lookup against a parallel r,g,b CSV that -// navmap_tools' Python side writes alongside the .pcd (see pcd_writer.py for -// why that's a plain CSV and not PCL's packed-float "rgb" PCD field). +// Builds a .navmap file from a point cloud (.pcd). +// +// Two paths, chosen by the input: +// * an organized (grid-shaped) cloud, which is what navmap_tools' Python side writes, or --colors: +// the cloud is loaded whole and meshed by navmap_ros::from_regular_grid / from_points, the +// connectivity of the grid being already known. --colors colours the mesh's vertices (the only +// real "texture/RGBD" hook NavMap has, see navmap_core/NavMap.hpp Colors) by nearest-neighbour +// lookup against a parallel r,g,b CSV (see pcd_writer.py for why that is a plain CSV and not +// PCL's packed-float "rgb" PCD field). +// * any other cloud (a SLAM map, a LiDAR scan, tens of millions of points), or any option of the +// second group: streamed, never held whole, reduced by voxel, and meshed in several floors by +// navmap_tools::cloud_to_navmap, which also closes the holes a SLAM leaves in the floor. +#include +#include +#include #include #include #include @@ -32,57 +39,149 @@ #include "pcl/search/kdtree.h" #include "navmap_ros/conversions.hpp" #include "navmap_ros/navmap_io.hpp" +#include "navmap_tools/cloud_to_navmap.hpp" namespace { struct Args { - std::string input; + std::vector inputs; std::string output; std::string colors; - std::string frame_id = "map"; - float resolution = 1.0f; - float max_slope_deg = 30.0f; + navmap_tools::Options opt; + bool streaming_options{false}; // an option only the streaming path has was given + bool georef{false}; + double origin[3] = {0, 0, 0}; }; -bool parse_args(int argc, char * argv[], Args & args) +void usage() +{ + std::cout << + "usage: pointcloud_to_navmap --input [--input ...] " + "--output [options]\n" + "\n" + " --resolution m finest NavCel (default 1.0); for bigmap, lengths.cell_size.min\n" + " --max-slope-deg d triangles steeper than this are not made (30)\n" + " --frame-id id frame of the map (map)\n" + " --colors file.csv colour the vertices from a row-aligned r,g,b CSV (one input, loaded\n" + " whole; not combined with the options below)\n" + "\n" + "Options of the streaming mesher. Used for every cloud that is not organized (a grid, as\n" + "navmap_gis_tool writes), or when any of them is given. ascii and binary .pcd files are\n" + "streamed (never held whole); big clouds are reduced by voxel first, so 100 million points\n" + "need as much memory as the map's extent, not as the points. The mesh keeps several floors,\n" + "the slopes and what is too steep, and takes the colour of the points (rgb field).\n" + "\n" + " --max-dz m vertical step of a surface / spread that means clutter (0.25)\n" + " --min-clearance m free height that separates two floors in a cell, the robot's (1.0)\n" + " --max-hole-area m2 enclosed empty regions up to this are interpolated (1.0)\n" + " --fill-holes m close the holes in the floors up to this wide, between two bits\n" + " of the same floor (2.0; 0: leave them): a SLAM cloud has no floor\n" + " points where it did not see it, and ground the robot could drive\n" + " over is cut\n" + " --min-surface-area m2 drop pieces of the map smaller than this (0)\n" + " --max-surfaces n keep only the n largest pieces (0: all)\n" + " --voxel m pre-reduction voxel: automatic by default (none up to 5 million\n" + " points, resolution / 4 above), 0 = never\n" + " --min-voxel-points n drop voxels with fewer points (noise; 1)\n" + " --crop x0,y0,x1,y1 only this window of the cloud\n" + " --zmin m --zmax m only these heights (a ceiling above the robot, say)\n" + " --origin lat,lon,alt where the map's (0, 0, 0) is: writes .yaml as georeferenced\n" + " --recenter shift the map so that its centre is (0, 0) (the shift is written\n" + " in .yaml as offset_x / offset_y): bigmap rasterises around its\n" + " origin\n" + "\n" + "The streaming path writes .yaml next to the map (origin, resolution, offset), in the\n" + "format bigmap reads.\n"; +} + +bool numbers(const std::string & s, double * v, int n) +{ + std::vector out; + std::size_t at = 0; + while (at <= s.size()) { + const auto c = s.find(',', at); + out.push_back(std::atof(s.substr(at, c == std::string::npos ? c : c - at).c_str())); + if (c == std::string::npos) {break;} + at = c + 1; + } + if (static_cast(out.size()) != n) {return false;} + for (int i = 0; i < n; ++i) { + v[i] = out[static_cast(i)]; + } + return true; +} + +// 0: go on, 1: done (--help), 2: bad arguments +int parse_args(int argc, char * argv[], Args & args) { + auto & opt = args.opt; for (int i = 1; i < argc; ++i) { - const std::string arg = argv[i]; - auto next = [&](const char * flag) -> const char * { - if (i + 1 >= argc) { - std::cerr << "missing value for " << flag << "\n"; - return nullptr; - } + const std::string a = argv[i]; + auto next = [&]() -> std::string { + if (i + 1 >= argc) {std::cerr << "missing value for " << a << "\n"; std::exit(2);} return argv[++i]; }; - if (arg == "--input") { - const char * v = next("--input"); if (!v) {return false;} args.input = v; - } else if (arg == "--output") { - const char * v = next("--output"); if (!v) {return false;} args.output = v; - } else if (arg == "--colors") { - const char * v = next("--colors"); if (!v) {return false;} args.colors = v; - } else if (arg == "--frame-id") { - const char * v = next("--frame-id"); if (!v) {return false;} args.frame_id = v; - } else if (arg == "--resolution") { - const char * v = next("--resolution"); if (!v) {return false;} - args.resolution = std::stof(v); - } else if (arg == "--max-slope-deg") { - const char * v = next("--max-slope-deg"); if (!v) {return false;} - args.max_slope_deg = std::stof(v); - } else { - std::cerr << "unknown argument: " << arg << "\n"; - return false; + if (a == "--help" || a == "-h") {usage(); return 1;} else if (a == "--input") { + args.inputs.push_back(next()); + } else if (a == "--output") {args.output = next();} else if (a == "--colors") { + args.colors = next(); + } else if (a == "--frame-id") {opt.frame_id = next();} else if (a == "--resolution") { + opt.resolution = std::stof(next()); + } else if (a == "--max-slope-deg") {opt.max_slope_deg = std::stof(next());} else { + args.streaming_options = true; + if (a == "--max-dz") {opt.max_dz = std::stof(next());} else if (a == "--min-clearance") { + opt.min_clearance = std::stof(next()); + } else if (a == "--max-hole-area") { + opt.max_hole_area = std::stof(next()); + } else if (a == "--fill-holes") { + opt.fill_holes = std::stof(next()); + } else if (a == "--min-surface-area") { + opt.min_surface_area = std::stof(next()); + } else if (a == "--max-surfaces") { + opt.max_surfaces = std::stoi(next()); + } else if (a == "--voxel") { + opt.voxel = std::stof(next()); + } else if (a == "--min-voxel-points") { + opt.min_voxel_points = static_cast(std::stoul(next())); + } else if (a == "--zmin") {opt.zmin = std::stod(next());} else if (a == "--zmax") { + opt.zmax = std::stod(next()); + } else if (a == "--recenter") {opt.recenter = true;} else if (a == "--crop") { + double w[4]; + if (!numbers(next(), w, 4)) {std::cerr << "--crop x0,y0,x1,y1\n"; return 2;} + opt.crop = true; opt.xmin = w[0]; opt.ymin = w[1]; opt.xmax = w[2]; opt.ymax = w[3]; + } else if (a == "--origin") { + if (!numbers(next(), args.origin, 3)) {std::cerr << "--origin lat,lon,alt\n"; return 2;} + args.georef = true; + } else { + std::cerr << "unknown argument: " << a << " (see --help)\n"; + return 2; + } } } - if (args.input.empty() || args.output.empty()) { - std::cerr << "usage: pointcloud_to_navmap --input --output " - "[--colors ] [--resolution 1.0] [--max-slope-deg 30.0] " - "[--frame-id map]\n"; - return false; + if (args.inputs.empty() || args.output.empty()) { + usage(); + return 2; } - return true; + if (!args.colors.empty() && (args.streaming_options || args.inputs.size() > 1)) { + std::cerr << "--colors takes one input and none of the streaming options (see --help)\n"; + return 2; + } + return 0; +} + +bool is_organized(const std::string & path) +{ + pcl::PCLPointCloud2 header; + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + int version = 0, type = 0; + unsigned int data_idx = 0; + if (pcl::PCDReader().readHeader(path, header, origin, orientation, version, type, data_idx) < 0) { + return false; // the streaming reader says what is wrong with the file + } + return header.height > 1; } // One "r,g,b" triple per line, same row order as the input .pcd. @@ -110,28 +209,23 @@ std::vector> load_colors_csv(const std::string & path) return colors; } -} // namespace - -int main(int argc, char * argv[]) +int run_whole_cloud(const Args & args) { - Args args; - if (!parse_args(argc, argv, args)) { - return 1; - } - + const std::string & input = args.inputs.front(); + const auto & opt = args.opt; pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - if (pcl::io::loadPCDFile(args.input, *cloud) != 0) { - std::cerr << "failed to load PCD: " << args.input << "\n"; + if (pcl::io::loadPCDFile(input, *cloud) != 0) { + std::cerr << "failed to load PCD: " << input << "\n"; return 1; } if (cloud->empty()) { - std::cerr << "input point cloud is empty: " << args.input << "\n"; + std::cerr << "input point cloud is empty: " << input << "\n"; return 1; } navmap_ros::BuildParams params; - params.resolution = args.resolution; - params.max_slope_deg = args.max_slope_deg; + params.resolution = opt.resolution; + params.max_slope_deg = opt.max_slope_deg; navmap_ros_interfaces::msg::NavMap out_msg; // navmap_tools always writes its own .pcd as an *organized* (grid-shaped) @@ -193,7 +287,7 @@ int main(int argc, char * argv[]) } } - out_msg.header.frame_id = args.frame_id; + out_msg.header.frame_id = opt.frame_id; std::error_code ec; if (!navmap_ros::io::save_msg_to_file(out_msg, args.output, {}, &ec)) { @@ -205,3 +299,72 @@ int main(int argc, char * argv[]) n_tris << " triangles" << (args.colors.empty() ? "" : ", colored") << ")\n"; return 0; } + +int run_streaming(Args & args) +{ + auto & opt = args.opt; + opt.log = [](const std::string & s) {std::cerr << "pointcloud_to_navmap: " << s << "\n";}; + + navmap_ros_interfaces::msg::NavMap msg; + navmap_tools::Report rep; + try { + rep = navmap_tools::cloud_to_navmap(args.inputs, opt, msg); + } catch (const std::exception & e) { + std::cerr << "pointcloud_to_navmap: " << e.what() << "\n"; + return 1; + } + std::error_code ec; + if (!navmap_ros::io::save_msg_to_file(msg, args.output, {}, &ec)) { + std::cerr << "pointcloud_to_navmap: cannot save " << args.output << ": " << ec.message() << + "\n"; + return 1; + } + // .yaml next to it, in the format bigmap reads: where the map's (0, 0, 0) is + { + std::string yaml = args.output; + const auto dot = yaml.rfind('.'); + yaml = (dot == std::string::npos ? yaml : yaml.substr(0, dot)) + ".yaml"; + std::ofstream f(yaml, std::ios::trunc); + f.precision(10); + f << "# Position of the map's (0, 0, 0). Without this file a map is loaded as a plain NavMap\n" + << "# without geolocation.\n" + << "georeferenced: " << (args.georef ? "true" : "false") << "\n" + << "origin_lat: " << args.origin[0] << "\n" + << "origin_lon: " << args.origin[1] << "\n" + << "reference_altitude: " << args.origin[2] << "\n" + << "min_resolution: " << opt.resolution << "\n" + << "# --recenter: x, y of the cloud + offset = x, y of this map\n" + << "offset_x: " << rep.offset_x << "\n" + << "offset_y: " << rep.offset_y << "\n"; + } + std::printf( + "wrote %s: %zu vertices, %zu triangles, %.2f m%s%s in %.1f s\n", args.output.c_str(), + rep.vertices, rep.triangles, static_cast(opt.resolution), + rep.coloured ? ", coloured" : "", rep.voxelized ? ", voxel-reduced" : "", rep.seconds); + std::printf( + " %zu points in the files, %zu used, %zu meshed; median %.1f points per cell" + " (this data holds %.2f m)\n", rep.points_in_files, rep.points_used, rep.points_meshed, + rep.median_points_per_cell, rep.supported_resolution); + std::printf(" surface at (m, vertices per 0.5 m band, >= 2 %%):"); + for (const auto & [z, n] : rep.levels) { + std::printf(" %.1f:%zu", static_cast(z), n); + } + std::printf("\n"); + for (const auto & w : rep.warnings) { + std::printf(" WARNING: %s\n", w.c_str()); + } + return 0; +} + +} // namespace + +int main(int argc, char * argv[]) +{ + Args args; + if (const int rc = parse_args(argc, argv, args)) { + return rc == 1 ? 0 : 2; + } + const bool whole = !args.colors.empty() || + (args.inputs.size() == 1 && !args.streaming_options && is_organized(args.inputs.front())); + return whole ? run_whole_cloud(args) : run_streaming(args); +} diff --git a/navmap_tools/src/voxel_cloud.cpp b/navmap_tools/src/voxel_cloud.cpp new file mode 100644 index 0000000..c5b0c08 --- /dev/null +++ b/navmap_tools/src/voxel_cloud.cpp @@ -0,0 +1,83 @@ +// 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. + +#include "navmap_tools/voxel_cloud.hpp" + +#include + +namespace navmap_tools +{ + +namespace +{ +constexpr std::int64_t kOffXY = 1 << 20, kOffZ = 1 << 21; // 21 + 21 + 22 bits +constexpr std::int64_t kMaxXY = (1 << 21) - 1, kMaxZ = (1 << 22) - 1; +} // namespace + +VoxelCloud::VoxelCloud(float voxel_xy, float voxel_z) +: vx_(voxel_xy), vz_(voxel_z) +{ + grid_.reserve(1u << 20); +} + +bool VoxelCloud::add(float x, float y, float z, const std::uint8_t * rgb) +{ + const std::int64_t ix = static_cast(std::floor(x / vx_)); + const std::int64_t iy = static_cast(std::floor(y / vx_)); + const std::int64_t iz = static_cast(std::floor(z / vz_)); + const std::int64_t kx = ix + kOffXY, ky = iy + kOffXY, kz = iz + kOffZ; + if (kx < 0 || kx > kMaxXY || ky < 0 || ky > kMaxXY || kz < 0 || kz > kMaxZ) { + ++dropped_; + return false; + } + const std::uint64_t key = (static_cast(kx) << 43) | + (static_cast(ky) << 22) | static_cast(kz); + Acc & a = grid_[key]; + a.sx += x - static_cast(ix) * vx_; + a.sy += y - static_cast(iy) * vx_; + a.sz += z - static_cast(iz) * vz_; + ++a.n; + if (rgb) { + a.r += rgb[0]; a.g += rgb[1]; a.b += rgb[2]; + coloured_ = true; + } + return true; +} + +PointSet VoxelCloud::centroids(std::uint32_t min_points) const +{ + PointSet out; + out.xyz.reserve(grid_.size() * 3); + out.count.reserve(grid_.size()); + if (coloured_) {out.rgb.reserve(grid_.size() * 3);} + for (const auto & [key, a] : grid_) { + if (a.n < min_points) {continue;} + const std::int64_t ix = static_cast(key >> 43) - kOffXY; + const std::int64_t iy = static_cast((key >> 22) & kMaxXY) - kOffXY; + const std::int64_t iz = static_cast(key & kMaxZ) - kOffZ; + const float inv = 1.0f / static_cast(a.n); + out.xyz.push_back(static_cast(ix) * vx_ + a.sx * inv); + out.xyz.push_back(static_cast(iy) * vx_ + a.sy * inv); + out.xyz.push_back(static_cast(iz) * vz_ + a.sz * inv); + out.count.push_back(a.n); + if (coloured_) { + out.rgb.push_back(static_cast(a.r / a.n)); + out.rgb.push_back(static_cast(a.g / a.n)); + out.rgb.push_back(static_cast(a.b / a.n)); + } + } + return out; +} + +} // namespace navmap_tools diff --git a/navmap_tools/test/data/dem_golden.bin b/navmap_tools/test/data/dem_golden.bin new file mode 100644 index 0000000000000000000000000000000000000000..d9fffe25d03649e868d49f6bd9920003f1de25af GIT binary patch literal 400 zcmXAjPblPZ7{=eWri_q{15$%lY+-_SwM#}T#M@aU`qO9P-OrWL z#jwn~1G4Mv6YafS8iE#a9Y2xa!4XG`zjH9M&Fq;FeNYqJ7Kq++!(Pv4)~>pk^0OG> zOM|}tG5R*0GTvMw8EfZ^`~agdom~qi14|XEqjyP#-b56w*d(FGEHt&uxJ%=hCw`tP z+NgzQa)IvM$R5fScj;fg80hAZ&tt$AQHj1+@ar=uZ(79U?niK4mi+p5$Ta6*y1&|@ ze`m_-dI!q<0G(Gs)e)@D7K;D%HHVx&*6P0)= i half+5..half+8, y -3..1 likewise; + # the roof's edges lie on cell borders, which the conservative rule counts: one more each way + holes = dem.cell_valid == 0 + assert holes[half - 3:half + 1, half + 5:half + 9].all() + assert holes.sum() <= 6 * 6 + assert not holes[half, half] and not holes[half + 10, half - 10] + assert 'cells without ground' in capsys.readouterr().out + # the same configuration without holes is another DEM: rebuilt, not reused + assert run(args) == 0 + assert read_dem_file(out).cell_valid is None + + +def _run_synthetic(tmp_path, *extra): + from navmap_tools.cli import run + + out = tmp_path / 'dem.bin' + rc = run(_BASE + ['--dem-source', 'synthetic', '--resolution', '2', '--dem-out', str(out), + '--cache-dir', str(tmp_path / 'c')] + list(extra)) + assert rc == 0 + return out + + +def test_dem_out_with_terrain_and_world_from_the_synthetic_source(tmp_path): + from navmap_tools.geo.dem_file import DemSource, read_dem_file + + template = tmp_path / 'w.in' + template.write_text('@LATITUDE@ @LONGITUDE@ @ELEVATION@ @TERRAIN_DIR@') + out = _run_synthetic( + tmp_path, '--dem-imagery', '--imagery-source', 'synthetic', '--zoom', '18', + '--terrain-out', str(tmp_path / 'terr'), '--terrain-resolution', '4', + '--world-template', str(template), '--world-out', str(tmp_path / 'world.sdf')) + dem = read_dem_file(out) + assert dem.source == DemSource.SYNTHETIC and dem.side == 21 + assert dem.imagery_zoom == 18 and dem.rgb is not None + terrain = tmp_path / 'terr' + assert (terrain / 'terrain.stl').is_file() and (terrain / 'terrain.png').is_file() + assert str((tmp_path / 'terr').resolve()) in (tmp_path / 'world.sdf').read_text() + + +def test_the_dem_is_reused_when_nothing_changed_and_rebuilt_when_it_did(tmp_path, capsys): + out = _run_synthetic(tmp_path) + first = out.read_bytes() + capsys.readouterr() + _run_synthetic(tmp_path) + assert 'same configuration' in capsys.readouterr().out + assert out.read_bytes() == first + _run_synthetic(tmp_path, '--reference-altitude', '600') # another configuration + assert out.read_bytes() != first + + +def test_force_refresh_builds_the_dem_again(tmp_path, capsys): + _run_synthetic(tmp_path) + capsys.readouterr() + _run_synthetic(tmp_path, '--force-refresh') + assert 'same configuration' not in capsys.readouterr().out + + +def test_a_dem_that_lacks_the_imagery_is_rebuilt_with_it(tmp_path): + from navmap_tools.geo.dem_file import read_dem_file + + out = _run_synthetic(tmp_path) + assert read_dem_file(out).rgb is None + _run_synthetic(tmp_path, '--dem-imagery', '--imagery-source', 'synthetic', '--zoom', '18') + assert read_dem_file(out).rgb is not None + + +def test_a_mesh_smaller_than_the_area_caps_the_radius(tmp_path, capsys): + import struct + + from navmap_tools.cli import run + from navmap_tools.geo.dem_file import read_dem_file + + stl = tmp_path / 'small.stl' + tris = [(-10, -10, 1, 10, -10, 1, 10, 10, 1), (-10, -10, 1, 10, 10, 1, -10, 10, 1)] + with open(stl, 'wb') as f: + f.write(b'\0' * 80 + struct.pack(' 10' in capsys.readouterr().out + + +def test_terrain_options_need_dem_out(): + from navmap_tools.cli import run + + with pytest.raises(SystemExit): + run(_BASE + ['--gazebo', '--terrain-out', 'x']) + with pytest.raises(SystemExit): + run(_BASE + ['--dem-out', 'd.bin', '--world-template', 't.in']) # no --terrain-out diff --git a/navmap_tools/test/test_cloud_to_navmap.cpp b/navmap_tools/test/test_cloud_to_navmap.cpp new file mode 100644 index 0000000..5efd866 --- /dev/null +++ b/navmap_tools/test/test_cloud_to_navmap.cpp @@ -0,0 +1,397 @@ +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_tools/cloud_to_navmap.hpp" +#include "navmap_tools/pcd_stream.hpp" +#include "navmap_tools/voxel_cloud.hpp" + +namespace +{ +struct Pt {float x, y, z; std::uint8_t r, g, b;}; + +std::string tmp(const std::string & name) +{ + return (std::filesystem::temp_directory_path() / ("navmap_tools_" + name)).string(); +} + +void write_pcd( + const std::string & path, const std::vector & pts, const std::string & data, + bool colour) +{ + std::ofstream f(path, std::ios::binary | std::ios::trunc); + f.precision(9); // enough digits for a float to come back bit for bit (the packed colour is one) + f << "# .PCD v0.7\nVERSION 0.7\nFIELDS x y z" << (colour ? " rgb" : "") << "\nSIZE 4 4 4" + << (colour ? " 4" : "") << "\nTYPE F F F" << (colour ? " F" : "") << "\nCOUNT 1 1 1" + << (colour ? " 1" : "") << "\nWIDTH " << pts.size() << + "\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS " + << pts.size() << "\nDATA " << data << "\n"; + for (const auto & p : pts) { + const std::uint32_t u = (static_cast(p.r) << + 16) | (static_cast(p.g) << 8) | p.b; + float rgbf; + std::memcpy(&rgbf, &u, 4); + if (data == "binary") { + f.write(reinterpret_cast(&p.x), 4); + f.write(reinterpret_cast(&p.y), 4); + f.write(reinterpret_cast(&p.z), 4); + if (colour) {f.write(reinterpret_cast(&rgbf), 4);} + } else { + f << p.x << " " << p.y << " " << p.z; + if (colour) {f << " " << rgbf;} + f << "\n"; + } + } +} + +// A 30 x 30 m floor (red) at z = 0 and a 15 x 30 m mezzanine (green) at z = 3 over half of it, with +// `spacing` between points, plus a little noise +std::vector two_floors(float spacing, unsigned seed = 1) +{ + std::mt19937 g(seed); + std::normal_distribution noise(0.0f, 0.01f); + std::vector pts; + for (float x = 0; x < 30.0f; x += spacing) { + for (float y = 0; y < 30.0f; y += spacing) { + pts.push_back({x, y, noise(g), 200, 20, 20}); + if (x >= 15.0f) {pts.push_back({x, y, 3.0f + noise(g), 20, 200, 20});} + } + } + return pts; +} + +std::set levels(const navmap_ros_interfaces::msg::NavMap & m) +{ + std::set z; + for (const float v : m.positions_z) { + z.insert(static_cast(std::lround(v))); + } + return z; +} +} // namespace + +TEST(PcdStream, BinaryAndAsciiGiveTheSamePointsAndColours) +{ + const auto pts = two_floors(1.0f); + for (const bool colour : {true, false}) { + for (const std::string data : {"binary", "ascii"}) { + const auto path = tmp("rt_" + data + (colour ? "_c" : "_n") + ".pcd"); + write_pcd(path, pts, data, colour); + const auto info = navmap_tools::pcd_info(path); + EXPECT_EQ(info.points, pts.size()); + EXPECT_EQ(info.has_color, colour); + EXPECT_TRUE(info.streamed); + std::size_t n = 0; + bool ok = true; + navmap_tools::read_pcd(path, [&](const navmap_tools::PcdChunk & c) { + for (std::size_t k = 0; k < c.size(); ++k, ++n) { + ok = ok && std::abs(c.xyz[3 * k] - pts[n].x) < 1e-3f && + std::abs(c.xyz[3 * k + 2] - pts[n].z) < 1e-3f; + if (colour) { + ok = ok && c.rgb[3 * k] == pts[n].r && c.rgb[3 * k + 1] == pts[n].g && + c.rgb[3 * k + 2] == pts[n].b; + } + } + }, 100); // many small blocks + EXPECT_EQ(n, pts.size()) << data; + EXPECT_TRUE(ok) << data << (colour ? " colour" : ""); + std::filesystem::remove(path); + } + } +} + +TEST(PcdStream, ABrokenFileSaysWhy) +{ + const auto path = tmp("broken.pcd"); + {std::ofstream f(path); f << "this is not a pcd\n";} + EXPECT_THROW(navmap_tools::pcd_info(path), std::runtime_error); + EXPECT_THROW(navmap_tools::pcd_info("/nonexistent/x.pcd"), std::runtime_error); + std::filesystem::remove(path); +} + +TEST(VoxelCloud, ReducesPointsToVoxelsAndKeepsThePlane) +{ + navmap_tools::VoxelCloud v(0.5f, 0.125f); + std::size_t n = 0; + for (int i = 0; i < 400; ++i) { + for (int j = 0; j < 400; ++j, ++n) { + const float x = 0.05f * static_cast(i), y = 0.05f * static_cast(j); + v.add(x, y, 0.3f + 0.1f * x, nullptr); // a ramp + } + } + const auto c = v.centroids(); + EXPECT_EQ(n, 400u * 400u); + // ~ (20 / 0.5)^2 columns, a few z bins on a slope + EXPECT_LT(c.size(), 20u * 2 * 20 * 2 * 3); + EXPECT_GT(c.size(), 1000u); + double worst = 0; + for (std::size_t k = 0; k < c.size(); ++k) { // every centroid is still on the ramp + worst = std::max(worst, std::abs(c.xyz[3 * k + 2] - (0.3f + 0.1f * c.xyz[3 * k]))); + } + EXPECT_LT(worst, 0.03); + std::uint32_t total = 0; + for (const auto k : c.count) { + total += k; + } + EXPECT_EQ(total, n); // no point is lost, only merged + EXPECT_TRUE(c.rgb.empty()); +} + +TEST(VoxelCloud, DropsIsolatedNoiseWhenAsked) +{ + navmap_tools::VoxelCloud v(0.5f, 0.125f); + for (int i = 0; i < 100; ++i) { + v.add(1.1f, 1.1f, 0.0f, nullptr); + } + v.add(8.0f, 8.0f, 5.0f, nullptr); // a lone point + EXPECT_EQ(v.centroids(1).size(), 2u); + EXPECT_EQ(v.centroids(2).size(), 1u); +} + +TEST(CloudToNavmap, TwoFloorsKeepBothAndTheirColours) +{ + const auto path = tmp("two.pcd"); + write_pcd(path, two_floors(0.25f), "binary", true); + navmap_tools::Options opt; + opt.resolution = 1.0f; + opt.min_clearance = 1.0f; + navmap_ros_interfaces::msg::NavMap m; + const auto rep = navmap_tools::cloud_to_navmap({path}, opt, m); + EXPECT_FALSE(rep.voxelized); + EXPECT_TRUE(rep.coloured); + const auto z = levels(m); + EXPECT_TRUE(z.count(0) && z.count(3)) << "both floors must be meshed"; + ASSERT_EQ(m.colors_r.size(), m.positions_z.size()); + std::size_t red0 = 0, n0 = 0, green3 = 0, n3 = 0; + for (std::size_t v = 0; v < m.positions_z.size(); ++v) { + if (std::abs(m.positions_z[v]) < 0.2f) { + ++n0; red0 += m.colors_r[v] > 150 && m.colors_g[v] < 80; + } + if (std::abs(m.positions_z[v] - 3.0f) < 0.2f) { + ++n3; green3 += m.colors_g[v] > 150 && m.colors_r[v] < 80; + } + } + EXPECT_GT(n0, 100u); + EXPECT_GT(n3, 100u); + EXPECT_EQ(red0, n0) << "the lower floor is red"; + EXPECT_EQ(green3, n3) << "the mezzanine is green, not the colour of the floor below it"; + EXPECT_EQ(m.header.frame_id, "map"); + std::filesystem::remove(path); +} + +TEST(CloudToNavmap, TheVoxelReductionGivesTheSameMapAsAllThePoints) +{ + const auto path = tmp("vox.pcd"); + write_pcd(path, two_floors(0.1f), "binary", true); // 200k points for 1 m cells + navmap_tools::Options opt; + opt.resolution = 1.0f; + navmap_ros_interfaces::msg::NavMap raw, red; + opt.voxel = 0.0f; + const auto r0 = navmap_tools::cloud_to_navmap({path}, opt, raw); + opt.voxel = 0.25f; + const auto r1 = navmap_tools::cloud_to_navmap({path}, opt, red); + EXPECT_FALSE(r0.voxelized); + EXPECT_TRUE(r1.voxelized); + EXPECT_LT(r1.points_meshed * 3, r0.points_meshed) << "far fewer points to mesh"; + EXPECT_EQ(levels(raw), levels(red)); + const double dv = std::abs(static_cast(raw.positions_x.size()) - red.positions_x.size()); + EXPECT_LT(dv, 0.05 * raw.positions_x.size()); + const double dt = std::abs(static_cast(raw.navcels_v0.size()) - red.navcels_v0.size()); + EXPECT_LT(dt, 0.05 * raw.navcels_v0.size()); + // The mesher's lattice starts at the lowest point, which the reduction moves by a few cm: the + // vertices are not the same, but both floors must be as flat and as high as with every point + for (const auto * m : {&raw, &red}) { + double worst = 0; + std::size_t bad = 0; + for (std::size_t v = 0; v < m->positions_z.size(); ++v) { + const float z = m->positions_z[v]; + const double e = std::min(std::abs(z), std::abs(z - 3.0f)); + worst = std::max(worst, e); + if (e > 0.05 && bad++ < 3) { + std::printf(" off-floor vertex %s at (%.2f, %.2f, %.2f)\n", m == &raw ? "raw" : "red", + m->positions_x[v], m->positions_y[v], z); + } + } + std::printf(" %s: %zu vertices, %zu off the floors (worst %.2f)\n", m == &raw ? "raw" : "red", + m->positions_z.size(), bad, worst); + EXPECT_LT(worst, 0.05); + } + std::filesystem::remove(path); +} + +TEST(CloudToNavmap, TooSparseDataIsReportedWithTheResolutionItHolds) +{ + const auto path = tmp("sparse.pcd"); + write_pcd(path, two_floors(0.9f), "binary", false); // ~1 point per 1 m cell + navmap_tools::Options opt; + opt.resolution = 1.0f; + navmap_ros_interfaces::msg::NavMap m; + const auto rep = navmap_tools::cloud_to_navmap({path}, opt, m); + EXPECT_LT(rep.median_points_per_cell, 3.0); + EXPECT_GT(rep.supported_resolution, 1.2) << "it must suggest a coarser resolution"; + ASSERT_FALSE(rep.warnings.empty()); + EXPECT_NE(rep.warnings[0].find("points per cell"), std::string::npos); + std::filesystem::remove(path); +} + +TEST(CloudToNavmap, CropAndHeightWindowSelectWhatIsUsed) +{ + const auto path = tmp("crop.pcd"); + write_pcd(path, two_floors(0.25f), "binary", false); + navmap_tools::Options opt; + opt.resolution = 1.0f; + opt.zmax = 1.0; // the mezzanine is a ceiling: left out + navmap_ros_interfaces::msg::NavMap m; + navmap_tools::cloud_to_navmap({path}, opt, m); + EXPECT_EQ(levels(m), (std::set{0})); + opt.zmax = 1e9; + opt.crop = true; opt.xmin = 0; opt.ymin = 0; opt.xmax = 10; opt.ymax = 10; + navmap_ros_interfaces::msg::NavMap c; + navmap_tools::cloud_to_navmap({path}, opt, c); + float xmax = 0, ymax = 0; + for (std::size_t v = 0; v < c.positions_x.size(); ++v) { + xmax = std::max(xmax, c.positions_x[v]); ymax = std::max(ymax, c.positions_y[v]); + } + EXPECT_LT(xmax, 11.0f); + EXPECT_LT(ymax, 11.0f); + std::filesystem::remove(path); +} + +TEST(CloudToNavmap, SeveralFilesAreJoined) +{ + const auto a = tmp("a.pcd"), b = tmp("b.pcd"); + auto pts = two_floors(0.25f); + std::vector first, second; + for (const auto & p : pts) { + (p.y < 15.0f ? first : second).push_back(p); + } + write_pcd(a, first, "binary", false); + write_pcd(b, second, "ascii", false); + navmap_tools::Options opt; + opt.resolution = 1.0f; + navmap_ros_interfaces::msg::NavMap m; + const auto rep = navmap_tools::cloud_to_navmap({a, b}, opt, m); + EXPECT_EQ(rep.points_in_files, pts.size()); + float ymin = 1e9f, ymax = -1e9f; + for (const float y : m.positions_y) { + ymin = std::min(ymin, y); ymax = std::max(ymax, y); + } + EXPECT_LT(ymin, 2.0f); + EXPECT_GT(ymax, 28.0f); + std::filesystem::remove(a); + std::filesystem::remove(b); +} + +TEST(CloudToNavmap, NothingToMeshSaysSo) +{ + const auto path = tmp("none.pcd"); + write_pcd(path, two_floors(0.5f), "binary", false); + navmap_tools::Options opt; + opt.crop = true; opt.xmin = 500; opt.ymin = 500; opt.xmax = 600; opt.ymax = 600; + navmap_ros_interfaces::msg::NavMap m; + EXPECT_THROW(navmap_tools::cloud_to_navmap({path}, opt, m), std::runtime_error); + std::filesystem::remove(path); +} + +TEST(CloudToNavmap, RecenterPutsTheMiddleOfTheMapAtTheOrigin) +{ + const auto path = tmp("recenter.pcd"); + auto pts = two_floors(0.25f); + for (auto & p : pts) { + p.x += 500.0f; p.y += 1200.0f; + } // far from the origin + write_pcd(path, pts, "binary", false); + navmap_tools::Options opt; + opt.resolution = 1.0f; + opt.recenter = true; + navmap_ros_interfaces::msg::NavMap m; + const auto rep = navmap_tools::cloud_to_navmap({path}, opt, m); + float x0 = 1e9f, x1 = -1e9f, y0 = 1e9f, y1 = -1e9f; + for (std::size_t v = 0; v < m.positions_x.size(); ++v) { + x0 = std::min(x0, m.positions_x[v]); x1 = std::max(x1, m.positions_x[v]); + y0 = std::min(y0, m.positions_y[v]); y1 = std::max(y1, m.positions_y[v]); + } + EXPECT_NEAR(0.5 * (x0 + x1), 0.0, 1.5); + EXPECT_NEAR(0.5 * (y0 + y1), 0.0, 1.5); + EXPECT_NEAR(rep.offset_x, -515.0, 1.5); + EXPECT_NEAR(rep.offset_y, -1215.0, 1.5); + std::filesystem::remove(path); +} + +namespace +{ +// a flat 30 x 30 m floor with spacing 0.25 and a rectangular hole [x0, x1] x [y0, y1] +// with no points +std::vector floor_with_hole(float x0, float x1, float y0, float y1) +{ + std::vector pts; + for (float x = 0; x < 30.0f; x += 0.25f) { + for (float y = 0; y < 30.0f; y += 0.25f) { + if (x >= x0 && x <= x1 && y >= y0 && y <= y1) {continue;} + pts.push_back({x, y, 0.0f, 128, 128, 128}); + } + } + return pts; +} + +// triangles whose centroid is in the window +std::size_t triangles_in( + const navmap_ros_interfaces::msg::NavMap & m, float x0, float x1, float y0, float y1) +{ + std::size_t n = 0; + for (std::size_t t = 0; t < m.navcels_v0.size(); ++t) { + const auto a = m.navcels_v0[t], b = m.navcels_v1[t], c = m.navcels_v2[t]; + const float cx = (m.positions_x[a] + m.positions_x[b] + m.positions_x[c]) / 3.0f; + const float cy = (m.positions_y[a] + m.positions_y[b] + m.positions_y[c]) / 3.0f; + n += cx > x0 && cx y0 && cy < y1; + } + return n; +} +} // namespace + +TEST(CloudToNavmap, ASmallHoleInTheFloorIsClosedAndALargeOneIsNot) +{ + const auto path = tmp("holes.pcd"); + auto pts = floor_with_hole(10.0f, 12.0f, 10.0f, 13.0f); // 2 x 3 m: narrower than 2.4 m + pts.erase(std::remove_if(pts.begin(), pts.end(), [](const Pt & p) { + return p.x >= 20.0f && p.x <= 27.0f && p.y >= 20.0f && p.y <= 27.0f; // 7 x 7 m + }), pts.end()); + write_pcd(path, pts, "binary", true); + navmap_tools::Options opt; + opt.resolution = 0.5f; + opt.fill_holes = 0.0f; + navmap_ros_interfaces::msg::NavMap open; + navmap_tools::cloud_to_navmap({path}, opt, open); + EXPECT_EQ(triangles_in(open, 10.4f, 11.6f, 10.4f, 12.6f), 0u) << "the hole is there"; + opt.fill_holes = 2.4f; + navmap_ros_interfaces::msg::NavMap closed; + const auto rep = navmap_tools::cloud_to_navmap({path}, opt, closed); + EXPECT_GT(rep.filled_cells, 0u); + EXPECT_GT(triangles_in(closed, 10.4f, 11.6f, 10.4f, 12.6f), 0u) << "the small hole is closed"; + EXPECT_EQ(triangles_in(closed, 22.0f, 25.0f, 22.0f, 25.0f), 0u) << "the 7 m hole is a real void"; + std::filesystem::remove(path); +} diff --git a/navmap_tools/test/test_dem.py b/navmap_tools/test/test_dem.py index 5eabe45..b99f237 100644 --- a/navmap_tools/test/test_dem.py +++ b/navmap_tools/test/test_dem.py @@ -270,3 +270,13 @@ def fake_fetch(lat_floor, lon_floor, cache, force=False): assert grid.north == pytest.approx(1.0) assert grid.pixel_size_lon == pytest.approx(0.5) assert grid.pixel_size_lat == pytest.approx(0.5) + + +def test_sample_array_agrees_with_the_scalar_sample(): + grid = _grid_2x2() + lon = np.array([0.0, 0.25, 0.5, 1.0, 3.0, -2.0]) + lat = np.array([1.0, 0.75, 0.5, 0.0, 0.5, 9.0]) # the last two clamp to the edge + got = grid.sample_array(lon, lat) + assert got.shape == (6,) + for k in range(6): + assert got[k] == pytest.approx(grid.sample(lon[k], lat[k])) diff --git a/navmap_tools/test/test_dem_file.py b/navmap_tools/test/test_dem_file.py new file mode 100644 index 0000000..2c0adcd --- /dev/null +++ b/navmap_tools/test/test_dem_file.py @@ -0,0 +1,269 @@ +# 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. +""" +Unit tests for navmap_tools.geo.dem_file -- the height-grid file bigmap_maps_manager reads. + +`test/data/dem_golden.bin` was made by the C++ side (BigMap's former DemGrid::build and save, from +the analytic function below); BigMap keeps the same file and checks it from C++. Here the +Python writer must reproduce its bytes and its sampling must agree with its values. +""" + +from pathlib import Path + +from navmap_tools.geo.dem_file import ( + add_imagery, + build_local_dem, + DemSource, + EnuProjection, + read_dem_file, + write_dem_file, +) + +import numpy as np + +import pytest + +GOLDEN = Path(__file__).parent / 'data' / 'dem_golden.bin' + + +class _Analytic: + """The function the golden file was made from.""" + + def sample_array(self, lon, lat): + return 650.0 + 30.0 * np.sin(lat * 20000.0) + 20.0 * np.cos(lon * 15000.0) + + +def _build(): + return build_local_dem( + _Analytic(), 40.4, -3.7, radius=4.0, resolution=1.0, source=DemSource.COPERNICUS, + data_resolution=2.0) + + +def test_python_build_agrees_with_the_cpp_golden_file(): + golden = read_dem_file(GOLDEN) + mine = _build() + assert mine.side == golden.side == 9 + assert mine.source == golden.source == DemSource.COPERNICUS + assert mine.resolution == golden.resolution + assert mine.data_resolution == golden.data_resolution == 2.0 + assert mine.reference_altitude == pytest.approx(golden.reference_altitude, abs=1e-4) + assert np.isnan(golden.requested_reference) and np.isnan(mine.requested_reference) + assert mine.z == pytest.approx(golden.z, abs=1e-3) # bicubic + ENU, as the C++ side + + +def test_python_writer_reproduces_the_bytes_of_the_cpp_writer(tmp_path): + out = tmp_path / 'again.bin' + write_dem_file(read_dem_file(GOLDEN), out) + assert out.read_bytes() == GOLDEN.read_bytes() + + +def test_origin_is_a_node_and_the_reference(): + dem = _build() + assert dem.z[4, 4] == pytest.approx(0.0, abs=1e-6) + + +def test_direct_sampling_when_data_resolution_is_not_coarser(): + dem = build_local_dem( + _Analytic(), 40.4, -3.7, radius=4.0, resolution=1.0, source=DemSource.IGN) + assert dem.data_resolution == 1.0 + assert dem.side == 9 + # row j is north, column i is east + proj = EnuProjection(40.4, -3.7) + lat, lon, _ = proj.to_geodetic(np.array([2.0]), np.array([-3.0]), 0.0) + expected = _Analytic().sample_array(lon, lat)[0] - dem.reference_altitude + assert dem.z[4 - 3, 4 + 2] == pytest.approx(expected, abs=1e-3) + + +def test_radius_that_is_not_a_multiple_rounds_the_grid_up(): + dem = build_local_dem( + _Analytic(), 40.4, -3.7, radius=4.2, resolution=1.0, source=DemSource.IGN) + assert dem.side == 11 # ceil(4.2) = 5 nodes each way + + +def test_given_reference_altitude_is_kept(): + dem = build_local_dem( + _Analytic(), 40.4, -3.7, radius=4.0, resolution=1.0, source=DemSource.IGN, + reference_altitude=600.0) + assert dem.reference_altitude == 600.0 + assert dem.requested_reference == 600.0 + assert dem.z[4, 4] == pytest.approx(_Analytic().sample_array( + np.array([-3.7]), np.array([40.4]))[0] - 600.0, abs=1e-3) + + +def test_scalar_samplers_work_too(): + class Scalar: + + def sample(self, lon, lat): + return 100.0 + + dem = build_local_dem(Scalar(), 40.4, -3.7, 3.0, 1.0, DemSource.MESH) + assert (dem.z == 0.0).all() + + +def test_bad_arguments_are_rejected(): + with pytest.raises(ValueError): + build_local_dem(_Analytic(), 40.4, -3.7, radius=0.0, resolution=1.0, source=2) + with pytest.raises(ValueError): + build_local_dem(_Analytic(), 40.4, -3.7, radius=10.0, resolution=-1.0, source=2) + with pytest.raises(ValueError): # over what DemGrid::load accepts + build_local_dem(_Analytic(), 40.4, -3.7, radius=30000.0, resolution=1.0, source=2) + + +def test_imagery_colours_round_trip(tmp_path): + class Red: + + def sample(self, lon, lat): + return (200, 10, 20) + + dem = build_local_dem(_Analytic(), 40.4, -3.7, 3.0, 1.0, DemSource.IGN) + add_imagery(dem, Red(), zoom=19) + path = tmp_path / 'dem.bin' + write_dem_file(dem, path) + back = read_dem_file(path) + assert back.imagery_zoom == 19 + assert back.rgb.shape == (7, 7, 3) + assert tuple(back.rgb[3, 3]) == (200, 10, 20) + assert back.z == pytest.approx(dem.z) + + +def test_file_without_imagery_has_no_colours(tmp_path): + dem = build_local_dem(_Analytic(), 40.4, -3.7, 3.0, 1.0, DemSource.IGN) + path = tmp_path / 'dem.bin' + write_dem_file(dem, path) + back = read_dem_file(path) + assert back.rgb is None and back.imagery_zoom == 0 + + +def test_the_cells_with_ground_round_trip_with_and_without_colours(tmp_path): + class Red: + + def sample(self, lon, lat): + return (200, 10, 20) + + for colours in (False, True): + dem = build_local_dem(_Analytic(), 40.4, -3.7, 3.0, 1.0, DemSource.IGN) + if colours: + add_imagery(dem, Red(), zoom=19) + cells = np.ones((6, 6), np.uint8) + cells[2, 4] = 0 # a hole at cell (4, 2) + dem.cell_valid = cells + path = tmp_path / 'dem.bin' + write_dem_file(dem, path) + back = read_dem_file(path) + assert np.array_equal(back.cell_valid, cells) + assert (back.rgb is not None) == colours + assert back.z == pytest.approx(dem.z) + assert read_dem_file(GOLDEN).cell_valid is None # older than the block: no holes + + +def test_a_broken_cells_block_and_the_wrong_number_of_cells_are_refused(tmp_path): + dem = build_local_dem(_Analytic(), 40.4, -3.7, 3.0, 1.0, DemSource.IGN) + dem.cell_valid = np.ones((6, 6), np.uint8) + path = tmp_path / 'dem.bin' + write_dem_file(dem, path) + data = path.read_bytes() + path.write_bytes(data[:-5]) + with pytest.raises(ValueError): + read_dem_file(path) + path.write_bytes(data[:-36 - 8] + b'XELLS\0\0\0' + data[-36:]) + with pytest.raises(ValueError): + read_dem_file(path) + dem.cell_valid = np.ones((5, 6), np.uint8) + with pytest.raises(ValueError): + write_dem_file(dem, tmp_path / 'other.bin') + + +def test_read_rejects_other_files(tmp_path): + path = tmp_path / 'junk.bin' + path.write_bytes(b'not a dem file at all') + with pytest.raises(ValueError): + read_dem_file(path) + + +def test_enu_matches_geodesy_at_the_origin_and_round_trips(): + proj = EnuProjection(40.4, -3.7) + e, n, u = proj.to_enu(np.array([40.4]), np.array([-3.7]), 0.0) + assert (abs(e[0]), abs(n[0]), abs(u[0])) < (1e-6, 1e-6, 1e-6) + lat, lon, alt = proj.to_geodetic(np.array([123.0]), np.array([-456.0]), 0.0) + e, n, _ = proj.to_enu(lat, lon, alt) + assert e[0] == pytest.approx(123.0, abs=1e-6) + assert n[0] == pytest.approx(-456.0, abs=1e-6) + # north is +y, east is +x + lat_n, lon_n, _ = proj.to_geodetic(np.array([0.0]), np.array([100.0]), 0.0) + assert lat_n[0] > 40.4 and lon_n[0] == pytest.approx(-3.7, abs=1e-6) + lat_e, lon_e, _ = proj.to_geodetic(np.array([100.0]), np.array([0.0]), 0.0) + assert lon_e[0] > -3.7 + + +def test_synthetic_dem_and_imagery_are_offline_and_deterministic(): + from navmap_tools.geo.dem_file import SyntheticDem, SyntheticImagery + dem = SyntheticDem(40.4, -3.7) + # at the origin x = y = 0: 650 + 20 * exp(-(80^2 + 60^2) / 3200) + 0 + 0 + assert dem.sample(-3.7, 40.4) == pytest.approx(650.0 + 20.0 * np.exp(-10000.0 / 3200.0)) + imagery = SyntheticImagery(40.4, -3.7) + assert imagery.sample(-3.7, 40.4) == (255, 255, 255) # on the white axes + r, g, b = imagery.sample_array(np.array([-3.6995]), np.array([40.4005]))[0] + assert g == 60 and (r, b) in ((200, 60), (60, 200)) # a square of the chessboard + + +def test_a_dem_file_made_from_a_synthetic_terrain_round_trips(tmp_path): + from navmap_tools.geo.dem_file import SyntheticDem, SyntheticImagery + dem = build_local_dem(SyntheticDem(40.4, -3.7), 40.4, -3.7, 10.0, 1.0, DemSource.SYNTHETIC) + add_imagery(dem, SyntheticImagery(40.4, -3.7), zoom=18) + write_dem_file(dem, tmp_path / 'd.bin') + back = read_dem_file(tmp_path / 'd.bin') + assert back.source == DemSource.SYNTHETIC and back.imagery_zoom == 18 + assert back.matches(40.4, -3.7, 10.0, 1.0, DemSource.SYNTHETIC) + + +def test_truncated_file_is_reported(tmp_path): + good = tmp_path / 'good.bin' + write_dem_file(build_local_dem(_Analytic(), 40.4, -3.7, 3.0, 1.0, DemSource.IGN), good) + short = tmp_path / 'short.bin' + short.write_bytes(good.read_bytes()[:100]) + with pytest.raises(ValueError): + read_dem_file(short) + + +def _patched_header(tmp_path, side=None, resolution=None, lat=None, reference=None): + """Return a good file with one header value replaced (a corrupt or hostile file).""" + import struct + good = tmp_path / 'good.bin' + write_dem_file(build_local_dem(_Analytic(), 40.4, -3.7, 3.0, 1.0, DemSource.IGN), good) + data = bytearray(good.read_bytes()) + s_, src, *hdr = struct.unpack_from(' + +#include +#include +#include + +#include "navmap_tools/floor_holes.hpp" + +namespace +{ +// A grid of n x n cells of 0.4 m at z = 0 with the cells of `hole` left out +::navmap::NavMap grid_floor(int n, const std::function & hole) +{ + ::navmap::NavMap nm; + nm.surfaces.resize(1); + for (int j = 0; j <= n; ++j) { + for (int i = 0; i <= n; ++i) { + nm.positions.x.push_back(0.4f * static_cast(i)); + nm.positions.y.push_back(0.4f * static_cast(j)); + nm.positions.z.push_back(0.0f); + } + } + const auto id = [n](int a, int b) {return static_cast(b * (n + 1) + a);}; + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + if (hole(i, j)) {continue;} + for (const auto & t : {std::array{id(i, j), id(i + 1, j), id(i + 1, j + 1)}, + std::array{id(i, j), id(i + 1, j + 1), id(i, j + 1)}}) + { + ::navmap::NavCel c; + c.v[0] = t[0]; c.v[1] = t[1]; c.v[2] = t[2]; + nm.surfaces[0].navcels.push_back(static_cast<::navmap::NavCelId>(nm.navcels.size())); + nm.navcels.push_back(c); + } + } + } + nm.rebuild_geometry_accels(); + return nm; +} +} // namespace + +TEST(FloorHoles, SmallHolesInTheFloorAreClosedAndLargeOnesAreNot) +{ + // a floor of 30 x 30 cells with a hole of 3 x 3 cells (1.2 m) and one of 10 x 10 (4 m) + const auto holes = [](int i, int j) { + return (i >= 5 && i < 8 && j >= 5 && j < 8) || (i >= 15 && i < 25 && j >= 15 && j < 25); + }; + const auto cells = navmap_tools::floor_hole_cells(grid_floor(30, holes), 0.4f, 2.4f); + // the small hole is gone (9 cells), the large one stays (its middle at least) + EXPECT_GE(cells.size(), 9u); + EXPECT_LT(cells.size(), 9u + 100u - 36u + 1u); +} + +TEST(FloorHoles, AStraightBorderDoesNotGrow) +{ + const auto cells = navmap_tools::floor_hole_cells( + grid_floor(20, [](int, int) {return false;}), 0.4f, 2.4f); + EXPECT_TRUE(cells.empty()); +} + +TEST(FloorHoles, ZeroWidthLeavesTheHoles) +{ + const auto cells = navmap_tools::floor_hole_cells( + grid_floor(20, [](int i, int j) {return i == 10 && j == 10;}), 0.4f, 0.0f); + EXPECT_TRUE(cells.empty()); +} diff --git a/navmap_tools/test/test_ign.py b/navmap_tools/test/test_ign.py new file mode 100644 index 0000000..a1ddddd --- /dev/null +++ b/navmap_tools/test/test_ign.py @@ -0,0 +1,146 @@ +# 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. +""" +Unit tests for navmap_tools.geo.ign -- no network involved. + +The WCS download is monkeypatched (`get_to_file` writes a canned ArcGrid body), so the +request URL, the caching and the sampling are checked without touching the IGN service. +""" + +from navmap_tools.geo import ign as ign_mod +from navmap_tools.geo.cache import DiskCache +from navmap_tools.geo.ign import IgnBlock, IgnDem, load_ign_dem, parse_asc +from navmap_tools.geo.projection import BBox + +import numpy as np + +import pytest + + +def _asc(values, xll=440000.0, yll=4470000.0, cell=5.0, nodata=-9999, center=False): + rows = '\n'.join(' '.join(str(v) for v in row) for row in values) + corner = 'xllcenter' if center else 'xllcorner' + corner_y = 'yllcenter' if center else 'yllcorner' + return ( + f'ncols {len(values[0])}\nnrows {len(values)}\n{corner} {xll}\n{corner_y} {yll}\n' + f'cellsize {cell}\nNODATA_value {nodata}\n{rows}\n') + + +def test_parse_asc_reads_header_and_values_north_first(): + block = parse_asc(_asc([[1, 2, 3], [4, 5, 6]])) + assert block.values.shape == (2, 3) + assert block.values[0, 0] == 1 and block.values[1, 2] == 6 # row 0 is the northern one + assert (block.x_min, block.y_min, block.cell) == (440000.0, 4470000.0, 5.0) + + +def test_parse_asc_with_centre_corner_shifts_half_a_cell(): + block = parse_asc(_asc([[1, 2], [3, 4]], center=True)) + assert block.x_min == 440000.0 - 2.5 + assert block.y_min == 4470000.0 - 2.5 + + +def test_parse_asc_marks_nodata_as_nan(): + block = parse_asc(_asc([[1, -9999], [3, 4]])) + assert np.isnan(block.values[0, 1]) and block.values[0, 0] == 1 + + +def test_parse_asc_skips_a_multipart_preamble(): + body = '--boundary\nContent-Type: application/asc\n\n' + _asc([[7]]) + '--boundary--\n' + assert parse_asc(body).values[0, 0] == 7 + + +@pytest.mark.parametrize('body', ['nothing here', 'ncols 2\nnrows 2\ncellsize 5\n1 2 3']) +def test_parse_asc_rejects_bad_bodies(body): + with pytest.raises(ValueError): + parse_asc(body) + + +def _block(): + # 3 x 3 cells of 5 m, UTM (0, 0)-(15, 15) in the block's own frame; north row first + return IgnBlock( + x_min=0.0, y_min=0.0, cell=5.0, + values=np.array([[3, 3, 3], [2, 2, 2], [1, 1, 1]], dtype=np.float32)) + + +def test_block_sample_is_bilinear_between_cell_centres(): + value, ok = _block().sample_array(np.array([7.5]), np.array([7.5])) # centre of the middle + assert ok[0] and value[0] == pytest.approx(2.0) + value, ok = _block().sample_array(np.array([7.5]), np.array([10.0])) # north of the middle + assert ok[0] and value[0] == pytest.approx(2.5) + + +def test_block_sample_outside_is_not_answered(): + _, ok = _block().sample_array(np.array([-1.0, 7.5]), np.array([7.5, 99.0])) + assert not ok.any() + + +def test_block_sample_next_to_missing_data_is_not_answered(): + block = _block() + block.values[1, 1] = np.nan + _, ok = block.sample_array(np.array([7.5, 2.5]), np.array([7.5, 2.5])) + assert not ok[0] and ok[1] # the centre needs the missing cell, the corner does not + + +def test_dem_uses_the_first_block_that_answers(): + far = IgnBlock(x_min=1e6, y_min=0.0, cell=5.0, values=np.zeros((2, 2), dtype=np.float32)) + dem = IgnDem(blocks=[far, _block()]) + x, y = 7.5, 7.5 + lon, lat = ign_mod._TO_UTM30.transform(x, y, direction='INVERSE') + assert dem.sample(lon, lat) == pytest.approx(2.0, abs=1e-3) + + +def test_dem_without_data_says_where(): + dem = IgnDem(blocks=[_block()]) + with pytest.raises(RuntimeError, match='only covers Spain'): + dem.sample(0.0, 0.0) + + +def test_pixel_size_is_the_cell_in_degrees(): + assert IgnDem(cell_m=5.0).pixel_size_lat * 111320.0 == pytest.approx(5.0) + + +def test_load_ign_dem_requests_blocks_once_and_caches_them(monkeypatch, tmp_path): + urls = [] + + def fake_get(url, dest, timeout, allow_404=False): + urls.append(url) + dest.write_text(_asc([[10, 10], [10, 10]])) + return True + + monkeypatch.setattr(ign_mod, 'get_to_file', fake_get) + cache = DiskCache(tmp_path) + bbox = BBox(west=-3.7010, south=40.4000, east=-3.7000, north=40.4010) # ~100 m + dem = load_ign_dem(bbox, cache) + assert len(urls) == len(dem.blocks) == 1 + url = urls[0] + assert url.startswith('https://servicios.idee.es/wcs-inspire/mdt?service=WCS&version=2.0.1') + assert 'coverageId=Elevacion25830_5' in url + assert 'subset=x%28' in url and 'subset=y%28' in url + assert 'format=application%2Fasc' in url + load_ign_dem(bbox, cache) + assert len(urls) == 1 # the second run came from the cache + + +def test_load_ign_dem_splits_a_large_area_in_blocks(monkeypatch, tmp_path): + urls = [] + + def fake_get(url, dest, timeout, allow_404=False): + urls.append(url) + dest.write_text(_asc([[0, 0], [0, 0]])) + return True + + monkeypatch.setattr(ign_mod, 'get_to_file', fake_get) + bbox = BBox(west=-3.72, south=40.39, east=-3.68, north=40.42) # ~3.4 km x 3.3 km + load_ign_dem(bbox, DiskCache(tmp_path), max_block_m=2000.0) + assert len(urls) == 4 diff --git a/navmap_tools/test/test_imagery.py b/navmap_tools/test/test_imagery.py index bc319ee..60cec94 100644 --- a/navmap_tools/test/test_imagery.py +++ b/navmap_tools/test/test_imagery.py @@ -294,3 +294,75 @@ def fake_fetch(z, x, y, cache, force=False): bbox = BBox(west=-0.01, south=-0.01, east=0.01, north=0.01) mosaic = load_imagery_mosaic(bbox, cache=None, zoom=20, auto_fallback=False) assert mosaic.zoom == 20 + + +# --------------------------------------------------------------------------- +# XYZ tiles of any server (url_template) +# --------------------------------------------------------------------------- + +def test_xyz_url_template_is_filled_and_cached_apart_from_esri(monkeypatch, tmp_path): + from navmap_tools.geo.cache import DiskCache + + urls = [] + + def fake_get(url, dest, timeout, allow_404=False): + urls.append(url) + from PIL import Image + Image.fromarray(np.zeros((256, 256, 3), dtype=np.uint8)).save(dest, format='JPEG') + return True + + monkeypatch.setattr(imagery_mod, 'get_to_file', fake_get) + cache = DiskCache(tmp_path) + template = 'https://tiles.example.org/{z}/{x}/{y}.jpg' + imagery_mod._fetch_tile_path(7, 3, 5, cache, url_template=template) + imagery_mod._fetch_tile_path(7, 3, 5, cache) # Esri's, same tile: a different cache entry + assert urls[0] == 'https://tiles.example.org/7/3/5.jpg' + assert 'arcgisonline' in urls[1] + imagery_mod._fetch_tile_path(7, 3, 5, cache, url_template=template) + assert len(urls) == 2 # the repeat came from the cache + + +def test_xyz_mosaic_skips_esri_placeholder_probe(monkeypatch, tmp_path): + calls = [] + + def fake_fetch(z, x, y, cache, force=False, url_template=None): + calls.append(url_template) + from PIL import Image + path = tmp_path / f'{z}_{x}_{y}.png' + Image.fromarray(np.zeros((256, 256, 3), dtype=np.uint8)).save(path) + return path + + def no_probe(*args, **kwargs): + raise AssertionError('the Esri placeholder probe must not run for a custom server') + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + monkeypatch.setattr(imagery_mod, 'find_available_zoom', no_probe) + bbox = BBox(west=-0.01, south=-0.01, east=0.01, north=0.01) + mosaic = load_imagery_mosaic( + bbox, cache=None, zoom=15, url_template='https://tiles.example.org/{z}/{x}/{y}.jpg') + assert mosaic.zoom == 15 + assert set(calls) == {'https://tiles.example.org/{z}/{x}/{y}.jpg'} + + +def test_sample_array_agrees_with_the_scalar_sample(): + from navmap_tools.geo.pnoa import PnoaMosaic + + rng = np.random.default_rng(1) + image = rng.integers(0, 256, size=(40, 40, 3), dtype=np.uint8) + mosaic = ImageryMosaic(image=image, zoom=15, tile_x0=16384, tile_y0=12380) + lon = np.array([-0.0001, 0.0, 0.0001]) + 0.0 + lat = np.array([40.4, 40.4001, 40.3999]) + lon, lat = lon - 3.7, lat + from navmap_tools.geo.imagery import lonlat_to_tile + x0, y0 = lonlat_to_tile(lon[0], lat[0], 15) + mosaic = ImageryMosaic(image=image, zoom=15, tile_x0=int(x0) - 0, tile_y0=int(y0) - 0) + got = mosaic.sample_array(lon, lat) + for k in range(3): + assert got[k] == pytest.approx(mosaic.sample(lon[k], lat[k]), abs=0.51) + pnoa = PnoaMosaic(image=image, origin_x=-411000.0, origin_y=4930000.0, pixel_size_m=1.0) + from navmap_tools.geo import pnoa as pnoa_mod + mx, my = pnoa_mod._TO_WEB_MERCATOR.transform(lon, lat) + pnoa = PnoaMosaic(image=image, origin_x=mx[0] - 10, origin_y=my[0] + 10, pixel_size_m=1.0) + got = pnoa.sample_array(lon, lat) + for k in range(3): + assert got[k] == pytest.approx(pnoa.sample(lon[k], lat[k]), abs=0.51) diff --git a/navmap_tools/test/test_mesh_dem.py b/navmap_tools/test/test_mesh_dem.py new file mode 100644 index 0000000..540d11e --- /dev/null +++ b/navmap_tools/test/test_mesh_dem.py @@ -0,0 +1,151 @@ +# 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. +"""Unit tests for navmap_tools.geo.mesh_dem (an STL as a terrain model) -- no network.""" + +import struct + +from navmap_tools.geo.mesh_dem import ( + hole_cells, + load_stl_triangles, + MeshDem, + MeshHeightfield, +) + +import numpy as np + +import pytest + + +def _quad(x0, y0, x1, y1, z): + return np.array([ + [x0, y0, z, x1, y0, z, x1, y1, z], + [x0, y0, z, x1, y1, z, x0, y1, z], + ], dtype=np.float32) + + +def _write_binary_stl(path, tris): + with open(path, 'wb') as f: + f.write(b'\0' * 80) + f.write(struct.pack(' i 4..6, y -1.5..0.5 -> j 2..4 + expected = np.zeros((8, 8), dtype=bool) + expected[2:5, 4:7] = True + assert np.array_equal(holes, expected) + + +def test_a_vertical_wall_marks_the_cells_it_crosses_and_far_triangles_none(): + # a wall from (-3.5, 1.2) to (2.5, 1.2), 3 m high: a segment in xy + wall = np.array([[-3.5, 1.2, 0, 2.5, 1.2, 0, 2.5, 1.2, 3]], dtype=np.float32) + holes = hole_cells(wall, 9, 1.0) + assert holes.sum() == 7 and holes[5, 0:7].all() # y 1.2 -> j 5; x -3.5..2.5 -> i 0..6 + far = np.array([[50, 50, 0, 51, 50, 0, 50, 51, 0]], dtype=np.float32) + assert not hole_cells(far, 9, 1.0).any() + assert not hole_cells(np.zeros((0, 9), np.float32), 9, 1.0).any() + + +def test_a_diagonal_triangle_marks_what_it_touches_and_nothing_beyond(): + tri = np.array([[-4, -4, 0, 4, -4, 0, -4, 4, 0]], dtype=np.float32) # the lower-left half + holes = hole_cells(tri, 9, 1.0) + # the long edge is x + y = 0; cell (i, j) has its lower-left corner at (i - 4, j - 4): it is + # touched if i + j <= 8 (i + j == 8: at that corner only, which the conservative rule counts) + for j in range(8): + for i in range(8): + assert holes[j, i] == (i + j <= 8), (i, j) diff --git a/navmap_tools/test/test_terrain_export.py b/navmap_tools/test/test_terrain_export.py new file mode 100644 index 0000000..0bd0100 --- /dev/null +++ b/navmap_tools/test/test_terrain_export.py @@ -0,0 +1,129 @@ +# 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. +"""Unit tests for navmap_tools.geo.terrain_export (a local DEM as a Gazebo terrain).""" + +import struct + +from navmap_tools.geo.dem_file import build_local_dem, DemSource, LocalDem +from navmap_tools.geo.terrain_export import write_gazebo_terrain, write_world_from_template + +import numpy as np + +import pytest + + +class _Ramp: + """Height rises 1 m per 10 m to the east.""" + + def sample_array(self, lon, lat): + return 100.0 + (lon + 3.8) * 111320.0 * 0.76 / 10.0 + + +def _dem(radius=8.0, resolution=1.0, rgb=False): + dem = build_local_dem(_Ramp(), 40.4, -3.8, radius, resolution, DemSource.IGN) + if rgb: + dem.rgb = np.zeros((dem.side, dem.side, 3), dtype=np.uint8) + dem.rgb[-1, 0] = (255, 0, 0) # north-west node + dem.imagery_zoom = 18 + return dem + + +def _stl(path): + raw = path.read_bytes() + (n,) = struct.unpack_from(' 0).all() # counter-clockwise seen from +z + assert np.allclose(np.linalg.norm(rec['n'], axis=1), 1.0, atol=1e-5) + xs = rec['v'][:, 0::3] + assert xs.min() == pytest.approx(-8.0) and xs.max() == pytest.approx(8.0) + + +def test_terrain_resolution_makes_the_collision_mesh_coarser(tmp_path): + write_gazebo_terrain(_dem(radius=8.0), tmp_path, terrain_resolution=4.0) + assert len(_stl(tmp_path / 'terrain.stl')) == 2 * 4 * 4 # every 4th node: 5 x 5 + # not a divisor of side - 1 = 16: the largest one below (4 -> 4, 6 -> 4, 16 stays) + write_gazebo_terrain(_dem(radius=8.0), tmp_path, terrain_resolution=6.0) + assert len(_stl(tmp_path / 'terrain.stl')) == 2 * 4 * 4 + + +def test_obj_has_vertices_uvs_normals_and_faces_that_match(tmp_path): + write_gazebo_terrain(_dem(), tmp_path) + lines = (tmp_path / 'terrain.obj').read_text().splitlines() + kinds = [ln.split()[0] for ln in lines] + assert kinds[:2] == ['mtllib', 'usemtl'] + assert kinds.count('v') == kinds.count('vt') == kinds.count('vn') == 17 * 17 + assert kinds.count('f') == 2 * 16 * 16 + first_face = [ln for ln in lines if ln.startswith('f ')][0].split()[1:] + assert first_face == ['1/1/1', '2/2/2', '19/19/19'] # (v00, v10, v11), 1-based + vn = np.array([[float(x) for x in ln.split()[1:]] for ln in lines if ln.startswith('vn ')]) + assert (vn[:, 2] > 0).all() + assert vn[:, 0].max() < 0 # the ramp rises to the east: the normal leans west + + +def test_texture_is_written_with_the_north_row_first(tmp_path): + from PIL import Image + write_gazebo_terrain(_dem(rgb=True), tmp_path) + assert 'map_Kd terrain.png' in (tmp_path / 'terrain.mtl').read_text() + img = np.asarray(Image.open(tmp_path / 'terrain.png')) + assert img.shape == (17, 17, 3) + assert tuple(img[0, 0]) == (255, 0, 0) # the north-west node is the first texel + + +def test_without_imagery_there_is_no_texture(tmp_path): + write_gazebo_terrain(_dem(), tmp_path) + assert not (tmp_path / 'terrain.png').exists() + assert 'map_Kd' not in (tmp_path / 'terrain.mtl').read_text() + + +def test_world_template_is_filled(tmp_path): + dem = _dem() + template = tmp_path / 't.world.in' + template.write_text( + '@LATITUDE@@LONGITUDE@@ELEVATION@' + 'file://@TERRAIN_DIR@/terrain.stl') + out = tmp_path / 'out' / 'world.sdf' + write_world_from_template(dem, template, out, tmp_path / 'terr') + text = out.read_text() + assert '40.4' in text and '-3.8' in text + assert f'{dem.reference_altitude:.10g}' in text + assert f'file://{(tmp_path / "terr").resolve()}/terrain.stl' in text + + +def test_world_template_without_terrain_dir_is_refused(tmp_path): + template = tmp_path / 't.in' + template.write_text('@LATITUDE@') + with pytest.raises(ValueError, match='TERRAIN_DIR'): + write_world_from_template(_dem(), template, tmp_path / 'w.sdf', tmp_path) + + +def test_local_dem_height_and_cache_match(): + dem = _dem() + assert isinstance(dem, LocalDem) + assert dem.height_at(0.0, 0.0) == pytest.approx(0.0, abs=1e-6) # the origin is z = 0 + assert dem.height_at(10.0, 0.0) == pytest.approx(dem.height_at(8.0, 0.0)) # clamped + assert dem.height_at(4.0, 0.0) > 0.3 + assert dem.matches(40.4, -3.8, 8.0, 1.0, DemSource.IGN) + assert not dem.matches(40.4, -3.8, 9.0, 1.0, DemSource.IGN) # another radius + assert not dem.matches(40.4, -3.8, 8.0, 1.0, DemSource.MESH) # another source + assert not dem.matches(40.4, -3.8, 8.0, 1.0, DemSource.IGN, requested_reference=600.0) + assert not dem.matches(40.4, -3.8, 8.0, 1.0, DemSource.IGN, data_resolution=5.0)