diff --git a/.gitignore b/.gitignore index 7874f4da8..23073ec0b 100644 --- a/.gitignore +++ b/.gitignore @@ -61,3 +61,13 @@ docs/cpp_api docs/_doxygen_cpp docs/_doxygen_ros1 docs/_build + +# Local benchmark launch and monitoring utilities +/interfaces/ros1/wavemap_ros/launch/benchmark_*.launch +/interfaces/ros1/wavemap_ros/launch/benchmarks/ +/interfaces/ros1/wavemap_ros/scripts/monitor_wavemap_benchmark.py +/interfaces/ros1/wavemap_ros/scripts/run_ouster_benchmark.py +/interfaces/ros1/wavemap_ros/scripts/summarize_wavemap_benchmark.py + +# Local backup files +*.orig diff --git a/README.md b/README.md index 737a112e6..96739124e 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,33 @@ -# Wavemap -C++ -Python -ROS1 -Docs +# Layered Wavemap +C++ +Python +ROS1 +Docs Version -License -[![3D reconstruction of Newer College's Cloister](https://github.com/ethz-asl/wavemap/assets/6238939/e432d4ea-440d-4e9d-adf9-af3ae3b09a10)](https://www.youtube.com/live/ftQhK75Ri1E?si=9txTYyJ78wQuhyN-&t=733) +License -## Hierarchical, multi-resolution volumetric mapping +Layered Wavemap extends Wavemap's compressed multi-resolution occupancy map with typed environmental attributes. Continuous fields such as reflectivity, signal, near-infrared intensity, and RGB share the wavelet hierarchy with occupancy. Categorical fields such as semantic classes use aligned sparse side layers with lossless majority-with-exceptions compression. + +[Get started](docs/pages/tutorials/layered_wavemap.rst) · [Run the RGB demo](#try-the-layered-rgb-demo) · [Original Wavemap documentation](https://ethz-asl.github.io/wavemap/) + +[![Layered Wavemap demo](https://i.ytimg.com/vi/l24QWAxm7Zw/hqdefault.jpg)](https://youtu.be/l24QWAxm7Zw) + +*Watch the Layered Wavemap demo on YouTube.* + +## Try the layered RGB demo + +The synthetic forest demo needs no sensor data. On Ubuntu 20.04 with ROS Noetic: + +```bash +cd ~/catkin_ws +catkin build wavemap_all +source devel/setup.bash +roslaunch wavemap_ros synthetic_rgb_layered_map.launch +``` + +The launch file opens RViz and progressively builds a colored layered map. Docker setup, native installation, real Ouster usage, map persistence, and a layer-extension walkthrough are in the [Layered Wavemap guide](docs/pages/tutorials/layered_wavemap.rst). + +## Wavemap foundation Wavemap achieves state-of-the-art memory and computational efficiency by combining Haar wavelet compression and a coarse-to-fine measurement integration scheme. Advanced measurement models allow it to attain exceptionally high recall rates on challenging obstacles like thin objects. The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single multi-resolution occupancy grid map. @@ -21,6 +41,7 @@ Wavemap provides [C++](https://ethz-asl.github.io/wavemap/pages/tutorials/cpp) a The framework's documentation is available on [GitHub Pages](https://ethz-asl.github.io/wavemap/) for easy online access. A PDF version of each release’s documentation can also be found in the respective [release notes](https://github.com/ethz-asl/wavemap/releases). ### Table of contents +* [Layered Wavemap: setup, use, and extension](docs/pages/tutorials/layered_wavemap.rst) * [Installation](https://ethz-asl.github.io/wavemap/pages/installation) * [Demos](https://ethz-asl.github.io/wavemap/pages/demos) * [Tutorials](https://ethz-asl.github.io/wavemap/pages/tutorials) diff --git a/docs/pages/tutorials/index.rst b/docs/pages/tutorials/index.rst index 8858cef2a..f7badbdff 100644 --- a/docs/pages/tutorials/index.rst +++ b/docs/pages/tutorials/index.rst @@ -6,6 +6,7 @@ This section provides hands-on examples of how to use wavemap's C++ and Python A :caption: Interfaces :maxdepth: 1 + layered_wavemap cpp python ros1 diff --git a/docs/pages/tutorials/layered_wavemap.rst b/docs/pages/tutorials/layered_wavemap.rst new file mode 100644 index 000000000..fa26da4fb --- /dev/null +++ b/docs/pages/tutorials/layered_wavemap.rst @@ -0,0 +1,452 @@ +.. _layered-wavemap-tutorial: + +Layered Wavemap +############### + +Layered Wavemap extends the original occupancy map with environmental +attributes that use the same spatial coordinates. This guide takes you from a +fresh checkout to a running RGB example, then shows how to use real point-cloud +fields and define a new layer. + +What is stored +************** + +Every layered map owns two related forms of storage: + +* Occupancy and wavelet-compatible continuous states share one wavelet octree. + Examples are reflectivity, signal strength, near-infrared intensity, and RGB. +* Categorical or finite-state values use sparse side layers aligned to the + minimum-resolution voxel indices. The current discrete backend stores a + dominant value per block, observed offsets, and exact exceptions. + +Occupancy always remains part of the continuous voxel and is not listed in a +user schema. Here, continuous and discrete classify the algebra of the stored +state; all layers are still spatially voxelized. + +A layer is continuous when adding, subtracting, and scaling its stored state +have a meaningful interpretation. It must also be possible to bound the state +and measure the magnitude of a wavelet detail coefficient. Scalar sensor +measurements, RGB vectors, and additive weighted-mean statistics satisfy these +requirements and can share the Haar wavelet hierarchy with occupancy. + +A layer is discrete when interpolation or arithmetic would change its meaning. +Semantic class IDs, object IDs, and Boolean flags are examples: averaging class +1 and class 2 does not define a valid class. Their C++ representation does not +determine the choice; an integer or floating-point value still belongs in a +discrete layer when arithmetic on that value is semantically invalid. Such +values use aligned side storage and do not participate in wavelet pruning. + +.. code-block:: text + + posed point cloud + +-- XYZ ----------------> original Wavemap occupancy integrator + +-- point attributes ---> layer update policy + +-- continuous: shared wavelet voxel + +-- discrete: aligned side layer + +Requirements +************ + +The supported ROS setup is Ubuntu 20.04 with ROS Noetic. Docker is the fastest +way to try the project when that environment is not already installed. Native +installation is more convenient while changing and rebuilding the code. + +Docker quick start +================== + +Install Docker and configure it for use without ``sudo``. Then clone this +repository and build the ROS image from the local checkout: + +.. code-block:: bash + + git clone https://github.com/GuilhermeCabaco/wavemap-digital-twin.git + cd wavemap-digital-twin + docker build --tag=wavemap_ros1 \ + --file tooling/docker/ros1/full.Dockerfile . + +The full Dockerfile copies the current checkout into a Catkin workspace, +installs its dependencies, and builds ``wavemap_all``. Downloading the upstream +Wavemap image alone does not include this repository's layered extensions. + +The supplied runner forwards an X11 display for RViz. It expects a host data +directory at ``/home/$USER/data``: + +.. code-block:: bash + + mkdir -p /home/$USER/data + chmod +x tooling/scripts/run_in_docker.sh + ./tooling/scripts/run_in_docker.sh \ + roslaunch wavemap_ros synthetic_rgb_layered_map.launch + +The runner is intended for Linux/X11. Other display systems require the Docker +GUI forwarding appropriate to that platform. + +Native ROS Noetic installation +============================== + +Install ROS Noetic, followed by the build tools: + +.. code-block:: bash + + sudo apt update + sudo apt install git build-essential python3-rosdep python3-catkin-tools + +Create and configure a workspace: + +.. code-block:: bash + + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws + source /opt/ros/noetic/setup.bash + catkin init + catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release + +Clone this repository into the source space: + +.. code-block:: bash + + cd ~/catkin_ws/src + git clone https://github.com/GuilhermeCabaco/wavemap-digital-twin.git wavemap + +Initialize rosdep once, install dependencies, and build: + +.. code-block:: bash + + sudo rosdep init + rosdep update + cd ~/catkin_ws + rosdep install -y --from-paths src --ignore-src + catkin build wavemap_all + source devel/setup.bash + +If rosdep is already initialized, continue with ``rosdep update``. Source +``devel/setup.bash`` in each new terminal. + +Run the dataset-free RGB demo +***************************** + +The synthetic demo constructs a small colored forest, publishes it as a +layered map, and opens RViz: + +.. code-block:: bash + + source ~/catkin_ws/devel/setup.bash + roslaunch wavemap_ros synthetic_rgb_layered_map.launch + +No bag, sensor driver, or localization system is needed. RViz subscribes to +``/synthetic_rgb_layered_map/layered_map``. Use +``Render voxels / Layer`` to switch between ``rgb`` and ``occupancy``. The node +prints a completion message after inserting all voxels. + +Control the animation with: + +.. code-block:: bash + + roslaunch wavemap_ros synthetic_rgb_layered_map.launch \ + voxels_per_update:=300 update_period:=0.03 + +Save the completed map with: + +.. code-block:: bash + + roslaunch wavemap_ros synthetic_rgb_layered_map.launch \ + save_path:=$HOME/synthetic_rgb.lwvmp + +After the completion message, stop the process with Ctrl-C. To inspect the file +later, add a ``LayeredMap`` display in RViz, change ``Source`` from ``Topic`` to +``File``, and select ``Load map from disk``. The built-in RGB schema handler +recognizes the saved file. + +Use the Ouster integration as a reference +****************************************** + +The repository includes Ouster-specific applications as worked examples of a +real layered sensor integration. No Ouster bag or live sensor stream is bundled +with the repository, and the commands below do not start localization. They +show how to launch the mapping side once a compatible point cloud and TF source +are available. For another sensor or dataset, use the same structure and adapt +the topic, field names, normalization ranges, projection model, and transforms. + +The example applications expect: + +* Point clouds on ``/ouster/points``. +* A valid TF transform from each cloud frame to the configured map frame. +* ``reflectivity`` values between 0 and 255. +* ``intensity`` and ``ambient`` values between 0 and 65535 when selected. + +The all-layer reference application stores occupancy, reflectivity, signal, +near infrared, and a geometrically derived ground/obstacle class. With a +compatible live stream or bag already available, start it as follows: + +.. code-block:: bash + + source ~/catkin_ws/devel/setup.bash + rosparam load \ + $(rospack find wavemap_ros)/config/wavemap_ouster_all_layers_demo.yaml \ + /wavemap + rosrun wavemap_ros signal_near_ir_class_layered_server __name:=wavemap + +In another terminal, run the sensor, localization, or bag playback that +publishes the cloud and TF transforms. Open the supplied RViz configuration: + +.. code-block:: bash + + source ~/catkin_ws/devel/setup.bash + rviz -d $(rospack find wavemap_ros)/config/rviz/layered_map.rviz + +Choose ``occupancy``, ``reflectivity``, ``signal``, ``near_ir``, or ``class`` +from the display's Layer property. Class 1 is ground and class 2 is obstacle. +This is an example based on local elevation, not a learned semantic model. + +Available applications +====================== + +.. list-table:: + :header-rows: 1 + :widths: 45 55 + + * - Executable + - Additional layers + * - ``occupancy_layered_server`` + - None; exercises the layered lifecycle + * - ``reflectivity_layered_server`` + - reflectivity + * - ``reflectivity_class_layered_server`` + - reflectivity and class + * - ``signal_layered_server`` + - reflectivity and signal + * - ``signal_near_ir_layered_server`` + - reflectivity, signal, and near infrared + * - ``signal_class_layered_server`` + - reflectivity, signal, and class + * - ``signal_near_ir_class_layered_server`` + - reflectivity, signal, near infrared, and class + +Hashed and chunked versions also exist for the occupancy and reflectivity +examples. Their source is under +``interfaces/ros1/wavemap_ros/app/ouster_layered``. + +Save and restore layered maps +***************************** + +A layered server named ``/wavemap`` exposes these services: + +.. code-block:: bash + + rosservice call /wavemap/save_layered_map \ + "file_path: '$HOME/forest_map.lwvmp'" + + rosservice call /wavemap/load_layered_map \ + "file_path: '$HOME/forest_map.lwvmp'" + + rosservice call /wavemap/layered_map_request_full + +The ``.lwvmp`` format stores its version, ordered layer schema, typed continuous +wavelet map, and compressed discrete side layers. Loading rejects incompatible +schemas and map geometry. Start the same typed server used to create the file +before calling ``load_layered_map``. A successful load publishes the full map. + +Define and configure a layer +**************************** + +A layer is a compile-time type. This scalar example uses exponential smoothing +with an observation weight of 0.2: + +.. code-block:: c++ + + using ReflectivityPolicy = + wavemap::layered::ExponentialScalarLayerUpdatePolicy<1, 5>; + + struct ReflectivityLayer + : wavemap::layered::schema::ContinuousLayer< + float, ReflectivityPolicy> { + static constexpr std::string_view name = "reflectivity"; + static constexpr wavemap::layered::ScalarLayerVisualization + visualization{{0.05f, 0.05f, 0.05f}, {1.f, 1.f, 1.f}}; + }; + +A class layer uses discrete storage because arithmetic on identifiers would not +preserve their meaning: + +.. code-block:: c++ + + struct ClassLayer + : wavemap::layered::schema::DiscreteLayer< + int, wavemap::layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "class"; + static constexpr std::array categories{ + wavemap::layered::IntegerCategoryLabel{ + 1, "Ground", {0.2f, 0.8f, 0.2f}}, + wavemap::layered::IntegerCategoryLabel{ + 2, "Obstacle", {0.9f, 0.2f, 0.15f}}}; + }; + +Combine the tags into a schema and derive the map type: + +.. code-block:: c++ + + using Schema = wavemap::layered::schema::LayerSchema< + ReflectivityLayer, ClassLayer>; + using Definition = wavemap::layered::LayeredMapDefinition; + +Occupancy is included automatically. Pass +``wavemap::layered::HashedChunkedWaveletOctreeBackend`` as the second +``LayeredMapDefinition`` template argument to select the chunked backend. + +Configure map geometry, pruning, bounds, and discrete grouping: + +.. code-block:: c++ + + wavemap::layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.discreteBlockHeight(2); + config.combinedPruningThreshold(1.f); + +A discrete block height of 2 groups 4 x 4 x 4 minimum-resolution positions. It +changes compression grouping, not map resolution. + +Choose an update policy +======================= + +Reusable policies live in +``library/cpp/include/wavemap/layered/integration/layer_update_policy.h``. They +include replacement, minimum, maximum, accumulation, exponential smoothing, +confidence blending, logical operations, and a stateful weighted mean. These are +provided defaults rather than a closed set. A user can define a custom policy +without changing the map or integration pipeline. + +A stateless policy is a default-constructible callable that receives the current +value and a ``LayerObservation`` and returns the next value. A stateful +continuous policy follows the same pattern but receives and returns State: + +.. code-block:: c++ + + struct RateLimitedUpdate { + float operator()( + float current, + const wavemap::layered::LayerObservation& observation) const { + const float change = std::clamp( + observation.value - current, -0.1f, 0.1f); + return current + change; + } + }; + + struct HeightLayer + : wavemap::layered::schema::ContinuousLayer< + float, RateLimitedUpdate> { + static constexpr std::string_view name = "height"; + }; + +A discrete policy may instead accept ``std::optional`` as its current +value so it can distinguish an unobserved voxel from an observed value. The +compile-time schema checks the selected policy signature and reports an error if +it is incompatible with the layer. + +For ordinary layers, Value and State are identical. Use +``StatefulContinuousLayer`` when observations need a richer stored state. A +custom wavelet-compatible state must provide: + +* ``ContinuousValueTraits`` operations for addition, subtraction, scaling, + thresholding, and magnitude. +* A ``LayerStreamCodec`` for persistence. +* A ``LayerStateConversion`` when State differs from public Value. +* A ``LayerRosCodec`` when using the generic ROS converter. + +The RGB and weighted-mean types under +``library/cpp/include/wavemap/layered/types`` are compact examples. + +Bind sensor data +================ + +Install the typed map with ``LayeredRosServerBuilder``, then register how sensor +fields populate it. Numeric PointCloud2 fields can be normalized directly: + +.. code-block:: c++ + + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + wavemap::layered::EndpointRange{0.5f, 25.f}); + +Other helpers include: + +* ``bindDirectPointField`` for values already in the layer's units. +* ``bindComputedPointField`` for values derived from numeric fields. +* ``bindGeometricClass`` for the included local-elevation classifier. +* A custom endpoint adapter for structured or application-specific decoding. + +Bindings default to endpoint-only integration. Continuous layers may select an +along-ray ``PointcloudLayerIntegrationMode``. Discrete fields are restricted to +endpoints. + +Expose and visualize the application +==================================== + +Add the executable to ``interfaces/ros1/wavemap_ros/CMakeLists.txt``, link it to +``wavemap_ros``, and rebuild: + +.. code-block:: bash + + cd ~/catkin_ws + catkin build wavemap_ros wavemap_ros_conversions wavemap_rviz_plugin + source devel/setup.bash + +Topic visualization obtains schema and color metadata from the ``LayeredMap`` +message. Loading a custom ``.lwvmp`` directly in RViz additionally requires a +typed ``LayeredMapFactory`` registration for the new schema. + +Verification and troubleshooting +******************************** + +Run the affected builds and tests after changing schemas, codecs, storage, or +ROS conversions: + +.. code-block:: bash + + cd ~/catkin_ws + catkin build wavemap wavemap_ros_conversions wavemap_ros \ + wavemap_rviz_plugin + catkin run_tests wavemap wavemap_ros_conversions wavemap_ros \ + wavemap_rviz_plugin + source /opt/ros/noetic/setup.bash + catkin_test_results --all build + +No map appears + Check the display topic, the fixed frame, TF availability, and whether the + node has published a map message. + +Point clouds are dropped + Verify cloud timestamps and frames, the map-to-sensor transform, and + ``max_wait_for_pose``. Motion undistortion needs usable per-point timing. + +An attribute remains empty + Inspect the PointCloud2 fields. Its name and numeric range must match the + registered binding. + +A saved map will not load + Use the same schema, backend, resolution, tree geometry, and discrete block + configuration that created it. + +Fine attribute detail disappears + Reduce the layer's pruning scale or increase its pruning weight. Continuous + fields contribute to one combined pruning decision. + +Relevant source locations +************************* + +* ``library/cpp/include/wavemap/layered/schema``: declarations and bundles. +* ``library/cpp/include/wavemap/layered/integration``: observations and update + policies. +* ``library/cpp/include/wavemap/layered/map``: map types, backends, and storage. +* ``library/cpp/include/wavemap/layered/io``: schema-aware persistence. +* ``interfaces/ros1/wavemap_ros/app/ouster_layered``: complete applications. +* ``interfaces/ros1/wavemap_ros_conversions``: ROS codecs. +* ``interfaces/ros1/wavemap_rviz_plugin``: topic and file visualization. diff --git a/interfaces/ros1/wavemap_msgs/CMakeLists.txt b/interfaces/ros1/wavemap_msgs/CMakeLists.txt index 58dba387c..4a0505b05 100644 --- a/interfaces/ros1/wavemap_msgs/CMakeLists.txt +++ b/interfaces/ros1/wavemap_msgs/CMakeLists.txt @@ -2,15 +2,25 @@ cmake_minimum_required(VERSION 3.0.2) project(wavemap_msgs) # Load catkin and required dependencies to generate msgs -find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) +find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs) # Declare message files to be built add_message_files(FILES + DiscreteLayer.msg + DiscreteLayerCategory.msg + DiscreteLayerCell.msg HashedBlock.msg HashedBlocks.msg HashedWaveletOctree.msg HashedWaveletOctreeBlock.msg Index3D.msg + LayerVisualization.msg + LayeredHashedWaveletOctree.msg + LayeredHashedWaveletOctreeBlock.msg + LayeredMap.msg + LayeredMapUpdate.msg + LayeredWaveletOctreeNode.msg + Layer.msg Map.msg OctreeNode.msg WaveletOctree.msg @@ -21,7 +31,7 @@ add_service_files(FILES FilePath.srv) # Generate the language-specific message and service files -generate_messages(DEPENDENCIES std_msgs) +generate_messages(DEPENDENCIES geometry_msgs std_msgs) # Finish registering wavemap_msgs as a catkin pacakge -catkin_package(CATKIN_DEPENDS message_runtime std_msgs) +catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs) diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg new file mode 100644 index 000000000..5a997be87 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayer.msg @@ -0,0 +1,18 @@ +# A compressed discrete voxel layer stored as parent cells with dominant values +# and explicit exceptions. +string name +string value_type + +# Same meaning as DiscreteCompressionConfig.block_height: +# 1 -> 2x2x2, 2 -> 4x4x4, etc. +int32 block_height + +# Optional labels for integer category values. Values without an entry remain +# valid and are displayed using their numeric value. +DiscreteLayerCategory[] categories + +DiscreteLayerCell[] cells + +# Parent cells removed since the preceding update. Only used by incremental +# LayeredMap messages; full snapshots implicitly remove all omitted cells. +Index3D[] deleted_parent_indices diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg new file mode 100644 index 000000000..722744c46 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCategory.msg @@ -0,0 +1,7 @@ +# Optional semantic label and schema-declared visualization color. +int32 value +string label +bool has_color +float32 r +float32 g +float32 b diff --git a/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg new file mode 100644 index 000000000..0bd1cc0ef --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/DiscreteLayerCell.msg @@ -0,0 +1,21 @@ +# One compressed parent cell in a discrete layer. +# The parent_index identifies the compressed parent block. +Index3D parent_index + +# Default value for all observed children unless an exception overrides it. +# Exactly one value array is used, according to DiscreteLayer.value_type. +int32 dominant_int32 +uint8 dominant_uint8 + +# Child offsets observed inside this parent cell. +# This keeps sparse unknown children unknown instead of implicitly assigning +# them the dominant value. +int32[] observed_offsets + +# Child offsets whose value differs from the dominant value. +int32[] exception_offsets + +# Exception values aligned with exception_offsets. Exactly one array is used, +# according to DiscreteLayer.value_type. +int32[] exception_int32_values +uint8[] exception_uint8_values diff --git a/interfaces/ros1/wavemap_msgs/msg/Layer.msg b/interfaces/ros1/wavemap_msgs/msg/Layer.msg new file mode 100644 index 000000000..8e5f5d41e --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/Layer.msg @@ -0,0 +1,6 @@ +string name +string type + +float32[] float32_values +int32[] int32_values +uint8[] uint8_values diff --git a/interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg b/interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg new file mode 100644 index 000000000..0f836bb3c --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayerVisualization.msg @@ -0,0 +1,10 @@ +# Optional visualization metadata declared by the layer schema. +string name +bool has_low_color +float32 low_r +float32 low_g +float32 low_b +bool has_high_color +float32 high_r +float32 high_g +float32 high_b diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg new file mode 100644 index 000000000..70d154e06 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctree.msg @@ -0,0 +1,15 @@ +float32 min_cell_width +float32 min_log_odds +float32 max_log_odds + +int32 tree_height + +string[] layer_names +string[] layer_types +Layer[] layer_min_values +Layer[] layer_max_values +LayerVisualization[] layer_visualizations + +Index3D[] allocated_block_indices + +LayeredHashedWaveletOctreeBlock[] blocks diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg new file mode 100644 index 000000000..b552eee9b --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredHashedWaveletOctreeBlock.msg @@ -0,0 +1,6 @@ +Index3D root_node_offset + +float32 root_node_occupancy_scale_coefficient +Layer[] root_node_layers + +LayeredWaveletOctreeNode[] nodes diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg new file mode 100644 index 000000000..01bd9de3d --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredMap.msg @@ -0,0 +1,11 @@ +Header header + +# True for a complete snapshot. False for an incremental patch whose +# continuous blocks and discrete parent cells replace only the included data. +bool is_full_update + +# Existing Wavemap structured message for the continuous wavelet map. +Map continuous_map + +# Additional non-wavelet discrete layers aligned with the same voxel index space. +DiscreteLayer[] discrete_layers diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg new file mode 100644 index 000000000..1a003d8e6 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredMapUpdate.msg @@ -0,0 +1,6 @@ +# A sensor-independent batch of spatial layer observations. +# Every layer contains one value per position, in the same order. +# Producers may include any subset of the configured map layers. +std_msgs/Header header +geometry_msgs/Point32[] positions +Layer[] layers diff --git a/interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg b/interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg new file mode 100644 index 000000000..cc91939c7 --- /dev/null +++ b/interfaces/ros1/wavemap_msgs/msg/LayeredWaveletOctreeNode.msg @@ -0,0 +1,3 @@ +float32[8] occupancy_detail_coefficients +Layer[] detail_layers +uint8 allocated_children_bitset diff --git a/interfaces/ros1/wavemap_msgs/msg/Map.msg b/interfaces/ros1/wavemap_msgs/msg/Map.msg index 41db3abe1..67cea39cf 100644 --- a/interfaces/ros1/wavemap_msgs/msg/Map.msg +++ b/interfaces/ros1/wavemap_msgs/msg/Map.msg @@ -3,3 +3,4 @@ Header header HashedBlocks[] hashed_blocks WaveletOctree[] wavelet_octree HashedWaveletOctree[] hashed_wavelet_octree +LayeredHashedWaveletOctree[] layered_hashed_wavelet_octree diff --git a/interfaces/ros1/wavemap_msgs/package.xml b/interfaces/ros1/wavemap_msgs/package.xml index 72be093e5..48db555cb 100644 --- a/interfaces/ros1/wavemap_msgs/package.xml +++ b/interfaces/ros1/wavemap_msgs/package.xml @@ -12,6 +12,7 @@ catkin + geometry_msgs message_generation message_runtime std_msgs diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt index e90bc7449..e7a845abe 100644 --- a/interfaces/ros1/wavemap_ros/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt @@ -7,7 +7,7 @@ project(wavemap_ros) find_package(OpenCV REQUIRED) find_package(catkin REQUIRED COMPONENTS wavemap wavemap_msgs wavemap_ros_conversions - roscpp rosbag cv_bridge image_transport tf2_ros + roscpp rosbag message_filters cv_bridge image_transport tf2_ros std_srvs sensor_msgs visualization_msgs) # Register catkin package @@ -16,7 +16,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS wavemap wavemap_msgs wavemap_ros_conversions - roscpp rosbag cv_bridge image_transport tf2_ros + roscpp rosbag message_filters cv_bridge image_transport tf2_ros std_srvs sensor_msgs visualization_msgs) @@ -53,6 +53,90 @@ add_executable(ros_server app/ros_server.cc) set_wavemap_target_properties(ros_server) target_link_libraries(ros_server ${PROJECT_NAME}) +add_executable(occupancy_layered_server + app/ouster_layered/occupancy_sensor_inputs.cc) +set_wavemap_target_properties(occupancy_layered_server) +target_include_directories(occupancy_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(occupancy_layered_server ${PROJECT_NAME}) + +add_executable(occupancy_chunked_layered_server + app/ouster_layered/occupancy_chunked_sensor_inputs.cc) +set_wavemap_target_properties(occupancy_chunked_layered_server) +target_include_directories(occupancy_chunked_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(occupancy_chunked_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_layered_server + app/ouster_layered/reflectivity_only_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_layered_server) +target_include_directories(reflectivity_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(reflectivity_layered_server ${PROJECT_NAME}) + +add_executable(signal_layered_server + app/ouster_layered/signal_sensor_inputs.cc) +set_wavemap_target_properties(signal_layered_server) +target_include_directories(signal_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_layered_server ${PROJECT_NAME}) + +add_executable(signal_near_ir_layered_server + app/ouster_layered/signal_near_ir_sensor_inputs.cc) +set_wavemap_target_properties(signal_near_ir_layered_server) +target_include_directories(signal_near_ir_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_near_ir_layered_server ${PROJECT_NAME}) + +add_executable(signal_class_layered_server + app/ouster_layered/signal_class_sensor_inputs.cc) +set_wavemap_target_properties(signal_class_layered_server) +target_include_directories(signal_class_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_class_layered_server ${PROJECT_NAME}) + +add_executable(signal_near_ir_class_layered_server + app/ouster_layered/signal_near_ir_class_sensor_inputs.cc) +set_wavemap_target_properties(signal_near_ir_class_layered_server) +target_include_directories(signal_near_ir_class_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(signal_near_ir_class_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_chunked_layered_server + app/ouster_layered/reflectivity_chunked_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_chunked_layered_server) +target_include_directories(reflectivity_chunked_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(reflectivity_chunked_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_class_layered_server + app/ouster_layered/ouster_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_class_layered_server) +target_include_directories(reflectivity_class_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(reflectivity_class_layered_server ${PROJECT_NAME}) + +add_executable(reflectivity_class_chunked_layered_server + app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc) +set_wavemap_target_properties(reflectivity_class_chunked_layered_server) +target_include_directories(reflectivity_class_chunked_layered_server PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(reflectivity_class_chunked_layered_server ${PROJECT_NAME}) + +add_executable(inspect_ouster_benchmark_maps + app/ouster_layered/inspect_ouster_benchmark_maps.cc) +set_wavemap_target_properties(inspect_ouster_benchmark_maps) +target_include_directories(inspect_ouster_benchmark_maps PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app/ouster_layered) +target_link_libraries(inspect_ouster_benchmark_maps ${PROJECT_NAME}) + +add_executable(synthetic_rgb_layered_map_demo + app/synthetic_rgb_layered_map_demo.cc) +set_wavemap_target_properties(synthetic_rgb_layered_map_demo) +target_include_directories(synthetic_rgb_layered_map_demo PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/app) +target_link_libraries(synthetic_rgb_layered_map_demo ${PROJECT_NAME}) + add_executable(rosbag_processor app/rosbag_processor.cc) set_wavemap_target_properties(rosbag_processor) target_link_libraries(rosbag_processor ${PROJECT_NAME}) diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/inspect_ouster_benchmark_maps.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/inspect_ouster_benchmark_maps.cc new file mode 100644 index 000000000..60a4e43af --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/inspect_ouster_benchmark_maps.cc @@ -0,0 +1,225 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "occupancy_map_layers.h" +#include "reflectivity_map_layers.h" +#include "reflectivity_only_map_layers.h" + +namespace occupancy_map = wavemap::examples::occupancy_map; +namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; +namespace reflectivity_map = wavemap::examples::reflectivity_map; + +namespace { +struct OccupancyStats { + size_t blocks = 0u; + size_t leaves = 0u; + size_t observed = 0u; + float min = std::numeric_limits::max(); + float max = std::numeric_limits::lowest(); +}; + +template +OccupancyStats occupancyStats(const MapT& map) { + OccupancyStats stats; + stats.blocks = map.getHashMap().size(); + map.forEachLeaf([&](const wavemap::OctreeIndex&, float occupancy) { + ++stats.leaves; + stats.observed += 1e-5f < std::abs(occupancy); + stats.min = std::min(stats.min, occupancy); + stats.max = std::max(stats.max, occupancy); + }); + return stats; +} + +struct Comparison { + size_t samples = 0u; + size_t above_tolerance = 0u; + double absolute_error_sum = 0.; + float max_absolute_error = 0.f; +}; + +template +Comparison compareDirection(const LhsMapT& lhs, const RhsMapT& rhs, + float tolerance = 1e-4f) { + Comparison comparison; + lhs.forEachLeaf([&](const wavemap::OctreeIndex& index, float lhs_value) { + const float error = std::abs(lhs_value - rhs.getCellValue(index)); + ++comparison.samples; + comparison.above_tolerance += tolerance < error; + comparison.absolute_error_sum += error; + comparison.max_absolute_error = + std::max(comparison.max_absolute_error, error); + }); + return comparison; +} + +template +void printComparison(const std::string& name, const LhsMapT& lhs, + const RhsMapT& rhs) { + const Comparison forward = compareDirection(lhs, rhs); + const Comparison reverse = compareDirection(rhs, lhs); + const auto print = [](const char* direction, const Comparison& comparison) { + std::cout << " " << direction << ": samples=" << comparison.samples + << " above_1e-4=" << comparison.above_tolerance + << " mean_abs=" + << (comparison.samples + ? comparison.absolute_error_sum / comparison.samples + : 0.) + << " max_abs=" << comparison.max_absolute_error << "\n"; + }; + std::cout << " " << name << "\n"; + print("forward", forward); + print("reverse", reverse); +} + +template +void printReflectivityStats(const std::string& name, + const typename DefinitionT::Map& map) { + size_t nonzero = 0u; + size_t out_of_bounds = 0u; + float min_value = std::numeric_limits::max(); + float max_value = std::numeric_limits::lowest(); + map.continuousMap().forEachVoxelLeaf( + [&](const wavemap::OctreeIndex&, + const typename DefinitionT::Voxel& voxel) { + const float value = voxel.data.template get(); + nonzero += 1e-6f < std::abs(value); + out_of_bounds += value < 0.f || 1.f < value; + min_value = std::min(min_value, value); + max_value = std::max(max_value, value); + }); + std::cout << " " << name << ": nonzero=" << nonzero + << " out_of_[0,1]=" << out_of_bounds + << " value_range=[" << min_value << ", " << max_value << "]\n"; +} + +void printOccupancyStats(const std::string& name, + const OccupancyStats& stats) { + std::cout << " " << name << ": blocks=" << stats.blocks + << " leaves=" << stats.leaves << " observed=" << stats.observed + << " range=[" << stats.min << ", " << stats.max << "]\n"; +} +} // namespace + +int main(int argc, char** argv) { + if (argc != 5) { + std::cerr << "Usage: " << argv[0] + << " \n"; + return 1; + } + + wavemap::MapBase::Ptr original_base; + if (!wavemap::io::fileToMap(argv[1], original_base)) { + std::cerr << "Could not load A\n"; + return 2; + } + auto original = + std::dynamic_pointer_cast(original_base); + if (!original) { + std::cerr << "A is not a HashedWaveletOctree\n"; + return 2; + } + + occupancy_map::Definition::Map occupancy(occupancy_map::makeMapConfig()); + reflectivity_only_map::Definition::Map reflectivity( + reflectivity_only_map::makeMapConfig()); + reflectivity_map::Definition::Map reflectivity_class( + reflectivity_map::makeMapConfig()); + std::string error; + if (!occupancy_map::Definition::MapIo::load(argv[2], occupancy, &error)) { + std::cerr << "Could not load B: " << error << "\n"; + return 2; + } + if (!reflectivity_only_map::Definition::MapIo::load(argv[3], reflectivity, + &error)) { + std::cerr << "Could not load C: " << error << "\n"; + return 2; + } + if (!reflectivity_map::Definition::MapIo::load(argv[4], reflectivity_class, + &error)) { + std::cerr << "Could not load D: " << error << "\n"; + return 2; + } + + std::cout << "Occupancy statistics\n"; + printOccupancyStats("A original", occupancyStats(*original)); + printOccupancyStats("B layered occupancy", + occupancyStats(occupancy.continuousMap())); + printOccupancyStats("C reflectivity", + occupancyStats(reflectivity.continuousMap())); + printOccupancyStats("D reflectivity+class", + occupancyStats(reflectivity_class.continuousMap())); + + std::cout << "Occupancy comparisons\n"; + printComparison("A vs B", *original, occupancy.continuousMap()); + printComparison("A vs C", *original, reflectivity.continuousMap()); + printComparison("A vs D", *original, reflectivity_class.continuousMap()); + printComparison("B vs C", occupancy.continuousMap(), + reflectivity.continuousMap()); + printComparison("C vs D", reflectivity.continuousMap(), + reflectivity_class.continuousMap()); + + std::cout << "Reflectivity statistics\n"; + printReflectivityStats( + "C reflectivity", reflectivity); + printReflectivityStats( + "D reflectivity", reflectivity_class); + + const auto& class_layer = + reflectivity_class.discreteLayers().get(); + size_t ground = 0u; + size_t obstacle = 0u; + for (const auto& [key, cell] : class_layer.cells()) { + (void)key; + for (const int offset : cell.observed_offsets) { + const auto exception = cell.exceptions.find(offset); + const int value = exception == cell.exceptions.end() + ? cell.dominant_value + : exception->second; + ground += value == static_cast( + wavemap::layered::GeometricClass::kGround); + obstacle += value == static_cast( + wavemap::layered::GeometricClass::kObstacle); + } + } + const size_t parent_count = class_layer.parentCount(); + const size_t observed_count = class_layer.observedValueCount(); + const size_t exception_count = class_layer.exceptionCount(); + const size_t dominant_matching_count = observed_count - exception_count; + // Exact size of the version-1 discrete section for this single int layer: + // section marker, layer metadata/configuration, and all cell records. + const size_t serialized_discrete_bytes = + 67u + 32u * parent_count + 4u * observed_count + + 8u * exception_count; + std::cout << "Class statistics\n" + << " block_height=" << class_layer.config().block_height + << " block_side=" << class_layer.config().blockSideLength() + << " possible_children=" + << class_layer.config().blockVoxelCount() << "\n" + << " parents=" << parent_count << " observed=" << observed_count + << " dominant_matching=" << dominant_matching_count + << " exceptions=" << exception_count << "\n" + << " exception_ratio=" + << (observed_count ? static_cast(exception_count) / + static_cast(observed_count) + : 0.) + << " mean_observed_per_parent=" + << (parent_count ? static_cast(observed_count) / + static_cast(parent_count) + : 0.) + << "\n" + << " serialized_discrete_bytes=" << serialized_discrete_bytes + << " ground=" << ground << " obstacle=" << obstacle << "\n"; + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc new file mode 100644 index 000000000..1e8f4072c --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_chunked_sensor_inputs.cc @@ -0,0 +1,44 @@ +#include "occupancy_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::occupancy_map::chunked { + +using Definition = ChunkedDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& /*server*/, + ServerBuilder::Extension& /*extension*/) { + // Occupancy continues to use Wavemap's original point-cloud integration. +} + +} // namespace wavemap::examples::occupancy_map::chunked + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::occupancy_map::chunked; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::occupancy_map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO("Installed chunked layered occupancy-only benchmark."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_map_layers.h new file mode 100644 index 000000000..d4ead448a --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_map_layers.h @@ -0,0 +1,35 @@ +#ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ + +#include +#include +#include + +namespace wavemap::examples::occupancy_map { + +// Occupancy is built into every layered map, so this schema intentionally has +// no user-defined layers. It is benchmark variant B. +using Schema = layered::schema::LayerSchema<>; +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using ChunkedDefinition = + DefinitionFor; + +template +inline typename DefinitionT::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::occupancy_map + +#endif // WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_OCCUPANCY_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_sensor_inputs.cc new file mode 100644 index 000000000..e2a127bb3 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/occupancy_sensor_inputs.cc @@ -0,0 +1,42 @@ +#include "occupancy_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::occupancy_map { + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& /*server*/, + ServerBuilder::Extension& /*extension*/) { + // Occupancy continues to use Wavemap's original point-cloud integration. +} + +} // namespace wavemap::examples::occupancy_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::occupancy_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO("Installed layered occupancy-only benchmark application (B)."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/ouster_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/ouster_sensor_inputs.cc new file mode 100644 index 000000000..ead884360 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/ouster_sensor_inputs.cc @@ -0,0 +1,61 @@ +#include "reflectivity_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::reflectivity_map { // Ouster mapping example + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + // The Ouster PointCloud2 stores reflectivity as a raw numeric field in + // [0, 255]. Decode it during the existing XYZ traversal, normalize it into + // the map's [0, 1] storage domain, and update occupied endpoints only. + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); + + // This bag has no class field. Derive a small example taxonomy directly + // from each posed cloud: 1 = local ground and 2 = obstacle. + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace wavemap::examples::reflectivity_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::reflectivity_map; + int discrete_block_height = 0; + private_nh.param("discrete_block_height", discrete_block_height, 0); + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(discrete_block_height), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+class benchmark application " + "(D) with discrete block height %d.", discrete_block_height); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc new file mode 100644 index 000000000..484edd721 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_chunked_sensor_inputs.cc @@ -0,0 +1,51 @@ +#include "reflectivity_only_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::reflectivity_only_map { +namespace chunked { + +using Definition = ChunkedDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace chunked +} // namespace wavemap::examples::reflectivity_only_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = + wavemap::examples::reflectivity_only_map::chunked; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::reflectivity_only_map:: + makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed chunked occupancy+reflectivity application."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc new file mode 100644 index 000000000..f93eb68ed --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_class_chunked_sensor_inputs.cc @@ -0,0 +1,58 @@ +#include "reflectivity_map_layers.h" + +#include + +#include +#include +#include + +namespace wavemap::examples::reflectivity_map::chunked { + +using Definition = ChunkedDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); + + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace wavemap::examples::reflectivity_map::chunked + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::reflectivity_map::chunked; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::reflectivity_map:: + makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed chunked occupancy+reflectivity+class benchmark " + "application."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_map_layers.h new file mode 100644 index 000000000..1a6eb88c1 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_map_layers.h @@ -0,0 +1,70 @@ +#ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_MAP_LAYERS_H_ + +#include +#include + +#include +#include +#include +#include + +namespace wavemap::examples::reflectivity_map { + +using ReflectivityReplacePolicy = layered::ReplaceLayerUpdatePolicy; +using ReflectivityExponentialPolicy = + layered::ExponentialScalarLayerUpdatePolicy<1, 5>; + +template +struct ReflectivityLayerTemplate + : layered::schema::ContinuousLayer { + static constexpr layered::ScalarLayerVisualization visualization{ + {0.05f, 0.05f, 0.05f}, {1.f, 1.f, 1.f}}; + static constexpr std::string_view name = "reflectivity"; +}; + +// Both policies are user-selectable. This example uses alpha=0.2 exponential +// smoothing; changing this alias to ReflectivityReplacePolicy selects latest +// observation replacement without changing the adapter or pipeline. +using ReflectivityLayer = + ReflectivityLayerTemplate; + +struct ClassLayer : layered::schema::DiscreteLayer> { + static constexpr std::string_view name = "class"; + static constexpr std::array categories{ + layered::IntegerCategoryLabel{1, "Ground", {0.2f, 0.8f, 0.2f}}, + layered::IntegerCategoryLabel{2, "Obstacle", {0.9f, 0.2f, 0.15f}}}; +}; + +// Occupancy is built into the shared continuous voxel and is not repeated in +// the user schema. +using Schema = layered::schema::LayerSchema; +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using ChunkedDefinition = + DefinitionFor; + +template +inline typename DefinitionT::Config makeMapConfig( + int discrete_block_height = 0) { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.discreteBlockHeight(discrete_block_height); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::reflectivity_map + +#endif // WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_map_layers.h new file mode 100644 index 000000000..6fec5239d --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_map_layers.h @@ -0,0 +1,36 @@ +#ifndef WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_ONLY_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_ONLY_MAP_LAYERS_H_ + +#include "reflectivity_map_layers.h" + +namespace wavemap::examples::reflectivity_only_map { + +using ReflectivityLayer = reflectivity_map::ReflectivityLayer; +using Schema = layered::schema::LayerSchema; +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using ChunkedDefinition = + DefinitionFor; + +template +inline typename DefinitionT::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::reflectivity_only_map + +#endif // WAVEMAP_EXAMPLES_DIGITAL_TWIN_OUSTER_REFLECTIVITY_ONLY_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_sensor_inputs.cc new file mode 100644 index 000000000..20d739d37 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/reflectivity_only_sensor_inputs.cc @@ -0,0 +1,45 @@ +#include "reflectivity_only_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::reflectivity_only_map { + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, + layered::EndpointRange{0.5f, 25.f}); +} + +} // namespace wavemap::examples::reflectivity_only_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::reflectivity_only_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity benchmark application (C)."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h new file mode 100644 index 000000000..b3e220bc5 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/rgb_map_layers.h @@ -0,0 +1,43 @@ +#ifndef WAVEMAP_ROS_APP_OUSTER_LAYERED_RGB_MAP_LAYERS_H_ +#define WAVEMAP_ROS_APP_OUSTER_LAYERED_RGB_MAP_LAYERS_H_ + +#include + +#include +#include +#include +#include +#include + +namespace wavemap::examples::rgb_map { + +struct RgbLayer + : layered::schema::ContinuousLayer< + layered::Rgb, + layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "rgb"; +}; + +using Schema = layered::schema::LayerSchema; +using Definition = layered::LayeredMapDefinition; + +inline Definition::Config makeMapConfig() { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.layer() + .storageBounds(layered::Rgb{0.f, 0.f, 0.f}, + layered::Rgb{1.f, 1.f, 1.f}) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.combinedPruningThreshold(1.f); + return config.build(); +} + +} // namespace wavemap::examples::rgb_map + +#endif // WAVEMAP_ROS_APP_OUSTER_LAYERED_RGB_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc new file mode 100644 index 000000000..ace5a0168 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_class_sensor_inputs.cc @@ -0,0 +1,61 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map::signal_class_map { + +using Definition = ClassDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, endpoint_range); +} + +} // namespace wavemap::examples::signal_map::signal_class_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map::signal_class_map; + int discrete_block_height = 0; + private_nh.param("discrete_block_height", discrete_block_height, 0); + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::signal_map::makeMapConfigFor( + discrete_block_height), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+signal+class benchmark " + "application (G) with discrete block height %d.", + discrete_block_height); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h new file mode 100644 index 000000000..e06028016 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_map_layers.h @@ -0,0 +1,86 @@ +#ifndef WAVEMAP_EXAMPLES_OUSTER_SIGNAL_MAP_LAYERS_H_ +#define WAVEMAP_EXAMPLES_OUSTER_SIGNAL_MAP_LAYERS_H_ + +#include + +#include "reflectivity_map_layers.h" + +namespace wavemap::examples::signal_map { + +using ReflectivityLayer = reflectivity_map::ReflectivityLayer; +using ClassLayer = reflectivity_map::ClassLayer; +using SignalPolicy = layered::ExponentialScalarLayerUpdatePolicy<1, 5>; + +struct SignalLayer + : layered::schema::ContinuousLayer { + static constexpr layered::ScalarLayerVisualization visualization{ + {0.02f, 0.04f, 0.20f}, {0.00f, 1.00f, 1.00f}}; + static constexpr std::string_view name = "signal"; +}; + +struct NearIrLayer + : layered::schema::ContinuousLayer { + static constexpr layered::ScalarLayerVisualization visualization{ + {0.08f, 0.00f, 0.15f}, {1.00f, 0.85f, 0.00f}}; + static constexpr std::string_view name = "near_ir"; +}; + +// Occupancy remains built into every shared continuous voxel. +using Schema = + layered::schema::LayerSchema; +using NearIrSchema = layered::schema::LayerSchema< + ReflectivityLayer, SignalLayer, NearIrLayer>; +using ClassSchema = layered::schema::LayerSchema< + ReflectivityLayer, SignalLayer, ClassLayer>; +using NearIrClassSchema = layered::schema::LayerSchema< + ReflectivityLayer, SignalLayer, NearIrLayer, ClassLayer>; + +template +using DefinitionFor = + layered::LayeredMapDefinition; +using Definition = DefinitionFor; +using NearIrDefinition = DefinitionFor; +using ClassDefinition = DefinitionFor; +using NearIrClassDefinition = DefinitionFor; + +template +inline typename DefinitionT::Config makeMapConfigFor( + [[maybe_unused]] int discrete_block_height = 0) { + layered::LayeredMapConfigBuilder config; + config.map() + .minCellWidth(0.25f) + .occupancyLogOdds(-2.f, 4.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(5.f); + config.occupancy().pruningScale(1e-3f).pruningWeight(1.f); + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + if constexpr ( + DefinitionT::Schema::template containsContinuous) { + config.template layer() + .storageBounds(0.f, 1.f) + .pruningScale(1e-3f) + .pruningWeight(1.f); + } + if constexpr ( + DefinitionT::Schema::template containsDiscrete) { + config.discreteBlockHeight(discrete_block_height); + } + config.combinedPruningThreshold(1.f); + return config.build(); +} + +inline Definition::Config makeMapConfig() { + return makeMapConfigFor(); +} + +} // namespace wavemap::examples::signal_map + +#endif // WAVEMAP_EXAMPLES_OUSTER_SIGNAL_MAP_LAYERS_H_ diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc new file mode 100644 index 000000000..839f1ea41 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_class_sensor_inputs.cc @@ -0,0 +1,64 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map::near_ir_class_map { + +using Definition = NearIrClassDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); + server.bindNormalizedPointField( + extension, "ambient", 0.f, 65535.f, endpoint_range); + + layered::LocalElevationClassifierConfig class_config; + class_config.grid_cell_width = 0.5f; + class_config.ground_tolerance = 0.2f; + class_config.obstacle_height = 0.35f; + class_config.min_points_per_cell = 3u; + server.bindGeometricClass( + extension, class_config, endpoint_range, -kHalfPi, kHalfPi); +} + +} // namespace wavemap::examples::signal_map::near_ir_class_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map::near_ir_class_map; + int discrete_block_height = 0; + private_nh.param("discrete_block_height", discrete_block_height, 0); + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::signal_map::makeMapConfigFor( + discrete_block_height), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+signal+near_ir+class benchmark " + "application (H) with discrete block height %d.", + discrete_block_height); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc new file mode 100644 index 000000000..6186f445e --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_near_ir_sensor_inputs.cc @@ -0,0 +1,50 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map::near_ir_map { + +using Definition = NearIrDefinition; +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); + server.bindNormalizedPointField( + extension, "ambient", 0.f, 65535.f, endpoint_range); +} + +} // namespace wavemap::examples::signal_map::near_ir_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map::near_ir_map; + map::ServerBuilder::install( + server, private_nh, server.worldFrame(), + wavemap::examples::signal_map::makeMapConfigFor(), + map::registerSensorInputs); + ROS_INFO("Installed occupancy+reflectivity+signal+near_ir benchmark application (F)."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc new file mode 100644 index 000000000..526d17a27 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/ouster_layered/signal_sensor_inputs.cc @@ -0,0 +1,50 @@ +#include "signal_map_layers.h" + +#include + +#include +#include + +namespace wavemap::examples::signal_map { + +using RosConverter = + convert::DescriptorContinuousRosConverter; +using ServerBuilder = LayeredRosServerBuilder; + +void registerSensorInputs(RosServer& server, + ServerBuilder::Extension& extension) { + const layered::EndpointRange endpoint_range{0.5f, 25.f}; + server.bindNormalizedPointField( + extension, "reflectivity", 0.f, 255.f, endpoint_range); + // The bag's Ouster profile is SIG16. Preserve its full sensor-defined range + // in normalized storage rather than fitting bounds to this recording. + server.bindNormalizedPointField( + extension, "intensity", 0.f, 65535.f, endpoint_range); +} + +} // namespace wavemap::examples::signal_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "wavemap_ros_server"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + wavemap::RosServer server( + nh, nh_private, + [](wavemap::RosServer& server, ros::NodeHandle private_nh) { + namespace map = wavemap::examples::signal_map; + map::ServerBuilder::install(server, private_nh, server.worldFrame(), + map::makeMapConfig(), + map::registerSensorInputs); + ROS_INFO( + "Installed occupancy+reflectivity+signal benchmark application " + "(E)."); + }); + + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc b/interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc new file mode 100644 index 000000000..800a9db68 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/app/synthetic_rgb_layered_map_demo.cc @@ -0,0 +1,200 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ouster_layered/rgb_map_layers.h" + +namespace wavemap::examples::rgb_map { +namespace { + +struct SceneVoxel { + Index3D index; + layered::Rgb color; +}; + +constexpr FloatingPoint kOccupiedLogOdds = 4.f; +constexpr int kFloorCells = 52; +constexpr int kPathHalfWidth = 3; + +const layered::Rgb kGrass{0.20f, 0.55f, 0.16f}; +const layered::Rgb kPath{0.46f, 0.27f, 0.12f}; +const layered::Rgb kTrunk{0.34f, 0.16f, 0.06f}; + +void appendFloor(std::vector& scene) { + const int min_coordinate = -kFloorCells / 2; + const int max_coordinate = min_coordinate + kFloorCells; + scene.reserve(kFloorCells * kFloorCells); + for (int y = min_coordinate; y < max_coordinate; ++y) { + const int path_center = + static_cast(std::round(2.5 * std::sin(y / 9.0))); + for (int x = min_coordinate; x < max_coordinate; ++x) { + const bool is_path = std::abs(x - path_center) <= kPathHalfWidth; + scene.push_back({Index3D{x, y, 0}, is_path ? kPath : kGrass}); + } + } +} + +void appendTree(const Index2D& center, std::mt19937& generator, + std::vector& scene) { + for (int z = 1; z <= 8; ++z) { + for (int dx = 0; dx < 2; ++dx) { + for (int dy = 0; dy < 2; ++dy) { + scene.push_back( + {Index3D{center.x() + dx, center.y() + dy, z}, kTrunk}); + } + } + } + + std::uniform_real_distribution random_value(0.f, 1.f); + std::uniform_real_distribution green_variation(-0.10f, 0.10f); + std::unordered_set added_leaf_indices; + for (int dz = -2; dz <= 3; ++dz) { + for (int dx = -4; dx <= 5; ++dx) { + for (int dy = -4; dy <= 5; ++dy) { + const float nx = (static_cast(dx) - 0.5f) / 4.6f; + const float ny = (static_cast(dy) - 0.5f) / 4.6f; + const float nz = (static_cast(dz) - 0.5f) / 3.2f; + const float radius = nx * nx + ny * ny + nz * nz; + const float keep_probability = + std::clamp(1.25f - radius, 0.f, 0.92f); + if (random_value(generator) > keep_probability) { + continue; + } + + const Index3D index{center.x() + dx, center.y() + dy, 9 + dz}; + if (!added_leaf_indices.emplace(index).second) { + continue; + } + const float variation = green_variation(generator); + scene.push_back( + {index, + layered::Rgb{std::clamp(0.12f + 0.4f * variation, 0.f, 1.f), + std::clamp(0.48f + variation, 0.f, 1.f), + std::clamp(0.10f + 0.25f * variation, 0.f, 1.f)}}); + } + } + } +} + +std::vector makeScene() { + std::vector scene; + appendFloor(scene); + + std::mt19937 generator(42u); + for (const Index2D& center : std::vector{ + {-18, -17}, {-17, 14}, {-11, -4}, {12, -17}, {16, 13}, {10, 4}}) { + appendTree(center, generator, scene); + } + return scene; +} + +} // namespace + +class SyntheticRgbLayeredMapDemo { + public: + SyntheticRgbLayeredMapDemo() + : private_nh_("~"), map_(makeMapConfig()), scene_(makeScene()) { + private_nh_.param("voxels_per_update", voxels_per_update_, 120); + private_nh_.param("update_period", update_period_, 0.08); + private_nh_.param("save_path", save_path_, std::string{}); + voxels_per_update_ = std::max(1, voxels_per_update_); + update_period_ = std::max(0.01, update_period_); + + publisher_ = private_nh_.advertise( + "layered_map", 1, true); + timer_ = private_nh_.createTimer( + ros::Duration(update_period_), + &SyntheticRgbLayeredMapDemo::insertNextVoxels, this); + + ROS_INFO_STREAM("Synthetic RGB Layered Wavemap contains " << scene_.size() + << " voxels and will be revealed in batches of " + << voxels_per_update_ << "."); + } + + private: + void insertNextVoxels(const ros::TimerEvent&) { + const size_t next_end = std::min( + next_voxel_ + static_cast(voxels_per_update_), scene_.size()); + for (; next_voxel_ < next_end; ++next_voxel_) { + Definition::ContinuousLayers layers; + layers.get() = scene_[next_voxel_].color; + map_.continuousMap().setVoxelValue( + scene_[next_voxel_].index, + Definition::Voxel{kOccupiedLogOdds, layers}); + } + publishMap(); + + if (next_voxel_ == scene_.size()) { + timer_.stop(); + map_.continuousMap().threshold(); + map_.continuousMap().prune(); + publishMap(); + saveMapIfRequested(); + ROS_INFO("Synthetic RGB Layered Wavemap is complete."); + } + } + + void publishMap() { + wavemap_msgs::LayeredMap message; + using RosConverter = + convert::DescriptorContinuousRosConverter; + if (!convert::layeredMapToRosMsg( + map_, "map", ros::Time::now(), message)) { + ROS_ERROR("Could not convert the synthetic RGB map to a ROS message."); + return; + } + publisher_.publish(message); + } + + void saveMapIfRequested() { + if (save_path_.empty()) { + return; + } + const std::filesystem::path output_path(save_path_); + if (output_path.has_parent_path()) { + std::filesystem::create_directories(output_path.parent_path()); + } + if (Definition::MapIo::save(output_path, map_)) { + ROS_INFO_STREAM("Saved synthetic RGB Layered Wavemap to " + << output_path); + } else { + ROS_ERROR_STREAM("Could not save synthetic RGB Layered Wavemap to " + << output_path); + } + } + + ros::NodeHandle private_nh_; + Definition::Map map_; + std::vector scene_; + size_t next_voxel_ = 0u; + int voxels_per_update_ = 120; + double update_period_ = 0.08; + std::string save_path_; + ros::Publisher publisher_; + ros::Timer timer_; +}; + +} // namespace wavemap::examples::rgb_map + +int main(int argc, char** argv) { + ros::init(argc, argv, "synthetic_rgb_layered_map"); + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + FLAGS_alsologtostderr = true; + FLAGS_colorlogtostderr = true; + + wavemap::examples::rgb_map::SyntheticRgbLayeredMapDemo demo; + ros::spin(); + return 0; +} diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml new file mode 100644 index 000000000..07fba6e37 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_layered.yaml @@ -0,0 +1,43 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +# Layered map geometry and layer configuration are supplied by the selected +# typed server executable. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml new file mode 100644 index 000000000..56b0b7cc7 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_layered.yaml @@ -0,0 +1,42 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +# The layered map geometry is supplied by occupancy_map_layers.h. +# Keep ROS serialization and periodic maintenance outside the integration timer. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml new file mode 100644 index 000000000..b42a29b36 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_chunked_occupancy_original.yaml @@ -0,0 +1,50 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +map: + type: hashed_chunked_wavelet_octree + min_cell_width: { meters: 0.25 } + min_log_odds: -2.0 + max_log_odds: 4.0 + tree_height: 7 + only_prune_blocks_if_unused_for: { seconds: 5.0 } + +# Keep ROS serialization and periodic maintenance outside the integration timer. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml new file mode 100644 index 000000000..ef08f851b --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_layered.yaml @@ -0,0 +1,43 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +# Layered map geometry and layer configuration are supplied by the selected +# typed server executable. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml new file mode 100644 index 000000000..7af2180ba --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_hashed_occupancy_original.yaml @@ -0,0 +1,50 @@ +general: + world_frame: "map" + logging_level: info + allow_reset_map_service: true + +map: + type: hashed_wavelet_octree + min_cell_width: { meters: 0.25 } + min_log_odds: -2.0 + max_log_odds: 4.0 + tree_height: 7 + only_prune_blocks_if_unused_for: { seconds: 5.0 } + +# Keep ROS serialization and periodic maintenance outside the integration timer. +map_operations: [] + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } + enable_benchmark_metrics: true diff --git a/interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv new file mode 100644 index 000000000..0a2e44b5e --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/benchmark_ouster_variants.csv @@ -0,0 +1,19 @@ +variant,backend,layers,server_executable,param_file,output_directory,map_filename +A,hashed,occupancy,ros_server,benchmark_ouster_hashed_occupancy_original.yaml,A_original,map.wvmp +B,hashed,occupancy,occupancy_layered_server,benchmark_ouster_hashed_layered.yaml,B_layered_occupancy,map.lwvmp +C,hashed,occupancy+reflectivity,reflectivity_layered_server,benchmark_ouster_hashed_layered.yaml,C_layered_reflectivity,map.lwvmp +D,hashed,occupancy+reflectivity+class,reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class,map.lwvmp +D_h1,hashed,occupancy+reflectivity+class (discrete block height 1),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h1,map.lwvmp +D_h2,hashed,occupancy+reflectivity+class (discrete block height 2),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h2,map.lwvmp +D_h3,hashed,occupancy+reflectivity+class (discrete block height 3),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h3,map.lwvmp +D_h4,hashed,occupancy+reflectivity+class (discrete block height 4),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h4,map.lwvmp +D_h5,hashed,occupancy+reflectivity+class (discrete block height 5),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h5,map.lwvmp +D_h6,hashed,occupancy+reflectivity+class (discrete block height 6),reflectivity_class_layered_server,benchmark_ouster_hashed_layered.yaml,D_layered_reflectivity_class_h6,map.lwvmp +E,hashed,occupancy+reflectivity+signal,signal_layered_server,benchmark_ouster_hashed_layered.yaml,E_layered_reflectivity_signal,map.lwvmp +F,hashed,occupancy+reflectivity+signal+near_ir,signal_near_ir_layered_server,benchmark_ouster_hashed_layered.yaml,F_layered_reflectivity_signal_near_ir,map.lwvmp +G,hashed,occupancy+reflectivity+signal+class,signal_class_layered_server,benchmark_ouster_hashed_layered.yaml,G_layered_reflectivity_signal_class,map.lwvmp +H,hashed,occupancy+reflectivity+signal+near_ir+class,signal_near_ir_class_layered_server,benchmark_ouster_hashed_layered.yaml,H_layered_reflectivity_signal_near_ir_class,map.lwvmp +chunked_A,chunked,occupancy,ros_server,benchmark_ouster_chunked_occupancy_original.yaml,chunked_A_original,map.wvmp +chunked_B,chunked,occupancy,occupancy_chunked_layered_server,benchmark_ouster_chunked_layered.yaml,chunked_B_layered_occupancy,map.lwvmp +chunked_C,chunked,occupancy+reflectivity,reflectivity_chunked_layered_server,benchmark_ouster_chunked_layered.yaml,chunked_C_layered_reflectivity,map.lwvmp +chunked_D,chunked,occupancy+reflectivity+class,reflectivity_class_chunked_layered_server,benchmark_ouster_chunked_layered.yaml,chunked_D_layered_reflectivity_class,map.lwvmp diff --git a/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz b/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz new file mode 100644 index 000000000..1b46ef6a5 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/rviz/layered_map.rviz @@ -0,0 +1,152 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LayeredMap1 + - /LayeredMap1/Render voxels1 + - /LayeredMap1/Render voxels1/Layer color1 + - /LayeredMap1/Render voxels1/Frame rate1 + Splitter Ratio: 0.5 + Tree Height: 1540 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: wavemap_rviz_plugin/LayeredMap + Enabled: true + Name: LayeredMap + Queue Size: 10 + Source: Topic + Render voxels: + Alpha: 1 + Cell selector: Surface + Color mode: Height + Enable: true + Flat color: 0; 0; 255 + Frame rate: + Max update time: 20 + Layer: occupancy + Layer color: + Bool true color: 255; 12; 12 + Scalar high color: 255; 255; 0 + Scalar low color: 0; 0; 255 + Scalar max: 0.2 + Scalar min: 0 + Show bool false: false + Log odds threshold: 0.0010000000474974513 + Max log odds: 1e+06 + Min log odds: 0.0010000000474974513 + Termination height: 0 + Unknown log odds: 9.999999747378752e-05 + Topic: /wavemap/layered_map + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.1961989402770996 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5203978419303894 + Target Frame: + Yaw: 0.670401394367218 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2022 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000331000006f2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000abfb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000006f20000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000006f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000006f20000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7c00000056fc0100000002fb0000000800540069006d0065010000000000000e7c0000058200fffffffb0000000800540069006d00650100000000000004500000000000000000000009e4000006f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3708 + X: 1860 + Y: 64 diff --git a/interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz b/interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz new file mode 100644 index 000000000..82e4b20c7 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/rviz/synthetic_rgb_layered_map.rviz @@ -0,0 +1,106 @@ +Panels: + - Class: rviz/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RGB Layered Wavemap1 + - /RGB Layered Wavemap1/Render voxels1 + Splitter Ratio: 0.5 + Tree Height: 650 + - Class: rviz/Views + Name: Views +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 40 + Reference Frame: + Value: true + - Class: wavemap_rviz_plugin/LayeredMap + Enabled: true + Name: Layered Wavemap + Queue Size: 10 + Source: Topic + Render voxels: + Alpha: 1 + Cell selector: Band + Color mode: Flat + Enable: true + Flat color: 255; 255; 255 + Frame rate: + Max update time: 20 + Layer: rgb + Layer color: + Bool true color: 255; 12; 12 + Scalar high color: 255; 255; 0 + Scalar low color: 0; 0; 255 + Scalar max: 1 + Scalar min: 0 + Show bool false: false + Max log odds: 1000000 + Min log odds: 0.001 + Termination height: 0 + Unknown log odds: 0.0001 + Topic: /synthetic_rgb_layered_map/layered_map + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 20 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.785398 + Focal Point: + X: 0 + Y: 0 + Z: 1 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.65 + Target Frame: + Yaw: 0.75 + Saved: ~ +Window Geometry: + Height: 900 + Width: 1440 diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml new file mode 100644 index 000000000..cdbd78015 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_all_layers_demo.yaml @@ -0,0 +1,51 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "layered_map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + # IG-LIO publishes one map-to-body pose per processed scan. This bag's + # Ouster t field is not usable as Wavemap-relative per-point time, so asking + # Wavemap for intermediate poses drops the cloud. + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml new file mode 100644 index 000000000..6e9791cfb --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_occupancy_only.yaml @@ -0,0 +1,57 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +# Original occupancy-only Wavemap data structure. No layered map is enabled. +map: + type: hashed_wavelet_octree + min_cell_width: { meters: 0.25 } + min_log_odds: -2.0 + max_log_odds: 4.0 + tree_height: 7 + only_prune_blocks_if_unused_for: { seconds: 5.0 } + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml new file mode 100644 index 000000000..aee2b4188 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_chunked_layered.yaml @@ -0,0 +1,48 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "layered_map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_chunked_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml new file mode 100644 index 000000000..cdbd78015 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/config/wavemap_ouster_reflectivity_layered.yaml @@ -0,0 +1,51 @@ +general: + world_frame: "map" + logging_level: debug + allow_reset_map_service: true + +map_operations: + - type: threshold_map + once_every: { seconds: 2.0 } + - type: prune_map + once_every: { seconds: 10.0 } + - type: publish_map + once_every: { seconds: 0.5 } + max_num_blocks_per_msg: 50 + topic: "layered_map" + +measurement_integrators: + ouster_os1: + projection_model: + type: ouster_projector + lidar_origin_to_beam_origin: { millimeters: 16.721 } + lidar_origin_to_sensor_origin_z_offset: { millimeters: 38.195 } + elevation: + num_cells: 128 + min_angle: { degrees: -21.39 } + max_angle: { degrees: 21.25 } + azimuth: + num_cells: 1024 + min_angle: { degrees: -180.0 } + max_angle: { degrees: 180.0 } + measurement_model: + type: continuous_beam + angle_sigma: { degrees: 0.1 } + range_sigma: { meters: 0.05 } + scaling_free: 0.2 + scaling_occupied: 0.4 + integration_method: + type: hashed_wavelet_integrator + min_range: { meters: 0.5 } + max_range: { meters: 25.0 } + +inputs: + - type: pointcloud_topic + topic_name: "/ouster/points" + topic_type: ouster + measurement_integrator_names: ouster_os1 + # IG-LIO publishes one map-to-body pose per processed scan. This bag's + # Ouster t field is not usable as Wavemap-relative per-point time, so asking + # Wavemap for intermediate poses drops the cloud. + undistort_motion: false + topic_queue_length: 10 + max_wait_for_pose: { seconds: 1.0 } diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h new file mode 100644 index 000000000..f4a1aa556 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_endpoint_adapter.h @@ -0,0 +1,347 @@ +#ifndef WAVEMAP_ROS_INPUTS_POINTCLOUD_ENDPOINT_ADAPTER_H_ +#define WAVEMAP_ROS_INPUTS_POINTCLOUD_ENDPOINT_ADAPTER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace wavemap { + +template +struct PointFieldBinding { + std::string field_name; + FloatingPoint scale = 1.f; + FloatingPoint offset = 0.f; + ValueT min_value = std::numeric_limits::lowest(); + ValueT max_value = std::numeric_limits::max(); + + explicit PointFieldBinding( + std::string field_name = {}, FloatingPoint scale = 1.f, + FloatingPoint offset = 0.f, + ValueT min_value = std::numeric_limits::lowest(), + ValueT max_value = std::numeric_limits::max()) + : field_name(std::move(field_name)), + scale(scale), + offset(offset), + min_value(min_value), + max_value(max_value) {} + + static PointFieldBinding normalized(std::string field_name, + FloatingPoint raw_min, + FloatingPoint raw_max) { + CHECK_LT(raw_min, raw_max); + const FloatingPoint scale = 1.f / (raw_max - raw_min); + return PointFieldBinding(std::move(field_name), scale, + -raw_min * scale, ValueT{0}, ValueT{1}); + } +}; + +class DecodedPointcloudEndpointChannel { + public: + virtual ~DecodedPointcloudEndpointChannel() = default; + virtual bool integrate( + const PosedPointcloud<>& pointcloud, + const std::vector& sorted_points) const = 0; +}; + +// A message-local decoder. PointcloudTopicInput owns the outer point loop and +// calls every prepared decoder once per point, so adding endpoint layers does +// not add another traversal of the PointCloud2 message. +class PointcloudEndpointDecoder { + public: + virtual ~PointcloudEndpointDecoder() = default; + virtual bool decodePoint(const sensor_msgs::PointCloud2& msg, + size_t point_offset) = 0; + virtual std::unique_ptr finish() = 0; +}; + +class PointcloudEndpointAdapter { + public: + virtual ~PointcloudEndpointAdapter() = default; + virtual std::unique_ptr prepare( + const sensor_msgs::PointCloud2& msg, size_t num_points) const = 0; +}; + +template +class DecodedScalarEndpointChannel final + : public DecodedPointcloudEndpointChannel { + public: + using Callback = + std::function&, const std::vector&)>; + + DecodedScalarEndpointChannel(std::vector values, Callback callback) + : values_(std::move(values)), callback_(std::move(callback)) {} + + bool integrate( + const PosedPointcloud<>& pointcloud, + const std::vector& sorted_points) + const override { + if (values_.size() != pointcloud.size() || + sorted_points.size() != pointcloud.size()) { + return false; + } + std::vector ordered_values(values_.size()); + for (size_t index = 0u; index < sorted_points.size(); ++index) { + if (sorted_points[index].source_index >= values_.size()) { + return false; + } + ordered_values[index] = values_[sorted_points[index].source_index]; + } + callback_(pointcloud, ordered_values); + return true; + } + + private: + std::vector values_; + Callback callback_; +}; + +template +class NumericPointcloudEndpointDecoder final + : public PointcloudEndpointDecoder { + public: + using Callback = typename DecodedScalarEndpointChannel::Callback; + + NumericPointcloudEndpointDecoder(const sensor_msgs::PointField& field, + size_t num_points, FloatingPoint scale, + FloatingPoint offset, ValueT min_value, + ValueT max_value, Callback callback) + : field_(field), + scale_(scale), + offset_(offset), + min_value_(min_value), + max_value_(max_value), + callback_(std::move(callback)) { + values_.reserve(num_points); + } + + bool decodePoint(const sensor_msgs::PointCloud2& msg, + size_t point_offset) override { + FloatingPoint raw = 0.f; + if (!convert::detail::readNumericPointField(msg, field_, point_offset, + raw) || + !std::isfinite(raw)) { + return false; + } + const FloatingPoint converted = offset_ + scale_ * raw; + if (!std::isfinite(converted)) { + return false; + } + if constexpr (std::is_integral_v) { + if (converted != std::trunc(converted) || + converted < static_cast( + std::numeric_limits::lowest()) || + static_cast(std::numeric_limits::max()) < + converted) { + return false; + } + } + values_.emplace_back( + std::clamp(static_cast(converted), min_value_, max_value_)); + return true; + } + + std::unique_ptr finish() override { + return std::make_unique>( + std::move(values_), std::move(callback_)); + } + + private: + sensor_msgs::PointField field_; + FloatingPoint scale_; + FloatingPoint offset_; + ValueT min_value_; + ValueT max_value_; + Callback callback_; + std::vector values_; +}; + +template +class NumericPointcloudEndpointAdapter final + : public PointcloudEndpointAdapter { + public: + using Callback = typename DecodedScalarEndpointChannel::Callback; + + NumericPointcloudEndpointAdapter(std::string field_name, FloatingPoint scale, + FloatingPoint offset, ValueT min_value, + ValueT max_value, Callback callback) + : field_name_(std::move(field_name)), + scale_(scale), + offset_(offset), + min_value_(min_value), + max_value_(max_value), + callback_(std::move(callback)) {} + + std::unique_ptr prepare( + const sensor_msgs::PointCloud2& msg, size_t num_points) const override { + const auto* field = convert::detail::findField(msg, field_name_); + if (!field || msg.point_step == 0u || msg.width == 0u || + msg.is_bigendian) { + return nullptr; + } + return std::make_unique>( + *field, num_points, scale_, offset_, min_value_, max_value_, callback_); + } + + private: + std::string field_name_; + FloatingPoint scale_; + FloatingPoint offset_; + ValueT min_value_; + ValueT max_value_; + Callback callback_; +}; + +// Lightweight view passed to a user conversion function. Field names and ROS +// datatypes are resolved once in prepare(); the per-point callback uses only +// numeric indices and performs no allocation or string lookup. +class NumericPointFieldValues { + public: + explicit NumericPointFieldValues(const std::vector& values) + : values_(values) {} + + size_t size() const { return values_.size(); } + FloatingPoint operator[](size_t index) const { return values_[index]; } + + private: + const std::vector& values_; +}; + +template +class DecodedOptionalScalarEndpointChannel final + : public DecodedPointcloudEndpointChannel { + public: + using OptionalValue = std::optional; + using Callback = std::function&, + const std::vector&)>; + + DecodedOptionalScalarEndpointChannel(std::vector values, + Callback callback) + : values_(std::move(values)), callback_(std::move(callback)) {} + + bool integrate( + const PosedPointcloud<>& pointcloud, + const std::vector& sorted_points) + const override { + if (values_.size() != pointcloud.size() || + sorted_points.size() != pointcloud.size()) { + return false; + } + std::vector ordered_values(values_.size()); + for (size_t index = 0u; index < sorted_points.size(); ++index) { + if (sorted_points[index].source_index >= values_.size()) { + return false; + } + ordered_values[index] = values_[sorted_points[index].source_index]; + } + callback_(pointcloud, ordered_values); + return true; + } + + private: + std::vector values_; + Callback callback_; +}; + +template +class ComputedNumericPointcloudEndpointDecoder final + : public PointcloudEndpointDecoder { + public: + using OptionalValue = std::optional; + using Converter = + std::function; + using Callback = + typename DecodedOptionalScalarEndpointChannel::Callback; + + ComputedNumericPointcloudEndpointDecoder( + std::vector fields, size_t num_points, + Converter converter, Callback callback) + : fields_(std::move(fields)), + field_values_(fields_.size()), + converter_(std::move(converter)), + callback_(std::move(callback)) { + values_.reserve(num_points); + } + + bool decodePoint(const sensor_msgs::PointCloud2& msg, + size_t point_offset) override { + for (size_t field_index = 0u; field_index < fields_.size(); ++field_index) { + auto& value = field_values_[field_index]; + if (!convert::detail::readNumericPointField( + msg, fields_[field_index], point_offset, value) || + !std::isfinite(value)) { + values_.emplace_back(std::nullopt); + return true; + } + } + values_.emplace_back(converter_(NumericPointFieldValues(field_values_))); + return true; + } + + std::unique_ptr finish() override { + return std::make_unique>( + std::move(values_), std::move(callback_)); + } + + private: + std::vector fields_; + std::vector field_values_; + Converter converter_; + Callback callback_; + std::vector values_; +}; + +template +class ComputedNumericPointcloudEndpointAdapter final + : public PointcloudEndpointAdapter { + public: + using Decoder = ComputedNumericPointcloudEndpointDecoder; + using Converter = typename Decoder::Converter; + using Callback = typename Decoder::Callback; + + ComputedNumericPointcloudEndpointAdapter( + std::vector field_names, Converter converter, + Callback callback) + : field_names_(std::move(field_names)), + converter_(std::move(converter)), + callback_(std::move(callback)) {} + + std::unique_ptr prepare( + const sensor_msgs::PointCloud2& msg, size_t num_points) const override { + if (field_names_.empty() || msg.point_step == 0u || msg.width == 0u || + msg.is_bigendian) { + return nullptr; + } + std::vector fields; + fields.reserve(field_names_.size()); + for (const auto& field_name : field_names_) { + const auto* field = convert::detail::findField(msg, field_name); + if (!field) { + return nullptr; + } + fields.emplace_back(*field); + } + return std::make_unique( + std::move(fields), num_points, converter_, callback_); + } + + private: + std::vector field_names_; + Converter converter_; + Callback callback_; +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_INPUTS_POINTCLOUD_ENDPOINT_ADAPTER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h index f9148eecd..16141e586 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/pointcloud_topic_input.h @@ -2,16 +2,20 @@ #define WAVEMAP_ROS_INPUTS_POINTCLOUD_TOPIC_INPUT_H_ #include +#include +#include #include #include #include #include #include +#include #include #include #include "wavemap_ros/inputs/ros_input_base.h" +#include "wavemap_ros/inputs/pointcloud_endpoint_adapter.h" #include "wavemap_ros/utils/pointcloud_undistorter.h" #ifdef LIVOX_AVAILABLE @@ -31,7 +35,7 @@ struct PointcloudTopicType : public TypeSelector { * Config struct for the pointcloud input handler. */ struct PointcloudTopicInputConfig - : public ConfigBase { //! Name of the ROS topic to subscribe to. std::string topic_name; @@ -75,6 +79,9 @@ struct PointcloudTopicInputConfig //! Name of the topic on which to republish the motion-undistorted //! pointclouds. Useful to share the undistorted pointclouds with other ROS //! nodes and for debugging. Disabled if not set. + //! Collect aggregate integration statistics and expose them through the + //! private benchmark_stats service. Disabled during normal operation. + bool enable_benchmark_metrics = false; std::string undistorted_pointcloud_topic_name; static MemberMap memberMap; @@ -100,6 +107,14 @@ class PointcloudTopicInput : public RosInputBase { return RosInputType::kPointcloudTopic; } PointcloudTopicType getTopicType() const { return config_.topic_type; } + void addEndpointAdapter(std::shared_ptr adapter) { + endpoint_adapters_.emplace_back(std::move(adapter)); + } + using PosedCloudCallback = + std::function& pointcloud)>; + void addPosedCloudCallback(PosedCloudCallback callback) { + posed_cloud_callbacks_.emplace_back(std::move(callback)); + } void callback(const sensor_msgs::PointCloud2& pointcloud_msg); #ifdef LIVOX_AVAILABLE @@ -117,7 +132,33 @@ class PointcloudTopicInput : public RosInputBase { PointcloudUndistorter pointcloud_undistorter_; ros::Subscriber pointcloud_sub_; - std::queue pointcloud_queue_; + struct QueuedPointcloud { + undistortion::StampedPointcloud pointcloud; + std::vector> + endpoint_channels; + }; + std::queue pointcloud_queue_; + std::vector> endpoint_adapters_; + std::vector posed_cloud_callbacks_; + + struct BenchmarkMetrics { + size_t received_scans = 0u; + size_t integrated_scans = 0u; + size_t dropped_scans = 0u; + size_t pending_scans = 0u; + size_t received_points = 0u; + size_t integrated_points = 0u; + size_t maximum_queue_size = 0u; + std::vector integration_durations; + } benchmark_metrics_; + std::mutex benchmark_metrics_mutex_; + ros::ServiceServer benchmark_stats_srv_; + + void recordReceivedScan(size_t num_points); + void recordDroppedScan(); + void recordIntegratedScan(size_t num_points, double duration_seconds); + bool benchmarkStatsCallback(std_srvs::Trigger::Request& request, + std_srvs::Trigger::Response& response); void processQueue() override; static bool hasField(const sensor_msgs::PointCloud2& msg, diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h new file mode 100644 index 000000000..01617c257 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/inputs/sensor_frame_transformer.h @@ -0,0 +1,80 @@ +#ifndef WAVEMAP_ROS_INPUTS_SENSOR_FRAME_TRANSFORMER_H_ +#define WAVEMAP_ROS_INPUTS_SENSOR_FRAME_TRANSFORMER_H_ + +#include +#include +#include +#include + +#include +#include + +#include "wavemap_ros/utils/tf_transformer.h" + +namespace wavemap { + +// Reuses the RosServer's TF buffer to convert custom sensor observations into +// the map's world frame. This helper owns neither a subscriber nor a TF +// listener and can therefore be stored directly by a user sensor-input class. +class SensorFrameTransformer { + public: + SensorFrameTransformer(std::shared_ptr transformer, + std::string world_frame) + : transformer_(std::move(transformer)), + world_frame_(std::move(world_frame)) {} + + const std::string& worldFrame() const { return world_frame_; } + + std::optional lookup( + const std_msgs::Header& header) const { + if (header.frame_id.empty()) { + ROS_WARN_THROTTLE(2.0, + "Cannot transform layered observations with an empty " + "source frame."); + return std::nullopt; + } + if (TfTransformer::sanitizeFrameId(header.frame_id) == + TfTransformer::sanitizeFrameId(world_frame_)) { + return Transformation3D(); + } + const auto transform = transformer_->lookupTransform( + world_frame_, header.frame_id, header.stamp); + if (!transform) { + ROS_WARN_STREAM_THROTTLE( + 2.0, "No transform from '" << header.frame_id << "' to '" + << world_frame_ << "' at " + << header.stamp << "; skipping layered " + "sensor observations."); + } + return transform; + } + + std::optional transformPosition( + const Point3D& sensor_position, + const std_msgs::Header& header) const { + const auto transform = lookup(header); + if (!transform) { + return std::nullopt; + } + return *transform * sensor_position; + } + + template + bool transformBatch(ObservationBatchT& batch, + const std_msgs::Header& header) const { + const auto transform = lookup(header); + if (!transform) { + return false; + } + batch.transformPositions(*transform); + return true; + } + + private: + std::shared_ptr transformer_; + std::string world_frame_; +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_INPUTS_SENSOR_FRAME_TRANSFORMER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h new file mode 100644 index 000000000..f0108bdce --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_hashed_wavelet_integrator_factory.h @@ -0,0 +1,98 @@ +#ifndef WAVEMAP_ROS_LAYERED_HASHED_WAVELET_INTEGRATOR_FACTORY_H_ +#define WAVEMAP_ROS_LAYERED_HASHED_WAVELET_INTEGRATOR_FACTORY_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap { + +// Typed counterpart of the original runtime IntegratorFactory boundary. The +// original factory remains unchanged for occupancy-only maps; layered servers +// use this path because their voxel type is known at compile time. +namespace detail { +template +std::unique_ptr createLayeredWaveletIntegratorImpl( + const param::Value& params, + typename MapT::Ptr occupancy_map, IntegratorType expected_type, + const char* expected_type_name, + std::shared_ptr thread_pool) { + const auto type = IntegratorType::from(params, "integration_method"); + if (!type || type.value() != expected_type) { + LOG(ERROR) << "This layered continuous-map backend requires " + "integration_method type " + << expected_type_name << "."; + return nullptr; + } + + const auto integrator_config = + ProjectiveIntegratorConfig::from(params, "integration_method"); + if (!integrator_config) { + LOG(ERROR) << "Layered integrator config could not be loaded."; + return nullptr; + } + + std::shared_ptr projection_model = + ProjectorFactory::create(params); + if (!projection_model) { + LOG(ERROR) << "Layered integrator projection model could not be created."; + return nullptr; + } + + auto posed_range_image = + std::make_shared>(projection_model->getDimensions()); + auto beam_offset_image = + std::make_shared>(projection_model->getDimensions()); + auto measurement_model = MeasurementModelFactory::create( + params, projection_model, posed_range_image, beam_offset_image); + if (!measurement_model) { + LOG(ERROR) << "Layered integrator measurement model could not be created."; + return nullptr; + } + + return std::make_unique( + integrator_config.value(), std::move(projection_model), + std::move(posed_range_image), std::move(beam_offset_image), + std::move(measurement_model), std::move(occupancy_map), + std::move(thread_pool)); +} +} // namespace detail + +template +std::unique_ptr createLayeredWaveletIntegrator( + const param::Value& params, + typename HashedWaveletOctreeT::Ptr occupancy_map, + std::shared_ptr thread_pool) { + return detail::createLayeredWaveletIntegratorImpl< + HashedWaveletIntegratorT, + HashedWaveletOctreeT>( + params, std::move(occupancy_map), + IntegratorType::kHashedWaveletIntegrator, + "hashed_wavelet_integrator", std::move(thread_pool)); +} + +template +std::unique_ptr createLayeredWaveletIntegrator( + const param::Value& params, + typename HashedChunkedWaveletOctreeT::Ptr occupancy_map, + std::shared_ptr thread_pool) { + return detail::createLayeredWaveletIntegratorImpl< + HashedChunkedWaveletIntegratorT, + HashedChunkedWaveletOctreeT>( + params, std::move(occupancy_map), + IntegratorType::kHashedChunkedWaveletIntegrator, + "hashed_chunked_wavelet_integrator", std::move(thread_pool)); +} + +} // namespace wavemap + +#endif // WAVEMAP_ROS_LAYERED_HASHED_WAVELET_INTEGRATOR_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h new file mode 100644 index 000000000..31d9382e1 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_ros_server_extension.h @@ -0,0 +1,324 @@ +#ifndef WAVEMAP_ROS_LAYERED_ROS_SERVER_EXTENSION_H_ +#define WAVEMAP_ROS_LAYERED_ROS_SERVER_EXTENSION_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap { +class LayeredRosServerExtensionBase { + public: + virtual ~LayeredRosServerExtensionBase() = default; + + virtual bool saveLayeredMap(const std::filesystem::path& file_path) = 0; + virtual bool loadLayeredMap(const std::filesystem::path& file_path, + std::string* error_message = nullptr) = 0; + virtual bool publishLayeredMap(const ros::Time& stamp = ros::Time::now()) = 0; + virtual MapBase::Ptr getContinuousMap() = 0; + virtual void clearLayeredMap() = 0; +}; + +template < + typename LayeredMapT, typename LayeredMapIoT, typename RosConverterT, + typename UpdateConverterT = + convert::DescriptorLayerObservationRosConverter> +class LayeredRosServerExtension : public LayeredRosServerExtensionBase { + public: + LayeredRosServerExtension(ros::NodeHandle nh_private, std::string world_frame, + LayeredMapT layered_map, + std::string layered_map_topic = "layered_map", + std::string update_topic = "layered_map_updates") + : nh_private_(std::move(nh_private)), + world_frame_(std::move(world_frame)), + layered_map_(std::move(layered_map)), + layered_pipeline_(layered_map_) { + layered_map_pub_ = nh_private_.advertise( + layered_map_topic, kQueueSize, kLatchPublisher); + layered_update_sub_ = nh_private_.subscribe( + update_topic, kQueueSize, + &LayeredRosServerExtension::layeredUpdateCallback, this); + advertiseServices(); + } + + LayeredMapT& getLayeredMap() { return layered_map_; } + const LayeredMapT& getLayeredMap() const { return layered_map_; } + MapBase::Ptr getContinuousMap() override { + return layered_map_.continuousMapPtr(); + } + void clearLayeredMap() override { + std::scoped_lock lock(mutex_); + layered_map_.continuousMap().clear(); + layered::forEachLayerDescriptor( + layered_map_.discreteLayers(), + [](const auto& /*descriptor*/, auto& layer) { layer.clear(); }); + } + + template + bool integrateEndpointLayer(const PosedPointcloud<>& pointcloud, + const std::vector< + layered::LayerValueT>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integratePointcloudLayer< + LayerTagT, + layered::PointcloudLayerIntegrationMode::kEndpointOnly>( + pointcloud, values, endpoint_range); + } + + template + bool integratePointcloudLayer( + const PosedPointcloud<>& pointcloud, + const std::vector>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integratePointcloudLayerImpl( + pointcloud, values.size(), + [&values](size_t index) + -> std::optional> { + return values[index]; + }, + endpoint_range); + } + + template + bool integrateOptionalEndpointLayer( + const PosedPointcloud<>& pointcloud, + const std::vector>>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integrateOptionalPointcloudLayer< + LayerTagT, + layered::PointcloudLayerIntegrationMode::kEndpointOnly>( + pointcloud, values, endpoint_range); + } + + template + bool integrateOptionalPointcloudLayer( + const PosedPointcloud<>& pointcloud, + const std::vector< + std::optional>>& values, + const layered::EndpointRange& endpoint_range = {}) { + return integratePointcloudLayerImpl( + pointcloud, values.size(), + [&values](size_t index) { return values[index]; }, endpoint_range); + } + + template + layered::LayerIntegrationResult integrateObservations( + const std::vector>>& observations) { + std::scoped_lock lock(mutex_); + return layered_pipeline_.template integrate(observations); + } + + template + layered::LayerIntegrationResult integrateEndpointObservations( + const std::vector>>& observations) { + return integrateObservations(observations); + } + + template + layered::LayerIntegrationResult integrateObservationBatch( + const ObservationBatchT& batch) { + std::scoped_lock lock(mutex_); + return layered_pipeline_.integrateBatch(batch); + } + + bool saveLayeredMap(const std::filesystem::path& file_path) override { + std::scoped_lock lock(mutex_); + // Match the original save_map behavior: persist a fully thresholded + // snapshot, including every typed continuous layer. + layered_map_.continuousMap().threshold(); + return LayeredMapIoT::save(file_path, layered_map_); + } + + bool loadLayeredMap(const std::filesystem::path& file_path, + std::string* error_message = nullptr) override { + { + std::scoped_lock lock(mutex_); + // Load into the existing object so the core Pipeline keeps owning the + // exact same continuous octree. + if (!LayeredMapIoT::load(file_path, layered_map_, error_message)) { + return false; + } + } + return publishLayeredMap(); + } + + bool publishLayeredMap(const ros::Time& stamp = ros::Time::now()) override { + wavemap_msgs::LayeredMap msg; + { + std::scoped_lock lock(mutex_); + if (!convert::layeredMapToRosMsg( + layered_map_, world_frame_, stamp, msg)) { + return false; + } + convert::clearDiscreteLayerBundleDirtyState( + layered_map_.discreteLayers()); + } + + layered_map_pub_.publish(msg); + return true; + } + + private: + template + bool integratePointcloudLayerImpl( + const PosedPointcloud<>& pointcloud, size_t value_count, + ValueAccessorT&& value_at, + const layered::EndpointRange& endpoint_range) { + if (value_count != pointcloud.size()) { + return false; + } + if constexpr (!layered::kIsWaveletCompatibleLayer) { + static_assert( + IntegrationMode == + layered::PointcloudLayerIntegrationMode::kEndpointOnly, + "Discrete point fields can only describe occupied endpoints. Use " + "positioned observations for other integration semantics."); + using Value = layered::LayerValueT; + std::vector> observations; + observations.reserve(pointcloud.size()); + for (size_t point_index = 0u; point_index < pointcloud.size(); + ++point_index) { + const auto value = value_at(point_index); + if (!value) { + continue; + } + const Point3D C_endpoint = pointcloud[point_index]; + if (!endpoint_range.contains(C_endpoint)) { + continue; + } + observations.emplace_back(pointcloud.getPose() * C_endpoint, *value); + } + std::scoped_lock lock(mutex_); + const auto result = + layered_pipeline_.template integrate(observations); + return result.rejected == 0u; + } else { + constexpr auto descriptor = layered::detail::descriptorForLayer< + LayerTagT, typename LayeredMapT::ContinuousLayers>(); + const FloatingPoint cell_width = + layered_map_.continuousMap().getMinCellWidth(); + const Point3D& W_origin = pointcloud.getOrigin(); + std::scoped_lock lock(mutex_); + for (size_t point_index = 0u; point_index < pointcloud.size(); + ++point_index) { + const auto value = value_at(point_index); + if (!value) { + continue; + } + const Point3D C_endpoint = pointcloud[point_index]; + if (!endpoint_range.contains(C_endpoint)) { + continue; + } + const Point3D W_endpoint = pointcloud.getPose() * C_endpoint; + const layered::LayerObservation> + observation(W_endpoint, *value); + layered::forEachPointcloudIntegrationIndex( + W_origin, W_endpoint, cell_width, + [&](const Index3D& voxel_index) { + layered::updateContinuousLayersAtVoxel( + layered_map_.continuousMap(), voxel_index, + [&](auto& layers) { + auto& current = descriptor.value(layers); + current = layered::LayerUpdatePolicyT{}( + current, observation); + }); + }); + } + return true; + } + } + + void layeredUpdateCallback( + const wavemap_msgs::LayeredMapUpdate::ConstPtr& msg) { + if (msg->header.frame_id != world_frame_) { + ROS_WARN_STREAM("Ignoring layered map update in frame '" + << msg->header.frame_id << "'; expected '" + << world_frame_ << "'."); + return; + } + + std::string error_message; + { + std::scoped_lock lock(mutex_); + if (!UpdateConverterT::integrate( + *msg, layered_pipeline_, &error_message)) { + ROS_WARN_STREAM("Ignoring invalid layered map update: " + << error_message); + return; + } + } + // The periodic layered publish operation consumes the continuous block + // timestamps and discrete dirty-parent state in one coherent update. + } + + void advertiseServices() { + save_layered_map_srv_ = + nh_private_.advertiseService( + "save_layered_map", [this](auto& request, auto& response) { + response.success = saveLayeredMap(request.file_path); + return true; + }); + + load_layered_map_srv_ = + nh_private_.advertiseService( + "load_layered_map", [this](auto& request, auto& response) { + std::string error_message; + response.success = loadLayeredMap(request.file_path, &error_message); + if (!response.success && !error_message.empty()) { + ROS_ERROR_STREAM(error_message); + } + return true; + }); + + request_full_layered_map_srv_ = + nh_private_.advertiseService( + "layered_map_request_full", [this](auto&, auto&) { + return publishLayeredMap(); + }); + } + + static constexpr int kQueueSize = 1; + static constexpr bool kLatchPublisher = true; + + ros::NodeHandle nh_private_; + std::string world_frame_; + + mutable std::mutex mutex_; + LayeredMapT layered_map_; + layered::LayeredPipeline layered_pipeline_; + + ros::Publisher layered_map_pub_; + ros::Subscriber layered_update_sub_; + ros::ServiceServer load_layered_map_srv_; + ros::ServiceServer save_layered_map_srv_; + ros::ServiceServer request_full_layered_map_srv_; +}; +} // namespace wavemap + +#endif // WAVEMAP_ROS_LAYERED_ROS_SERVER_EXTENSION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h new file mode 100644 index 000000000..2e93dbb2e --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/layered_server_builder.h @@ -0,0 +1,72 @@ +#ifndef WAVEMAP_ROS_LAYERED_SERVER_BUILDER_H_ +#define WAVEMAP_ROS_LAYERED_SERVER_BUILDER_H_ + +#include +#include +#include + +#include + +#include "wavemap_ros/layered_hashed_wavelet_integrator_factory.h" +#include "wavemap_ros/layered_ros_server_extension.h" +#include "wavemap_ros/map_operations/layered_publish_map_operation.h" +#include "wavemap_ros/ros_server.h" + +namespace wavemap { + +// Installs one compile-time LayeredMap definition into the existing ROS server +// lifecycle. The original occupancy-only construction remains untouched. +// SensorBindingsT receives (RosServer&, TypedExtension&) and only registers +// how sensor data populates the schema's layers. +template +class LayeredRosServerBuilder { + public: + using Definition = MapDefinitionT; + using Map = typename Definition::Map; + using Config = typename Definition::Config; + using MapIo = typename Definition::MapIo; + using Voxel = typename Definition::Voxel; + using ObservationBatch = + layered::LayerObservationBatch; + using Extension = LayeredRosServerExtension; + + template + static Extension& install(RosServer& server, ros::NodeHandle nh_private, + std::string world_frame, Config config, + SensorBindingsT&& sensor_bindings) { + auto extension = std::make_unique( + nh_private, world_frame, Map(config)); + auto* const extension_ptr = extension.get(); + + std::forward(sensor_bindings)(server, *extension_ptr); + + server.layered_extension_ = std::move(extension); + server.occupancy_map_ = server.layered_extension_->getContinuousMap(); + + auto continuous_map = extension_ptr->getLayeredMap().continuousMapPtr(); + server.layered_integrator_factory_ = + [continuous_map](const param::Value& params, + std::shared_ptr thread_pool) { + return createLayeredWaveletIntegrator( + params, continuous_map, std::move(thread_pool)); + }; + + server.layered_publish_operation_factory_ = + [&server, extension_ptr, world_frame = std::move(world_frame)]( + const PublishMapOperationConfig& publish_config, + ros::NodeHandle operation_nh_private) { + using Operation = + UnifiedLayeredPublishMapOperation; + return std::make_unique( + publish_config, &extension_ptr->getLayeredMap(), + server.thread_pool_, world_frame, + std::move(operation_nh_private)); + }; + + return *extension_ptr; + } +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_LAYERED_SERVER_BUILDER_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h new file mode 100644 index 000000000..507864523 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/layered_publish_map_operation.h @@ -0,0 +1,212 @@ +#ifndef WAVEMAP_ROS_MAP_OPERATIONS_LAYERED_PUBLISH_MAP_OPERATION_H_ +#define WAVEMAP_ROS_MAP_OPERATIONS_LAYERED_PUBLISH_MAP_OPERATION_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap_ros/map_operations/publish_map_operation.h" + +namespace wavemap { + +// Typed equivalent of PublishMapOperation for HashedWaveletOctreeT. +// It uses the same topic, batching, timestamps, full-map request service, and +// deletion semantics as the original occupancy-only publisher. +template +class LayeredPublishMapOperation final : public MapOperationBase { + public: + using Map = HashedWaveletOctreeT; + + LayeredPublishMapOperation( + const PublishMapOperationConfig& config, typename Map::Ptr map, + std::shared_ptr thread_pool, std::string world_frame, + ros::NodeHandle nh_private) + : MapOperationBase(map), + config_(config.checkValid()), + map_(std::move(map)), + thread_pool_(std::move(thread_pool)), + world_frame_(std::move(world_frame)) { + map_pub_ = nh_private.advertise(config_.topic, 10); + republish_whole_map_srv_ = + nh_private.advertiseService( + config_.topic + "_request_full", [this](auto&, auto&) { + publishMap(ros::Time::now(), true); + return true; + }); + } + + void run(bool force_run) override { + const ros::Time current_time = ros::Time::now(); + if (force_run || + config_.once_every < (current_time - last_run_timestamp_).toSec()) { + publishMap(current_time, false); + last_run_timestamp_ = current_time; + } + } + + private: + void publishMap(const ros::Time& current_time, bool republish_whole_map) { + ProfilerZoneScoped; + if (map_->empty()) { + return; + } + + const Timestamp selection_start = Time::now(); + std::unordered_set changed_blocks; + map_->forEachBlock( + [&changed_blocks, republish_whole_map, + last_published = last_run_timestamp_internal_]( + const Index3D& block_index, const auto& block) { + if (republish_whole_map || + last_published < block.getLastUpdatedStamp()) { + changed_blocks.emplace(block_index); + } + }); + last_run_timestamp_internal_ = selection_start; + + while (!changed_blocks.empty()) { + std::unordered_set blocks_to_publish; + for (const auto& block_index : changed_blocks) { + if (auto* block = map_->getBlock(block_index); block) { + block->threshold(); + blocks_to_publish.emplace(block_index); + if (config_.max_num_blocks_per_msg <= + static_cast(blocks_to_publish.size())) { + break; + } + } + } + + wavemap_msgs::Map map_msg; + convert::mapToRosMsg( + *map_, world_frame_, current_time, map_msg, blocks_to_publish, + thread_pool_); + map_pub_.publish(map_msg); + + for (const auto& block_index : blocks_to_publish) { + changed_blocks.erase(block_index); + } + } + } + + const PublishMapOperationConfig config_; + const typename Map::Ptr map_; + const std::shared_ptr thread_pool_; + const std::string world_frame_; + ros::Time last_run_timestamp_; + Timestamp last_run_timestamp_internal_; + ros::Publisher map_pub_; + ros::ServiceServer republish_whole_map_srv_; +}; + +// Unified publisher for a complete LayeredMap. Occupancy and other continuous +// fields use Wavemap's block timestamps, while discrete layers use dirty +// compressed-parent tracking. Continuous-only schemas pay only one empty tuple +// traversal per publication cycle. +template +class UnifiedLayeredPublishMapOperation final : public MapOperationBase { + public: + using ContinuousMap = typename LayeredMapT::ContinuousMap; + + UnifiedLayeredPublishMapOperation( + const PublishMapOperationConfig& config, + LayeredMapT* layered_map, + std::shared_ptr thread_pool, std::string world_frame, + ros::NodeHandle nh_private) + : MapOperationBase(CHECK_NOTNULL(layered_map)->continuousMapPtr()), + config_(config.checkValid()), + layered_map_(std::move(layered_map)), + thread_pool_(std::move(thread_pool)), + world_frame_(std::move(world_frame)) { + map_pub_ = nh_private.advertise(config_.topic, 10); + } + + void run(bool force_run) override { + const ros::Time current_time = ros::Time::now(); + if (force_run || + config_.once_every < (current_time - last_run_timestamp_).toSec()) { + publishIncrement(current_time); + last_run_timestamp_ = current_time; + } + } + + private: + void publishIncrement(const ros::Time& stamp) { + const Timestamp selection_start = Time::now(); + std::unordered_set changed_blocks; + layered_map_->continuousMap().forEachBlock( + [&changed_blocks, last_published = last_run_timestamp_internal_]( + const Index3D& block_index, const auto& block) { + if (last_published < block.getLastUpdatedStamp()) { + changed_blocks.emplace(block_index); + } + }); + const bool has_discrete_changes = + convert::discreteLayerBundleHasDirtyState( + layered_map_->discreteLayers()); + last_run_timestamp_internal_ = selection_start; + if (changed_blocks.empty() && !has_discrete_changes) { + return; + } + + bool discrete_patch_pending = has_discrete_changes; + do { + std::unordered_set blocks_to_publish; + for (const auto& block_index : changed_blocks) { + if (auto* block = + layered_map_->continuousMap().getBlock(block_index); + block) { + block->threshold(); + blocks_to_publish.emplace(block_index); + if (config_.max_num_blocks_per_msg <= + static_cast(blocks_to_publish.size())) { + break; + } + } + } + + wavemap_msgs::LayeredMap msg; + convert::layeredMapPatchToRosMsg( + *layered_map_, world_frame_, stamp, msg, blocks_to_publish, + thread_pool_); + if (!discrete_patch_pending) { + msg.discrete_layers.clear(); + } + map_pub_.publish(msg); + discrete_patch_pending = false; + for (const auto& block_index : blocks_to_publish) { + changed_blocks.erase(block_index); + } + } while (!changed_blocks.empty()); + + convert::clearDiscreteLayerBundleDirtyState( + layered_map_->discreteLayers()); + } + + const PublishMapOperationConfig config_; + LayeredMapT* const layered_map_; + const std::shared_ptr thread_pool_; + const std::string world_frame_; + ros::Time last_run_timestamp_; + Timestamp last_run_timestamp_internal_; + ros::Publisher map_pub_; +}; + +} // namespace wavemap + +#endif // WAVEMAP_ROS_MAP_OPERATIONS_LAYERED_PUBLISH_MAP_OPERATION_H_ diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h index 801f53138..a1d0e6121 100644 --- a/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h +++ b/interfaces/ros1/wavemap_ros/include/wavemap_ros/ros_server.h @@ -2,8 +2,11 @@ #define WAVEMAP_ROS_ROS_SERVER_H_ #include +#include #include +#include #include +#include #include #include @@ -15,13 +18,21 @@ #include #include #include +#include #include #include "wavemap_ros/inputs/ros_input_base.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" +#include "wavemap_ros/inputs/sensor_frame_transformer.h" +#include "wavemap_ros/layered_ros_server_extension.h" +#include "wavemap_ros/map_operations/publish_map_operation.h" #include "wavemap_ros/utils/ros_logging_level.h" #include "wavemap_ros/utils/tf_transformer.h" namespace wavemap { +template +class LayeredRosServerBuilder; + /** * Config struct for wavemap's ROS server. */ @@ -45,9 +56,16 @@ struct RosServerConfig : ConfigBase { class RosServer { public: + using MapInstaller = + std::function; + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private); + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + MapInstaller map_installer); RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const RosServerConfig& config); + RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + const RosServerConfig& config, MapInstaller map_installer); void clear(); @@ -56,6 +74,11 @@ class RosServer { Pipeline& getPipeline() { return *pipeline_; } const Pipeline& getPipeline() const { return *pipeline_; } + const std::string& worldFrame() const { return config_.world_frame; } + + SensorFrameTransformer makeSensorFrameTransformer() const { + return SensorFrameTransformer(transformer_, config_.world_frame); + } MapOperationBase* addOperation(const param::Value& operation_params, ros::NodeHandle nh_private); @@ -63,6 +86,142 @@ class RosServer { RosInputBase* addInput(const param::Value& integrator_params, const ros::NodeHandle& nh, ros::NodeHandle nh_private); RosInputBase* addInput(std::unique_ptr input); + using PointcloudEndpointAdapterRegistrar = + std::function; + void addPointcloudEndpointAdapterRegistrar( + PointcloudEndpointAdapterRegistrar registrar); + template + void bindPointField( + LayeredExtensionT& extension, + PointFieldBinding> binding = {}, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_arithmetic_v, + "bindPointField supports numeric scalar layers. Use a " + "custom PointcloudEndpointAdapter for structured values."); + if (binding.field_name.empty()) { + binding.field_name = + std::string(layered::LayerTraits::name); + } + addPointcloudEndpointAdapterRegistrar( + [&extension, binding = std::move(binding), endpoint_range]( + PointcloudTopicInput& input) { + using Adapter = NumericPointcloudEndpointAdapter; + input.addEndpointAdapter(std::make_shared( + binding.field_name, binding.scale, binding.offset, + binding.min_value, binding.max_value, + [&extension, endpoint_range](const PosedPointcloud<>& pointcloud, + const std::vector& values) { + extension.template integratePointcloudLayer( + pointcloud, values, endpoint_range); + })); + }); + } + template + void bindNormalizedPointField( + LayeredExtensionT& extension, std::string field_name, + FloatingPoint raw_min, FloatingPoint raw_max, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_arithmetic_v, + "Normalized point fields require a numeric scalar layer."); + bindPointField( + extension, + PointFieldBinding::normalized(std::move(field_name), raw_min, + raw_max), + endpoint_range); + } + template + void bindDirectPointField( + LayeredExtensionT& extension, std::string field_name = {}, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + static_assert(std::is_arithmetic_v, + "Direct point fields require a numeric scalar layer."); + bindPointField( + extension, PointFieldBinding(std::move(field_name)), + endpoint_range); + } + template + void bindComputedPointField( + LayeredExtensionT& extension, + std::vector source_field_names, ConverterT converter, + layered::EndpointRange endpoint_range = {}) { + using Value = layered::LayerValueT; + using FieldValues = NumericPointFieldValues; + using OptionalValue = std::optional; + static_assert( + std::is_invocable_r_v, + "A computed point-field converter must return optional " + "and accept const NumericPointFieldValues&."); + addPointcloudEndpointAdapterRegistrar( + [&extension, source_field_names = std::move(source_field_names), + converter = std::move(converter), endpoint_range]( + PointcloudTopicInput& input) { + using Adapter = ComputedNumericPointcloudEndpointAdapter; + input.addEndpointAdapter(std::make_shared( + source_field_names, converter, + [&extension, endpoint_range]( + const PosedPointcloud<>& pointcloud, + const std::vector& values) { + extension.template integrateOptionalPointcloudLayer< + LayerTagT, IntegrationMode>( + pointcloud, values, endpoint_range); + })); + }); + } + template + void bindGeometricClass( + LayeredExtensionT& extension, + layered::LocalElevationClassifierConfig classifier_config = {}, + layered::EndpointRange endpoint_range = {}, + Radians min_azimuth = -kPi, + Radians max_azimuth = kPi) { + using Value = layered::LayerValueT; + static_assert(std::is_integral_v, + "Geometric class layers require an integral value type."); + addPointcloudEndpointAdapterRegistrar( + [&extension, classifier = layered::LocalElevationClassifier( + classifier_config), endpoint_range, min_azimuth, + max_azimuth]( + PointcloudTopicInput& input) { + input.addPosedCloudCallback( + [&extension, classifier, endpoint_range, min_azimuth, + max_azimuth](const PosedPointcloud<>& pointcloud) { + std::vector class_points; + class_points.reserve(pointcloud.size()); + for (const Point3D& point : pointcloud) { + const FloatingPoint azimuth = std::atan2(point.y(), point.x()); + if (min_azimuth <= azimuth && azimuth <= max_azimuth) { + class_points.emplace_back(point); + } + } + const PosedPointcloud<> class_pointcloud(pointcloud.getPose(), + class_points); + const auto observations = + layered::makeGeometricClassObservations( + class_pointcloud, classifier, endpoint_range); + const auto result = + extension.template integrateObservations< + LayerTagT>(observations); + return result.rejected == 0u; + }); + }); + } const std::vector>& getInputs() { return inputs_; } @@ -72,16 +231,32 @@ class RosServer { bool loadMap(const std::filesystem::path& file_path); private: + template + friend class LayeredRosServerBuilder; + const RosServerConfig config_; // Map data structure MapBase::Ptr occupancy_map_; + std::unique_ptr layered_extension_; + using LayeredIntegratorFactory = std::function( + const param::Value&, std::shared_ptr)>; + LayeredIntegratorFactory layered_integrator_factory_; + using LayeredPublishOperationFactory = + std::function( + const PublishMapOperationConfig&, ros::NodeHandle)>; + LayeredPublishOperationFactory layered_publish_operation_factory_; + std::vector + pointcloud_endpoint_adapter_registrars_; // Threadpool shared among all input handlers and operations std::shared_ptr thread_pool_; // Map management pipeline std::shared_ptr pipeline_; + param::Array map_operation_param_array_;// dt_architecture_changes + param::Map measurement_integrator_param_map_;// dt_architecture_changes + param::Array input_param_array_;// dt_architecture_changes // Measurement and pose inputs std::vector> inputs_; diff --git a/interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch b/interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch new file mode 100644 index 000000000..f7e464185 --- /dev/null +++ b/interfaces/ros1/wavemap_ros/launch/synthetic_rgb_layered_map.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + diff --git a/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch b/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch index 41f24cff7..89daec007 100644 --- a/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch +++ b/interfaces/ros1/wavemap_ros/launch/wavemap_server.launch @@ -11,6 +11,8 @@ doc="Automatically launch Rviz."/> + diff --git a/interfaces/ros1/wavemap_ros/package.xml b/interfaces/ros1/wavemap_ros/package.xml index 14fcf6d93..38d1cc545 100644 --- a/interfaces/ros1/wavemap_ros/package.xml +++ b/interfaces/ros1/wavemap_ros/package.xml @@ -20,6 +20,7 @@ rosbag cv_bridge image_transport + message_filters tf2_ros std_srvs diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc index bbd78edc6..fafc93052 100644 --- a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc +++ b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc @@ -1,6 +1,9 @@ #include "wavemap_ros/inputs/pointcloud_topic_input.h" +#include #include +#include +#include #include #include @@ -14,6 +17,7 @@ #include #include #include +#include namespace wavemap { DECLARE_CONFIG_MEMBERS(PointcloudTopicInputConfig, @@ -28,7 +32,8 @@ DECLARE_CONFIG_MEMBERS(PointcloudTopicInputConfig, (undistort_motion) (num_undistortion_interpolation_intervals_per_cloud) (projected_range_image_topic_name) - (undistorted_pointcloud_topic_name)); + (undistorted_pointcloud_topic_name) + (enable_benchmark_metrics)); bool PointcloudTopicInputConfig::isValid(bool verbose) const { bool all_valid = true; @@ -74,6 +79,12 @@ PointcloudTopicInput::PointcloudTopicInput( config_.undistorted_pointcloud_topic_name, config_.topic_queue_length); } + if (config_.enable_benchmark_metrics) { + benchmark_stats_srv_ = nh_private.advertiseService( + "benchmark_stats", &PointcloudTopicInput::benchmarkStatsCallback, + this); + benchmark_metrics_.integration_durations.reserve(10000u); + } } void PointcloudTopicInput::callback( @@ -110,6 +121,37 @@ void PointcloudTopicInput::callback( : config_.sensor_frame_id; undistortion::StampedPointcloud stamped_pointcloud{ stamp_nsec, std::move(sensor_frame_id), num_points}; + std::vector> endpoint_decoders; + endpoint_decoders.reserve(endpoint_adapters_.size()); + for (const auto& adapter : endpoint_adapters_) { + if (auto decoder = adapter->prepare(pointcloud_msg, num_points)) { + endpoint_decoders.emplace_back(std::move(decoder)); + } + } + + // Decode all configured endpoint fields in one shared traversal. The XYZ + // conversion below remains unchanged to preserve Wavemap's input behavior. + bool endpoint_decoding_valid = true; + for (size_t row = 0u; row < pointcloud_msg.height; ++row) { + for (size_t column = 0u; column < pointcloud_msg.width; ++column) { + const size_t point_offset = + row * pointcloud_msg.row_step + column * pointcloud_msg.point_step; + for (auto& decoder : endpoint_decoders) { + endpoint_decoding_valid &= + decoder->decodePoint(pointcloud_msg, point_offset); + } + } + } + std::vector> + endpoint_channels; + if (endpoint_decoding_valid) { + endpoint_channels.reserve(endpoint_decoders.size()); + for (auto& decoder : endpoint_decoders) { + endpoint_channels.emplace_back(decoder->finish()); + } + } else { + ROS_WARN("Skipping malformed pointcloud endpoint channels."); + } // Load the points with time information if undistortion is enabled bool loaded = false; @@ -121,8 +163,10 @@ void PointcloudTopicInput::callback( if (hasField(pointcloud_msg, "t")) { sensor_msgs::PointCloud2ConstIterator t_it(pointcloud_msg, "t"); - for (; pos_it != pos_it.end(); ++pos_it, ++t_it) { - stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], *t_it); + size_t source_index = 0u; + for (; pos_it != pos_it.end(); ++pos_it, ++t_it, ++source_index) { + stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], *t_it, + source_index); } loaded = true; } else { @@ -142,13 +186,17 @@ void PointcloudTopicInput::callback( // If undistortion is disabled or loading failed, only load positions if (!loaded) { - for (; pos_it != pos_it.end(); ++pos_it) { - stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], 0); + size_t source_index = 0u; + for (; pos_it != pos_it.end(); ++pos_it, ++source_index) { + stamped_pointcloud.emplace(pos_it[0], pos_it[1], pos_it[2], 0, + source_index); } } // Add it to the integration queue - pointcloud_queue_.emplace(std::move(stamped_pointcloud)); + pointcloud_queue_.push( + {std::move(stamped_pointcloud), std::move(endpoint_channels)}); + recordReceivedScan(num_points); } #ifdef LIVOX_AVAILABLE @@ -176,18 +224,111 @@ void PointcloudTopicInput::callback( } // Add it to the integration queue - pointcloud_queue_.emplace(std::move(stamped_pointcloud)); + pointcloud_queue_.push({std::move(stamped_pointcloud), {}}); + recordReceivedScan(pointcloud_msg.points.size()); } #endif +void PointcloudTopicInput::recordReceivedScan(size_t num_points) { + if (!config_.enable_benchmark_metrics) { + return; + } + const std::lock_guard lock(benchmark_metrics_mutex_); + ++benchmark_metrics_.received_scans; + ++benchmark_metrics_.pending_scans; + benchmark_metrics_.received_points += num_points; + benchmark_metrics_.maximum_queue_size = + std::max(benchmark_metrics_.maximum_queue_size, + benchmark_metrics_.pending_scans); +} + +void PointcloudTopicInput::recordDroppedScan() { + if (config_.enable_benchmark_metrics) { + const std::lock_guard lock(benchmark_metrics_mutex_); + ++benchmark_metrics_.dropped_scans; + --benchmark_metrics_.pending_scans; + } +} + +void PointcloudTopicInput::recordIntegratedScan( + size_t num_points, double duration_seconds) { + if (!config_.enable_benchmark_metrics) { + return; + } + const std::lock_guard lock(benchmark_metrics_mutex_); + ++benchmark_metrics_.integrated_scans; + --benchmark_metrics_.pending_scans; + benchmark_metrics_.integrated_points += num_points; + benchmark_metrics_.integration_durations.emplace_back(duration_seconds); +} + +bool PointcloudTopicInput::benchmarkStatsCallback( + std_srvs::Trigger::Request& /*request*/, + std_srvs::Trigger::Response& response) { + if (!config_.enable_benchmark_metrics) { + response.success = false; + response.message = "Benchmark metrics are disabled."; + return true; + } + + BenchmarkMetrics metrics; + { + const std::lock_guard lock(benchmark_metrics_mutex_); + metrics = benchmark_metrics_; + } + std::vector sorted = metrics.integration_durations; + std::sort(sorted.begin(), sorted.end()); + const auto percentile = [&sorted](double fraction) { + if (sorted.empty()) { + return 0.0; + } + const double position = fraction * static_cast(sorted.size() - 1u); + const size_t lower = static_cast(position); + const size_t upper = std::min(lower + 1u, sorted.size() - 1u); + const double weight = position - static_cast(lower); + return sorted[lower] * (1.0 - weight) + sorted[upper] * weight; + }; + const double total = + std::accumulate(sorted.begin(), sorted.end(), 0.0); + const double mean = sorted.empty() ? 0.0 : total / sorted.size(); + + std::ostringstream json; + json.precision(10); + json << "{" + << "\"received_scans\":" << metrics.received_scans << "," + << "\"integrated_scans\":" << metrics.integrated_scans + << "," + << "\"dropped_scans\":" << metrics.dropped_scans << "," + << "\"pending_scans\":" << metrics.pending_scans << "," + << "\"received_points\":" << metrics.received_points << "," + << "\"integrated_points\":" << metrics.integrated_points + << "," + << "\"maximum_queue_size\":" + << metrics.maximum_queue_size << "," + << "\"integration_time_s\":{" + << "\"total\":" << total << "," + << "\"min\":" << (sorted.empty() ? 0.0 : sorted.front()) << "," + << "\"mean\":" << mean << "," + << "\"median\":" << percentile(0.5) << "," + << "\"p95\":" << percentile(0.95) << "," + << "\"max\":" << (sorted.empty() ? 0.0 : sorted.back()) + << "}}"; + + response.success = true; + response.message = json.str(); + return true; +} + void PointcloudTopicInput::processQueue() { ProfilerZoneScoped; while (!pointcloud_queue_.empty()) { - auto& oldest_msg = pointcloud_queue_.front(); + auto& queued_cloud = pointcloud_queue_.front(); + auto& oldest_msg = queued_cloud.pointcloud; // Drop messages if they're older than max_wait_for_pose if (config_.max_wait_for_pose < - convert::nanoSecondsToSeconds(pointcloud_queue_.back().getEndTime() - + convert::nanoSecondsToSeconds( + pointcloud_queue_.back().pointcloud.getEndTime() - oldest_msg.getStartTime())) { ROS_WARN_STREAM( "Max waiting time of " @@ -196,8 +337,10 @@ void PointcloudTopicInput::processQueue() { << oldest_msg.getSensorFrame() << "\" and time interval [" << oldest_msg.getStartTime() << ", " << oldest_msg.getEndTime() << "] vs newest cloud end time " - << pointcloud_queue_.back().getEndTime() << ". Dropping cloud."); + << pointcloud_queue_.back().pointcloud.getEndTime() + << ". Dropping cloud."); pointcloud_queue_.pop(); + recordDroppedScan(); continue; } @@ -232,6 +375,7 @@ void PointcloudTopicInput::processQueue() { } pointcloud_queue_.pop(); + recordDroppedScan(); continue; } } else { @@ -260,9 +404,25 @@ void PointcloudTopicInput::processQueue() { << " points. Remaining pointclouds in queue: " << pointcloud_queue_.size() - 1 << "."); integration_timer_.start(); - pipeline_->runPipeline(config_.measurement_integrator_names, - posed_pointcloud); + pipeline_->runIntegrators(config_.measurement_integrator_names, + posed_pointcloud); + for (const auto& endpoint_channel : queued_cloud.endpoint_channels) { + if (!endpoint_channel->integrate(posed_pointcloud, + oldest_msg.getPoints())) { + ROS_WARN("Skipping malformed pointcloud endpoint channel."); + } + } + for (const auto& callback : posed_cloud_callbacks_) { + if (!callback(posed_pointcloud)) { + ROS_WARN("A derived endpoint layer rejected the posed pointcloud."); + } + } + // Publish/prune only after occupancy and endpoint fields from this + // measurement have both been committed. + pipeline_->runOperations(); integration_timer_.stop(); + recordIntegratedScan(posed_pointcloud.size(), + integration_timer_.getLastEpisodeDuration()); ROS_DEBUG_STREAM("Integrated new pointcloud in " << integration_timer_.getLastEpisodeDuration() << "s. Total integration time: " diff --git a/interfaces/ros1/wavemap_ros/src/ros_server.cc b/interfaces/ros1/wavemap_ros/src/ros_server.cc index 3d94171a4..e8f475205 100644 --- a/interfaces/ros1/wavemap_ros/src/ros_server.cc +++ b/interfaces/ros1/wavemap_ros/src/ros_server.cc @@ -11,6 +11,8 @@ #include #include "wavemap_ros/inputs/ros_input_factory.h" +#include "wavemap_ros/inputs/pointcloud_topic_input.h" +#include "wavemap_ros/layered_server_builder.h" #include "wavemap_ros/map_operations/map_ros_operation_factory.h" namespace wavemap { @@ -36,21 +38,41 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private) : RosServer(nh, nh_private, RosServerConfig::from( param::convert::toParamValue(nh_private, "general")) - .value()) {} + .value(), + {}) {} + +RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + MapInstaller map_installer) + : RosServer(nh, nh_private, + RosServerConfig::from( + param::convert::toParamValue(nh_private, "general")) + .value(), + std::move(map_installer)) {} RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, const RosServerConfig& config) + : RosServer(nh, nh_private, config, {}) {} + +RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, + const RosServerConfig& config, + MapInstaller map_installer) : config_(config.checkValid()), transformer_(std::make_shared()) { // Set the logging level for wavemap's C++ library (uses glog) and ROS config_.logging_level.applyToGlog(); config_.logging_level.applyToRosConsole(); - // Setup data structure - const auto data_structure_params = - param::convert::toParamValue(nh_private, "map"); - occupancy_map_ = - MapFactory::create(data_structure_params, MapType::kHashedBlocks); + // A typed application may install one layered map before the original + // pipeline is created. Without an installer this is the unchanged original + // occupancy-only server construction path. + if (map_installer) { + map_installer(*this, nh_private); + } else { + const auto data_structure_params = + param::convert::toParamValue(nh_private, "map"); + occupancy_map_ = + MapFactory::create(data_structure_params, MapType::kHashedBlocks); + } CHECK_NOTNULL(occupancy_map_); // Setup thread pool @@ -64,29 +86,57 @@ RosServer::RosServer(ros::NodeHandle nh, ros::NodeHandle nh_private, CHECK_NOTNULL(pipeline_); // Add map operations to pipeline + /* dt_architecture_changes const param::Array map_operation_param_array = param::convert::toParamArray(nh_private, "map_operations"); for (const auto& operation_params : map_operation_param_array) { addOperation(operation_params, nh_private); + }*/ + // dt_architecture_changes + map_operation_param_array_ = + param::convert::toParamArray(nh_private, "map_operations"); + for (const auto& operation_params : map_operation_param_array_) { + addOperation(operation_params, nh_private); } // Add measurement integrators to pipeline + /* dt_architecture_changes const param::Map measurement_integrator_param_map = param::convert::toParamMap(nh_private, "measurement_integrators"); for (const auto& [integrator_name, integrator_params] : measurement_integrator_param_map) { pipeline_->addIntegrator(integrator_name, integrator_params); + }*/ + measurement_integrator_param_map_ = + param::convert::toParamMap(nh_private, "measurement_integrators"); + for (const auto& [integrator_name, integrator_params] : + measurement_integrator_param_map_) { + if (layered_integrator_factory_) { + pipeline_->addIntegrator( + integrator_name, + layered_integrator_factory_(integrator_params, thread_pool_)); + } else { + pipeline_->addIntegrator(integrator_name, integrator_params); + } } + // Setup measurement inputs + /* dt_architecture_changes const param::Array input_param_array = param::convert::toParamArray(nh_private, "inputs"); for (const auto& integrator_params : input_param_array) { addInput(integrator_params, nh, nh_private); + }*/ + input_param_array_ = + param::convert::toParamArray(nh_private, "inputs"); + for (const auto& integrator_params : input_param_array_) { + addInput(integrator_params, nh, nh_private); } // Connect to ROS advertiseServices(nh_private); + } void RosServer::clear() { @@ -105,9 +155,26 @@ RosInputBase* RosServer::addInput(const param::Value& integrator_params, auto input = RosInputFactory::create(integrator_params, pipeline_, transformer_, config_.world_frame, nh, nh_private); + if (auto* pointcloud_input = + dynamic_cast(input.get())) { + for (const auto& registrar : pointcloud_endpoint_adapter_registrars_) { + registrar(*pointcloud_input); + } + } return addInput(std::move(input)); } +void RosServer::addPointcloudEndpointAdapterRegistrar( + PointcloudEndpointAdapterRegistrar registrar) { + for (const auto& input : inputs_) { + if (auto* pointcloud_input = + dynamic_cast(input.get())) { + registrar(*pointcloud_input); + } + } + pointcloud_endpoint_adapter_registrars_.emplace_back(std::move(registrar)); +} + RosInputBase* RosServer::addInput(std::unique_ptr input) { if (input) { return inputs_.emplace_back(std::move(input)).get(); @@ -133,6 +200,17 @@ MapOperationBase* RosServer::addOperation(const param::Value& operation_params, if (const auto type = MapRosOperationType{type_name.value()}; type.isValid()) { + if (type == MapRosOperationType::kPublishMap && + layered_publish_operation_factory_) { + const auto publish_config = + PublishMapOperationConfig::from(operation_params); + if (!publish_config) { + ROS_ERROR("Could not load layered publish_map configuration."); + return nullptr; + } + return pipeline_->addOperation(layered_publish_operation_factory_( + publish_config.value(), std::move(nh_private))); + } auto operation = MapRosOperationFactory::create( type, operation_params, occupancy_map_, thread_pool_, transformer_, config_.world_frame, nh_private); @@ -164,9 +242,42 @@ bool RosServer::saveMap(const std::filesystem::path& file_path) const { return false; } +/*dt_architecture_changes bool RosServer::loadMap(const std::filesystem::path& file_path) { return io::fileToMap(file_path, occupancy_map_); } +*/ +//dt_architecture_changes +bool RosServer::loadMap(const std::filesystem::path& file_path) { + MapBase::Ptr loaded_map; + if (!io::fileToMap(file_path, loaded_map)) { + return false; + } + + occupancy_map_ = loaded_map; + + clearInputs(); + pipeline_.reset(); + pipeline_ = std::make_shared(occupancy_map_, thread_pool_); + + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + for (const auto& operation_params : map_operation_param_array_) { + addOperation(operation_params, nh_private); + } + + for (const auto& [integrator_name, integrator_params] : + measurement_integrator_param_map_) { + pipeline_->addIntegrator(integrator_name, integrator_params); + } + + for (const auto& input_params : input_param_array_) { + addInput(input_params, nh, nh_private); + } + + return true; +} void RosServer::advertiseServices(ros::NodeHandle& nh_private) { reset_map_srv_ = nh_private.advertiseServiceclearLayeredMap(); + } else if (occupancy_map_) { occupancy_map_->clear(); } ROS_INFO("Map reset request was successfully executed."); diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt index 2c505017e..0076807e8 100644 --- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt +++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt @@ -3,17 +3,18 @@ project(wavemap_ros_conversions) # Find dependencies find_package(catkin REQUIRED - COMPONENTS roscpp eigen_conversions wavemap wavemap_msgs) + COMPONENTS roscpp eigen_conversions sensor_msgs visualization_msgs wavemap wavemap_msgs) # Register catkin package catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp eigen_conversions wavemap wavemap_msgs) + CATKIN_DEPENDS roscpp eigen_conversions sensor_msgs visualization_msgs wavemap wavemap_msgs) # Libraries add_library(${PROJECT_NAME} src/config_conversions.cc + src/layered_map_file_conversions.cc src/map_msg_conversions.cc src/time_conversions.cc) set_wavemap_target_properties(${PROJECT_NAME}) diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h new file mode 100644 index 000000000..2ca706315 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layer_observation_conversions.h @@ -0,0 +1,169 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYER_OBSERVATION_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYER_OBSERVATION_CONVERSIONS_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace wavemap::convert { +namespace detail { + +template +bool layerPayloadIsValid(const wavemap_msgs::Layer& layer, + size_t observation_count) { + return layer.type == LayerRosCodec::typeName() && + LayerRosCodec::payloadIsValid(layer, observation_count); +} + +template +bool integrateLayer( + const wavemap_msgs::Layer& layer, + const std::vector& positions, PipelineT& pipeline, + bool apply, std::string* error_message) { + using Value = layered::LayerValueT; + if (!layerPayloadIsValid(layer, positions.size())) { + if (error_message) { + *error_message = "Layer '" + layer.name + + "' has the wrong type or value count."; + } + return false; + } + + if (!apply) { + return true; + } + + std::vector> observations; + observations.reserve(positions.size()); + for (size_t index = 0u; index < positions.size(); ++index) { + observations.emplace_back( + positions[index], LayerRosCodec::read(layer, index)); + } + const auto result = pipeline.template integrate(observations); + if (result.rejected != 0u) { + if (error_message) { + *error_message = "Layer '" + layer.name + + "' contains an invalid spatial observation."; + } + return false; + } + return true; +} + +template +bool integrateBundle( + const wavemap_msgs::LayeredMapUpdate& msg, + const std::vector& positions, PipelineT& pipeline, + std::vector& consumed, bool apply, std::string* error_message) { + bool success = true; + const auto descriptors = layered::layerDescriptors(); + if constexpr (std::tuple_size_v == 0u) { + (void)msg; + (void)positions; + (void)pipeline; + (void)consumed; + (void)apply; + (void)error_message; + } else { + std::apply( + [&](const auto&... descriptor) { + auto process_descriptor = [&](const auto& current_descriptor) { + using Descriptor = std::decay_t; + using LayerTag = typename Descriptor::LayerTag; + size_t match_count = 0u; + size_t match_index = 0u; + for (size_t index = 0u; index < msg.layers.size(); ++index) { + if (msg.layers[index].name == current_descriptor.name()) { + ++match_count; + match_index = index; + } + } + if (match_count > 1u) { + success = false; + if (error_message) { + *error_message = "Layer '" + + std::string(current_descriptor.name()) + + "' occurs more than once."; + } + return; + } + if (match_count == 1u) { + consumed[match_index] = true; + success = success && + integrateLayer( + msg.layers[match_index], positions, pipeline, + apply, error_message); + } + }; + (process_descriptor(descriptor), ...); + }, + descriptors); + } + return success; +} + +} // namespace detail + +template +struct DescriptorLayerObservationRosConverter { + using Pipeline = layered::LayeredPipeline; + + static bool integrate(const wavemap_msgs::LayeredMapUpdate& msg, + Pipeline& pipeline, + std::string* error_message = nullptr) { + if (msg.positions.empty() || msg.layers.empty()) { + if (error_message) { + *error_message = + "A layered map update requires positions and at least one layer."; + } + return false; + } + + std::vector positions; + positions.reserve(msg.positions.size()); + for (const auto& position_msg : msg.positions) { + const Point3D position = point32MsgToPoint3D(position_msg); + if (!position.array().isFinite().all()) { + if (error_message) { + *error_message = "A layered map update position is not finite."; + } + return false; + } + positions.emplace_back(position); + } + + std::vector consumed(msg.layers.size(), false); + if (!detail::integrateBundle( + msg, positions, pipeline, consumed, false, error_message) || + !detail::integrateBundle( + msg, positions, pipeline, consumed, false, error_message)) { + return false; + } + + const auto unknown = std::find(consumed.cbegin(), consumed.cend(), false); + if (unknown != consumed.cend()) { + const size_t index = std::distance(consumed.cbegin(), unknown); + if (error_message) { + *error_message = "Unknown layer '" + msg.layers[index].name + "'."; + } + return false; + } + + std::fill(consumed.begin(), consumed.end(), false); + return detail::integrateBundle( + msg, positions, pipeline, consumed, true, error_message) && + detail::integrateBundle( + msg, positions, pipeline, consumed, true, error_message); + } +}; + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYER_OBSERVATION_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h new file mode 100644 index 000000000..92970bad6 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/descriptor_layered_map_conversions.h @@ -0,0 +1,308 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYERED_MAP_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYERED_MAP_CONVERSIONS_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap::convert { + +template +struct LayerRosCodec; + +namespace detail { +template +struct HasLayerRosCodec : std::false_type {}; + +template +struct HasLayerRosCodec< + ValueT, + std::void_t::typeName()), + decltype(LayerRosCodec::append( + std::declval(), + std::declval())), + decltype(LayerRosCodec::read( + std::declval(), size_t{})), + decltype(LayerRosCodec::payloadIsValid( + std::declval(), size_t{}))>> + : std::true_type {}; +} // namespace detail + +template +inline constexpr bool kHasLayerRosCodec = + detail::HasLayerRosCodec::value; + +template <> +struct LayerRosCodec { + static constexpr const char* typeName() { return "float32"; } + + static void append(const FloatingPoint& value, wavemap_msgs::Layer& layer) { + layer.float32_values.emplace_back(value); + } + + static FloatingPoint read(const wavemap_msgs::Layer& layer, + size_t value_index) { + return layer.float32_values[value_index]; + } + + static bool payloadIsValid(const wavemap_msgs::Layer& layer, + size_t value_count) { + return layer.float32_values.size() == value_count && + layer.int32_values.empty() && layer.uint8_values.empty(); + } +}; + +template <> +struct LayerRosCodec { + static constexpr const char* typeName() { return "float32_rgb"; } + + static void append(const layered::Rgb& value, + wavemap_msgs::Layer& layer) { + layer.float32_values.emplace_back(value.r); + layer.float32_values.emplace_back(value.g); + layer.float32_values.emplace_back(value.b); + } + + static layered::Rgb read(const wavemap_msgs::Layer& layer, + size_t value_index) { + const size_t first_component = 3u * value_index; + return {layer.float32_values[first_component], + layer.float32_values[first_component + 1u], + layer.float32_values[first_component + 2u]}; + } + + static bool payloadIsValid(const wavemap_msgs::Layer& layer, + size_t value_count) { + return layer.float32_values.size() == 3u * value_count && + layer.int32_values.empty() && layer.uint8_values.empty(); + } +}; + +template <> +struct LayerRosCodec { + static constexpr const char* typeName() { return "int32"; } + + static void append(int value, wavemap_msgs::Layer& layer) { + layer.int32_values.emplace_back(value); + } + + static int read(const wavemap_msgs::Layer& layer, size_t value_index) { + return layer.int32_values[value_index]; + } + + static bool payloadIsValid(const wavemap_msgs::Layer& layer, + size_t value_count) { + return layer.int32_values.size() == value_count && + layer.float32_values.empty() && layer.uint8_values.empty(); + } +}; + +template +struct DescriptorContinuousRosConverter { + using ContinuousLayers = typename VoxelT::AdditionalData; + + static_assert( + []() constexpr { + bool supported = true; + const auto descriptors = layered::layerDescriptors(); + std::apply( + [&](const auto&... descriptor) { + ((supported = supported && + kHasLayerRosCodec::LayerTag>>), + ...); + }, + descriptors); + return supported; + }(), + "Every continuous layer value published through ROS needs a " + "LayerRosCodec specialization."); + + static std::vector layerNames() { + std::vector names; + const auto descriptors = layered::layerDescriptors(); + names.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + (names.emplace_back(descriptor.name()), ...); + }, + descriptors); + return names; + } + + static std::vector layerTypes() { + std::vector types; + const auto descriptors = layered::layerDescriptors(); + types.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + (types.emplace_back( + LayerRosCodec::LayerTag>>:: + typeName()), + ...); + }, + descriptors); + return types; + } + + static std::vector layerVisualizations() { + std::vector visualizations; + const auto descriptors = layered::layerDescriptors(); + visualizations.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + ([&]() { + using LayerTag = + typename std::decay_t::LayerTag; + auto& msg = visualizations.emplace_back(); + msg.name = descriptor.name(); + if (const auto visualization = + layered::scalarLayerVisualization()) { + msg.has_low_color = true; + msg.low_r = visualization->low.r; + msg.low_g = visualization->low.g; + msg.low_b = visualization->low.b; + msg.has_high_color = true; + msg.high_r = visualization->high.r; + msg.high_g = visualization->high.g; + msg.high_b = visualization->high.b; + } + }(), ...); + }, descriptors); + return visualizations; + + } + static std::vector makeLayers() { + std::vector layers; + const auto names = layerNames(); + const auto types = layerTypes(); + layers.resize(names.size()); + for (size_t layer_index = 0u; layer_index < layers.size(); ++layer_index) { + layers[layer_index].name = names[layer_index]; + layers[layer_index].type = types[layer_index]; + } + return layers; + } + + static std::vector makeLayerMinimums( + const typename CellDataTraits::ThresholdConfig& config) { + return makeLayerBounds(config, true); + } + + static std::vector makeLayerMaximums( + const typename CellDataTraits::ThresholdConfig& config) { + return makeLayerBounds(config, false); + } + + static void appendLayerValues(const VoxelT& voxel, + std::vector& layers) { + size_t layer_index = 0u; + layered::forEachLayerDescriptor( + voxel.data, [&](const auto& descriptor, const auto& state) { + using LayerTag = typename std::decay_t::LayerTag; + using Value = layered::LayerValueT; + LayerRosCodec::append( + layered::LayerStateConversion::value(state), + layers[layer_index++]); + }); + } + + static VoxelT readCellData( + FloatingPoint occupancy, const std::vector& layers, + size_t value_index) { + VoxelT voxel; + voxel.occupancy = occupancy; + size_t layer_index = 0u; + layered::forEachLayerDescriptor( + voxel.data, [&](const auto& descriptor, auto& state) { + using LayerTag = typename std::decay_t::LayerTag; + using Value = layered::LayerValueT; + state = layered::LayerStateConversion::initialState( + LayerRosCodec::read(layers[layer_index++], value_index)); + }); + return voxel; + } + + private: + static std::vector makeLayerBounds( + const typename CellDataTraits::ThresholdConfig& config, + bool minimum) { + auto layers = makeLayers(); + size_t layer_index = 0u; + const auto descriptors = layered::layerDescriptors(); + std::apply( + [&](const auto&... descriptor) { + (appendLayerBound(descriptor, config, minimum, + layers[layer_index++]), + ...); + }, + descriptors); + return layers; + } + + template + static void appendLayerBound( + const DescriptorT& /*descriptor*/, + const typename CellDataTraits::ThresholdConfig& config, + bool minimum, wavemap_msgs::Layer& layer) { + using LayerTag = typename DescriptorT::LayerTag; + const auto& bounds = config.data.template get(); + using Value = layered::LayerValueT; + LayerRosCodec::append(minimum ? bounds.min : bounds.max, layer); + } +}; + +template +auto descriptorNamedDiscreteLayersForRos(DiscreteLayersT& layers) { + return layered::transformLayerDescriptors( + layered::layerDescriptors(), + [&](const auto& descriptor) { + using LayerTag = typename std::decay_t::LayerTag; + return namedDiscreteLayerForRos(std::string(descriptor.name()), + descriptor.value(layers), + layered::integerCategoryMetadata()); + }); +} + +template +auto descriptorNamedDiscreteLayersForRos(const DiscreteLayersT& layers) { + return layered::transformLayerDescriptors( + layered::layerDescriptors(), + [&](const auto& descriptor) { + using LayerTag = typename std::decay_t::LayerTag; + return namedDiscreteLayerForRos(std::string(descriptor.name()), + descriptor.value(layers), + layered::integerCategoryMetadata()); + }); +} + +// Schema-generated discrete bundles already carry their authoritative names +// and value types in their descriptors. They therefore need no user-written +// ROS serialization registration. +template +struct DiscreteLayerBundleRosTraits>> { + using Bundle = layered::schema::DiscreteLayerBundle< + layered::schema::LayerSchema>; + + static auto layers(Bundle& layers) { + return descriptorNamedDiscreteLayersForRos(layers); + } + + static auto layers(const Bundle& layers) { + return descriptorNamedDiscreteLayersForRos(layers); + } +}; + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_DESCRIPTOR_LAYERED_MAP_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h new file mode 100644 index 000000000..08fc1e7e5 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/discrete_layer_marker_conversions.h @@ -0,0 +1,160 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_DISCRETE_LAYER_MARKER_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_DISCRETE_LAYER_MARKER_CONVERSIONS_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace layered_map_viz { +namespace detail { +template +inline constexpr bool kAlwaysFalse = false; + +template +bool forEachTupleElementImpl(TupleT&& tuple, FuncT&& func, + std::index_sequence) { + bool ok = true; + ((ok = ok && func(std::get(tuple))), ...); + return ok; +} + +template +bool forEachTupleElement(TupleT&& tuple, FuncT&& func) { + constexpr size_t tuple_size = + std::tuple_size_v>; + return forEachTupleElementImpl(std::forward(tuple), + std::forward(func), + std::make_index_sequence{}); +} +} // namespace detail + +inline std_msgs::ColorRGBA color(float r, float g, float b, float a = 1.f) { + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +struct LayerMarkerConfig { + std::string topic; + std::string ns; + float marker_scale = 0.1f; +}; + +template +struct NamedDiscreteMarkerLayer { + std::string name; + const LayerT& layer; + ColorPolicyT color_policy; + LayerMarkerConfig marker_config; +}; + +template +NamedDiscreteMarkerLayer namedDiscreteMarkerLayer( + std::string name, const LayerT& layer, ColorPolicyT color_policy, + LayerMarkerConfig marker_config = {}) { + if (marker_config.topic.empty()) { + marker_config.topic = "/wavemap/layered_map/discrete/" + name; + } + if (marker_config.ns.empty()) { + marker_config.ns = name; + } + return {std::move(name), layer, std::move(color_policy), + std::move(marker_config)}; +} + +template +struct DiscreteLayerMarkerTraits { + static auto layers(const DiscreteLayersT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteLayerMarkerTraits must be specialized for this " + "discrete layer bundle type."); + } +}; + +template +bool discreteLayerToMarkerArray(const std::string& frame_id, + const ros::Time& stamp, + const wavemap::layered::DiscreteLayer& layer, + const ColorPolicyT& color_policy, + const LayerMarkerConfig& config, + visualization_msgs::MarkerArray& msg) { + msg.markers.clear(); + + visualization_msgs::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = stamp; + marker.ns = config.ns; + marker.id = 0; + marker.type = visualization_msgs::Marker::CUBE_LIST; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = config.marker_scale; + marker.scale.y = config.marker_scale; + marker.scale.z = config.marker_scale; + + for (const auto& [parent_key, cell] : layer.cells()) { + for (const int offset : cell.observed_offsets) { + const auto exception_it = cell.exceptions.find(offset); + const ValueT value = exception_it != cell.exceptions.end() + ? exception_it->second + : cell.dominant_value; + + std_msgs::ColorRGBA voxel_color; + if (!color_policy(value, voxel_color)) { + continue; + } + + const int side_length = layer.config().blockSideLength(); + const int local_x = offset % side_length; + const int local_y = (offset / side_length) % side_length; + const int local_z = offset / (side_length * side_length); + + geometry_msgs::Point point; + point.x = (parent_key.x * side_length + local_x) * config.marker_scale; + point.y = (parent_key.y * side_length + local_y) * config.marker_scale; + point.z = (parent_key.z * side_length + local_z) * config.marker_scale; + marker.points.emplace_back(point); + marker.colors.emplace_back(voxel_color); + } + } + + if (!marker.points.empty()) { + msg.markers.emplace_back(std::move(marker)); + } + return true; +} + +template +bool publishDiscreteLayerMarkers(const DiscreteLayersT& layers, + const std::string& frame_id, + const ros::Time& stamp, + PublisherFactoryT&& publisher_factory) { + const auto named_layers = DiscreteLayerMarkerTraits::layers(layers); + return detail::forEachTupleElement(named_layers, [&](const auto& named_layer) { + visualization_msgs::MarkerArray marker_msg; + if (!discreteLayerToMarkerArray(frame_id, stamp, named_layer.layer, + named_layer.color_policy, + named_layer.marker_config, marker_msg)) { + return false; + } + publisher_factory(named_layer.marker_config.topic).publish(marker_msg); + return true; + }); +} +} // namespace layered_map_viz + +#endif // WAVEMAP_ROS_CONVERSIONS_DISCRETE_LAYER_MARKER_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h new file mode 100644 index 000000000..1cae061fc --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h @@ -0,0 +1,408 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_IMPL_LAYERED_MAP_MSG_CONVERSIONS_INL_H_ +#define WAVEMAP_ROS_CONVERSIONS_IMPL_LAYERED_MAP_MSG_CONVERSIONS_INL_H_ + +#include + +#include +#include +#include + +namespace wavemap::convert { +template +bool mapToRosMsg(const HashedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Layered equivalent of the existing MapBase wrapper + // Fills the common ROS header and stores the typed map inside wavemap_msgs::Map. + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + + // The custom voxel type needs the user-provided converter + auto& layered_map_msg = msg.layered_hashed_wavelet_octree.emplace_back(); + convert::mapToRosMsg(map, layered_map_msg, std::move(include_blocks), std::move(thread_pool)); + return true; +} + +template +void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Use a small margin when deciding whether child nodes are saturated + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + // Copy the map metadata first + msg.min_cell_width = map.getMinCellWidth(); + msg.min_log_odds = map.getMinLogOdds(); + msg.max_log_odds = map.getMaxLogOdds(); + msg.tree_height = map.getTreeHeight(); + msg.layer_names = CellDataRosConverterT::layerNames(); + msg.layer_types = CellDataRosConverterT::layerTypes(); + msg.layer_min_values = + CellDataRosConverterT::makeLayerMinimums(map.getThresholdConfig()); + msg.layer_max_values = + CellDataRosConverterT::makeLayerMaximums(map.getThresholdConfig()); + msg.layer_visualizations = + CellDataRosConverterT::layerVisualizations(); + + // Always publish the full list of allocated block indices + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + }); + + // If the caller requested a subset of blocks, discard indices that are not currently allocated + if (include_blocks) { + for (auto include_block_it = include_blocks->begin(); + include_block_it != include_blocks->end();) { + if (map.hasBlock(*include_block_it)) { + ++include_block_it; + } else { + include_block_it = include_blocks->erase(include_block_it); + } + } + } else { + include_blocks.emplace(); + map.forEachBlock( + [&include_blocks](const Index3D& block_index, const auto& /*block*/) { + include_blocks->emplace(block_index); + }); + } + + int block_idx = 0; + msg.blocks.resize(include_blocks->size()); + for (const auto& block_index : include_blocks.value()) { + if (const auto* block = map.getBlock(block_index); block) { + auto& block_msg = msg.blocks[block_idx++]; + // Block conversion is independent per block, + // so it can safely run in the optional thread pool when the caller has one available. + if (thread_pool) { + thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, + &block_msg]() { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + }); + } else { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + } + } else { + ROS_ERROR("Block index not found. This should never happen."); + } + } + if (thread_pool) { + thread_pool->wait_all(); + } +} + +template +void blockToRosMsg( + const typename HashedWaveletOctreeT::BlockIndex& block_index, + const typename HashedWaveletOctreeT::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg) { + using Block = typename HashedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + // Store the block position and root scale separately from the internal wavelet nodes + // Occupancy is kept in the explicit occupancy field, while extra layers are encoded through the user-provided converter + msg.root_node_offset.x = block_index.x(); + msg.root_node_offset.y = block_index.y(); + msg.root_node_offset.z = block_index.z(); + msg.root_node_occupancy_scale_coefficient = static_cast(block.getRootScale()); + msg.root_node_layers = CellDataRosConverterT::makeLayers(); + CellDataRosConverterT::appendLayerValues(block.getRootScale(), msg.root_node_layers); + + // Traverse the block's wavelet tree without recursion + // Each stack entry carries the node and its reconstructed scale coefficient, to decide whether the children are saturated and should be serialized + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto& node = stack.top().node; + stack.pop(); + + auto& node_msg = msg.nodes.emplace_back(); + node_msg.detail_layers = CellDataRosConverterT::makeLayers(); + size_t coefficient_idx = 0u; + // Each wavelet node stores eight detail coefficients. + // Occupancy is copied into the fixed array; all custom layer values are appended layer-major into detail_layers by CellDataRosConverterT + for (const CellDataT& coefficient : node.data()) { + node_msg.occupancy_detail_coefficients[coefficient_idx++] = + static_cast(coefficient); + CellDataRosConverterT::appendLayerValues(coefficient, + node_msg.detail_layers); + } + + // Reconstruct child scales from the parent scale and detail coefficients + // Only children whose occupancy is not saturated need to be sent further + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + + if (const auto* child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); + // The bitset records which children have serialized node data. + node_msg.allocated_children_bitset += (1 << relative_child_idx); + } + } + } +} + +template +bool mapToRosMsg(const HashedChunkedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Layered equivalent of the existing MapBase wrapper + // Fills the common ROS header and stores the typed map inside wavemap_msgs::Map. + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + + // The custom voxel type needs the user-provided converter + auto& layered_map_msg = msg.layered_hashed_wavelet_octree.emplace_back(); + convert::mapToRosMsg(map, layered_map_msg, std::move(include_blocks), std::move(thread_pool)); + return true; +} + +template +void mapToRosMsg(const HashedChunkedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks, std::shared_ptr thread_pool) { + ProfilerZoneScoped; + // Use a small margin when deciding whether child nodes are saturated + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + // Copy the map metadata first + msg.min_cell_width = map.getMinCellWidth(); + msg.min_log_odds = map.getMinLogOdds(); + msg.max_log_odds = map.getMaxLogOdds(); + msg.tree_height = map.getTreeHeight(); + msg.layer_names = CellDataRosConverterT::layerNames(); + msg.layer_types = CellDataRosConverterT::layerTypes(); + msg.layer_min_values = + CellDataRosConverterT::makeLayerMinimums(map.getThresholdConfig()); + msg.layer_max_values = + CellDataRosConverterT::makeLayerMaximums(map.getThresholdConfig()); + msg.layer_visualizations = + CellDataRosConverterT::layerVisualizations(); + + // Always publish the full list of allocated block indices + msg.allocated_block_indices.reserve(map.getHashMap().size()); + map.forEachBlock([&msg](const Index3D& block_index, const auto& /*block*/) { + auto& block_index_msg = msg.allocated_block_indices.emplace_back(); + block_index_msg.x = block_index.x(); + block_index_msg.y = block_index.y(); + block_index_msg.z = block_index.z(); + }); + + // If the caller requested a subset of blocks, discard indices that are not currently allocated + if (include_blocks) { + for (auto include_block_it = include_blocks->begin(); + include_block_it != include_blocks->end();) { + if (map.hasBlock(*include_block_it)) { + ++include_block_it; + } else { + include_block_it = include_blocks->erase(include_block_it); + } + } + } else { + include_blocks.emplace(); + map.forEachBlock( + [&include_blocks](const Index3D& block_index, const auto& /*block*/) { + include_blocks->emplace(block_index); + }); + } + + int block_idx = 0; + msg.blocks.resize(include_blocks->size()); + for (const auto& block_index : include_blocks.value()) { + if (const auto* block = map.getBlock(block_index); block) { + auto& block_msg = msg.blocks[block_idx++]; + // Block conversion is independent per block, + // so it can safely run in the optional thread pool when the caller has one available. + if (thread_pool) { + thread_pool->add_task([block_index, block, min_log_odds, max_log_odds, + &block_msg]() { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + }); + } else { + blockToRosMsg( + block_index, *block, min_log_odds, max_log_odds, block_msg); + } + } else { + ROS_ERROR("Block index not found. This should never happen."); + } + } + if (thread_pool) { + thread_pool->wait_all(); + } +} + +template +void blockToRosMsg( + const typename HashedChunkedWaveletOctreeT::BlockIndex& block_index, + const typename HashedChunkedWaveletOctreeT::Block& block, + FloatingPoint min_log_odds, FloatingPoint max_log_odds, + wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg) { + using Block = typename HashedChunkedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + // Store the block position and root scale separately from the internal wavelet nodes + // Occupancy is kept in the explicit occupancy field, while extra layers are encoded through the user-provided converter + msg.root_node_offset.x = block_index.x(); + msg.root_node_offset.y = block_index.y(); + msg.root_node_offset.z = block_index.z(); + msg.root_node_occupancy_scale_coefficient = static_cast(block.getRootScale()); + msg.root_node_layers = CellDataRosConverterT::makeLayers(); + CellDataRosConverterT::appendLayerValues(block.getRootScale(), msg.root_node_layers); + + // Traverse the block's wavelet tree without recursion + // Each stack entry carries the node and its reconstructed scale coefficient, to decide whether the children are saturated and should be serialized + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto node = stack.top().node; + stack.pop(); + + auto& node_msg = msg.nodes.emplace_back(); + node_msg.detail_layers = CellDataRosConverterT::makeLayers(); + size_t coefficient_idx = 0u; + // Each wavelet node stores eight detail coefficients. + // Occupancy is copied into the fixed array; all custom layer values are appended layer-major into detail_layers by CellDataRosConverterT + for (const CellDataT& coefficient : node.data()) { + node_msg.occupancy_detail_coefficients[coefficient_idx++] = + static_cast(coefficient); + CellDataRosConverterT::appendLayerValues(coefficient, + node_msg.detail_layers); + } + + // Reconstruct child scales from the parent scale and detail coefficients + // Only children whose occupancy is not saturated need to be sent further + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + + if (auto child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); + // The bitset records which children have serialized node data. + node_msg.allocated_children_bitset += (1 << relative_child_idx); + } + } + } +} + +template +void rosMsgToMap(const wavemap_msgs::LayeredHashedWaveletOctree& msg, + typename HashedWaveletOctreeT::Ptr& map) { + ProfilerZoneScoped; + // Rebuild the wavemap config from the message metadata + // If the destination map is null or incompatible, allocate a fresh layered map instance. + HashedWaveletOctreeConfig config; + config.min_cell_width = msg.min_cell_width; + config.min_log_odds = msg.min_log_odds; + config.max_log_odds = msg.max_log_odds; + config.tree_height = msg.tree_height; + + if (!map || map->getConfig() != config) { + map = std::make_shared>(config); + } + + // Synchronize allocated blocks with the message + // Blocks not listed in allocated_block_indices are removed from the destination map. + std::unordered_set allocated_blocks; + for (const auto& block_index : msg.allocated_block_indices) { + allocated_blocks.emplace(block_index.x, block_index.y, block_index.z); + } + map->eraseBlockIf( + [&allocated_blocks](const Index3D& block_index, const auto& /*block*/) { + return !allocated_blocks.count(block_index); + }); + + using Block = typename HashedWaveletOctreeT::Block; + using NodeType = typename Block::OctreeType::NodeType; + + for (const auto& block_msg : msg.blocks) { + // Allocate or fetch the target block and reconstruct its root voxel data from occupancy plus the serialized custom layers + const Index3D block_index{block_msg.root_node_offset.x, + block_msg.root_node_offset.y, + block_msg.root_node_offset.z}; + auto& block = map->getOrAllocateBlock(block_index); + block.getRootScale() = CellDataRosConverterT::readCellData( + block_msg.root_node_occupancy_scale_coefficient, + block_msg.root_node_layers, 0u); + + std::stack stack; + stack.emplace(&block.getRootNode()); + for (const auto& node_msg : block_msg.nodes) { + DCHECK(!stack.empty()); + NodeType* node = stack.top(); + stack.pop(); + + // Reconstruct each detail coefficient + // The converter receives the fixed occupancy coefficient and the layer-major custom data for the same coefficient index. + for (size_t coefficient_idx = 0u; coefficient_idx < node->data().size(); + ++coefficient_idx) { + node->data()[coefficient_idx] = CellDataRosConverterT::readCellData( + node_msg.occupancy_detail_coefficients[coefficient_idx], + node_msg.detail_layers, coefficient_idx); + } + + // Recreate only the children marked in the transmitted bitset + // Their node contents will be filled by subsequent node messages in stack traversal order. + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const bool child_exists = bit_ops::is_bit_set( + node_msg.allocated_children_bitset, relative_child_idx); + if (child_exists) { + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); + } + } + } + } +} + +template +bool rosMsgToMap(const wavemap_msgs::Map& msg, typename HashedWaveletOctreeT::Ptr& map) { + ProfilerZoneScoped; + // The typed layered wrapper accepts exactly one layered map and no legacy map payloads. This keeps the old generic MapBase path unchanged while avoiding ambiguous custom conversions. + const bool has_one_layered_map = msg.layered_hashed_wavelet_octree.size() == 1u; + const bool has_legacy_payload = !msg.hashed_blocks.empty() || !msg.wavelet_octree.empty() || !msg.hashed_wavelet_octree.empty(); + if (!has_one_layered_map || has_legacy_payload) { + ROS_WARN( + "Layered map ROS msg must contain exactly one layered hashed wavelet " + "octree and no legacy map payloads. Ignoring."); + map = nullptr; + return false; + } + + convert::rosMsgToMap(msg.layered_hashed_wavelet_octree.front(), map); + return static_cast(map); +} +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_IMPL_LAYERED_MAP_MSG_CONVERSIONS_INL_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h new file mode 100644 index 000000000..4cc308cb8 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layer_observation_msg_conversions.h @@ -0,0 +1,24 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_LAYER_OBSERVATION_MSG_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_LAYER_OBSERVATION_MSG_CONVERSIONS_H_ + +#include +#include + +#include +#include + +namespace wavemap::convert { + +template +struct RosLayerObservationBatch { + std_msgs::Header header; + std::vector> observations; + size_t rejected = 0u; + std::string error; + + explicit operator bool() const { return error.empty(); } +}; + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_LAYER_OBSERVATION_MSG_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h new file mode 100644 index 000000000..9ebfc2e2b --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_file_conversions.h @@ -0,0 +1,23 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_FILE_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_FILE_CONVERSIONS_H_ + +#include +#include + +#include +#include + +namespace wavemap::convert { + +// Loads a version-1 .lwvmp file by interpreting the layer schema stored in the +// file. This path supports Wavemap's built-in stream codecs and does not +// require the original compile-time LayerSchema to be present in the process. +bool layeredMapFileToRosMsg(const std::filesystem::path& file_path, + const std::string& frame_id, + const ros::Time& stamp, + wavemap_msgs::LayeredMap& msg, + std::string* error_message = nullptr); + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_FILE_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h new file mode 100644 index 000000000..c376dad44 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/layered_map_msg_conversions.h @@ -0,0 +1,427 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_MSG_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_MSG_CONVERSIONS_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap::convert { +namespace detail { +template +inline constexpr bool kLayeredMapRosAlwaysFalse = false; + +template +inline wavemap_msgs::Index3D indexKeyToRosMsg(const IndexKeyT& key) { + wavemap_msgs::Index3D msg; + msg.x = key.x; + msg.y = key.y; + msg.z = key.z; + return msg; +} + +template +inline IndexKeyT rosMsgToIndexKey(const wavemap_msgs::Index3D& msg) { + return IndexKeyT(wavemap::Index3D(msg.x, msg.y, msg.z)); +} +} // namespace detail + +template +struct NamedDiscreteLayerForRos { + std::string name; + LayerT& layer; + std::vector categories; +}; + +template +NamedDiscreteLayerForRos namedDiscreteLayerForRos( + std::string name, LayerT& layer, + std::vector categories = {}) { + return {std::move(name), layer, std::move(categories)}; +} + +inline void setDiscreteCategoryLabels( + const std::vector& categories, + wavemap_msgs::DiscreteLayer& msg) { + msg.categories.clear(); + msg.categories.reserve(categories.size()); + for (const auto& metadata : categories) { + auto& category = msg.categories.emplace_back(); + category.value = metadata.value; + category.label = metadata.label; + category.has_color = metadata.color.has_value(); + if (metadata.color) { + category.r = metadata.color->r; + category.g = metadata.color->g; + category.b = metadata.color->b; + } + } +} + +template +struct DiscreteLayerBundleRosTraits { + static auto layers(DiscreteLayersT&) { + static_assert(detail::kLayeredMapRosAlwaysFalse, + "DiscreteLayerBundleRosTraits must be specialized for " + "this discrete layer bundle type."); + } +}; + +template +struct DiscreteRosValueConverter { + static const char* typeName() { + static_assert(detail::kLayeredMapRosAlwaysFalse, + "DiscreteRosValueConverter must be specialized for this " + "discrete layer value type."); + return "unsupported"; + } +}; + +template <> +struct DiscreteRosValueConverter { + static const char* typeName() { return "int"; } + + static void setDominantValue(wavemap_msgs::DiscreteLayerCell& msg, + int value) { + msg.dominant_int32 = value; + } + + static bool readDominantValue(const wavemap_msgs::DiscreteLayerCell& msg, + int& value) { + value = msg.dominant_int32; + return true; + } + + static void appendExceptionValue(wavemap_msgs::DiscreteLayerCell& msg, + int value) { + msg.exception_int32_values.emplace_back(value); + } + + static bool readExceptionValue(const wavemap_msgs::DiscreteLayerCell& msg, + size_t index, int& value) { + if (msg.exception_int32_values.size() <= index) { + return false; + } + value = msg.exception_int32_values[index]; + return true; + } + + static bool hasExpectedExceptionValueCount( + const wavemap_msgs::DiscreteLayerCell& msg) { + return msg.exception_int32_values.size() == msg.exception_offsets.size(); + } +}; + +template <> +struct DiscreteRosValueConverter { + static const char* typeName() { return "bool"; } + + static void setDominantValue(wavemap_msgs::DiscreteLayerCell& msg, + bool value) { + msg.dominant_uint8 = value ? 1u : 0u; + } + + static bool readDominantValue(const wavemap_msgs::DiscreteLayerCell& msg, + bool& value) { + value = msg.dominant_uint8 != 0u; + return true; + } + + static void appendExceptionValue(wavemap_msgs::DiscreteLayerCell& msg, + bool value) { + msg.exception_uint8_values.emplace_back(value ? 1u : 0u); + } + + static bool readExceptionValue(const wavemap_msgs::DiscreteLayerCell& msg, + size_t index, bool& value) { + if (msg.exception_uint8_values.size() <= index) { + return false; + } + value = msg.exception_uint8_values[index] != 0u; + return true; + } + + static bool hasExpectedExceptionValueCount( + const wavemap_msgs::DiscreteLayerCell& msg) { + return msg.exception_uint8_values.size() == msg.exception_offsets.size(); + } +}; + +template +void discreteCellToRosMsg( + const typename DiscreteLayerT::Cell& cell, + const IndexKeyT& parent_key, + wavemap_msgs::DiscreteLayerCell& cell_msg) { + using ValueT = typename DiscreteLayerT::Value; + cell_msg.parent_index = detail::indexKeyToRosMsg(parent_key); + DiscreteRosValueConverter::setDominantValue( + cell_msg, cell.dominant_value); + + cell_msg.observed_offsets.reserve(cell.observed_offsets.size()); + for (const int offset : cell.observed_offsets) { + cell_msg.observed_offsets.emplace_back(offset); + } + cell_msg.exception_offsets.reserve(cell.exceptions.size()); + for (const auto& [offset, value] : cell.exceptions) { + cell_msg.exception_offsets.emplace_back(offset); + DiscreteRosValueConverter::appendExceptionValue(cell_msg, value); + } +} + +template +void discreteLayerToRosMsg(const std::string& name, + const DiscreteLayerT& layer, + wavemap_msgs::DiscreteLayer& msg, + const std::vector& + categories = {}) { + using ValueT = typename DiscreteLayerT::Value; + + msg.name = name; + msg.value_type = DiscreteRosValueConverter::typeName(); + msg.block_height = layer.config().block_height; + setDiscreteCategoryLabels(categories, msg); + msg.cells.clear(); + msg.cells.reserve(layer.cells().size()); + + for (const auto& [parent_key, cell] : layer.cells()) { + wavemap_msgs::DiscreteLayerCell cell_msg; + discreteCellToRosMsg(cell, parent_key, cell_msg); + msg.cells.emplace_back(std::move(cell_msg)); + } +} + +template +void discreteLayerPatchToRosMsg(const std::string& name, + const DiscreteLayerT& layer, + wavemap_msgs::DiscreteLayer& msg, + const std::vector& + categories = {}) { + using ValueT = typename DiscreteLayerT::Value; + msg.name = name; + msg.value_type = DiscreteRosValueConverter::typeName(); + msg.block_height = layer.config().block_height; + setDiscreteCategoryLabels(categories, msg); + for (const auto& parent_key : layer.dirtyParentKeys()) { + const auto cell_it = layer.cells().find(parent_key); + if (cell_it == layer.cells().end()) { + msg.deleted_parent_indices.emplace_back( + detail::indexKeyToRosMsg(parent_key)); + } else { + auto& cell_msg = msg.cells.emplace_back(); + discreteCellToRosMsg(cell_it->second, parent_key, + cell_msg); + } + } +} + +template +bool rosMsgToDiscreteLayer(const wavemap_msgs::DiscreteLayer& msg, + const std::string& expected_name, + DiscreteLayerT& layer) { + using ValueT = typename DiscreteLayerT::Value; + using CellsT = std::remove_cv_t>; + using IndexKeyT = typename CellsT::key_type; + using ConfigT = std::remove_cv_t>; + + if (msg.name != expected_name || + msg.value_type != DiscreteRosValueConverter::typeName()) { + return false; + } + + std::map cells; + for (const wavemap_msgs::DiscreteLayerCell& cell_msg : msg.cells) { + if (!DiscreteRosValueConverter::hasExpectedExceptionValueCount( + cell_msg)) { + return false; + } + + typename DiscreteLayerT::Cell cell; + if (!DiscreteRosValueConverter::readDominantValue( + cell_msg, cell.dominant_value)) { + return false; + } + + for (const int offset : cell_msg.observed_offsets) { + cell.observed_offsets.insert(offset); + } + + for (size_t exception_i = 0u; + exception_i < cell_msg.exception_offsets.size(); ++exception_i) { + ValueT value{}; + if (!DiscreteRosValueConverter::readExceptionValue( + cell_msg, exception_i, value)) { + return false; + } + cell.exceptions[cell_msg.exception_offsets[exception_i]] = value; + } + + cells[detail::rosMsgToIndexKey(cell_msg.parent_index)] = cell; + } + + layer.replaceCellsForLoad(ConfigT{msg.block_height}, std::move(cells)); + return true; +} + +template +bool forEachRosTupleElementImpl(TupleT&& tuple, FuncT&& func, + std::index_sequence) { + bool ok = true; + ((ok = ok && func(std::get(tuple))), ...); + return ok; +} + +template +bool forEachRosTupleElement(TupleT&& tuple, FuncT&& func) { + constexpr size_t tuple_size = + std::tuple_size_v>; + return forEachRosTupleElementImpl(std::forward(tuple), + std::forward(func), + std::make_index_sequence{}); +} + +template +bool discreteLayerBundleToRosMsg(const DiscreteLayersT& layers, + std::vector& msg) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + msg.clear(); + msg.reserve(std::tuple_size_v>); + + return forEachRosTupleElement(named_layers, [&](const auto& named_layer) { + wavemap_msgs::DiscreteLayer layer_msg; + discreteLayerToRosMsg(named_layer.name, named_layer.layer, layer_msg, + named_layer.categories); + msg.emplace_back(std::move(layer_msg)); + return true; + }); +} + +template +bool discreteLayerBundlePatchToRosMsg( + const DiscreteLayersT& layers, + std::vector& msg) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + msg.clear(); + return forEachRosTupleElement(named_layers, [&](const auto& named_layer) { + if (named_layer.layer.dirtyParentKeys().empty()) { + return true; + } + auto& layer_msg = msg.emplace_back(); + discreteLayerPatchToRosMsg(named_layer.name, named_layer.layer, layer_msg, + named_layer.categories); + return true; + }); +} + +template +void clearDiscreteLayerBundleDirtyState(DiscreteLayersT& layers) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + forEachRosTupleElement(named_layers, [](auto named_layer) { + named_layer.layer.clearDirtyParentKeys(); + return true; + }); +} + +template +bool discreteLayerBundleHasDirtyState(const DiscreteLayersT& layers) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + bool has_dirty_state = false; + forEachRosTupleElement(named_layers, [&](const auto& named_layer) { + has_dirty_state = has_dirty_state || + !named_layer.layer.dirtyParentKeys().empty(); + return true; + }); + return has_dirty_state; +} + +template +bool rosMsgToDiscreteLayerBundle( + const std::vector& msg, + DiscreteLayersT& layers) { + const auto named_layers = + DiscreteLayerBundleRosTraits::layers(layers); + constexpr size_t expected_layer_count = + std::tuple_size_v>; + if (msg.size() != expected_layer_count) { + return false; + } + + size_t layer_index = 0u; + return forEachRosTupleElement(named_layers, [&](auto named_layer) { + return rosMsgToDiscreteLayer(msg[layer_index++], named_layer.name, + named_layer.layer); + }); +} + +template +bool layeredMapToRosMsg(const LayeredMapT& map, const std::string& frame_id, + const ros::Time& stamp, + wavemap_msgs::LayeredMap& msg) { + msg.header.frame_id = frame_id; + msg.header.stamp = stamp; + msg.is_full_update = true; + if (!mapToRosMsg(map.continuousMap(), frame_id, + stamp, msg.continuous_map)) { + return false; + } + return discreteLayerBundleToRosMsg(map.discreteLayers(), + msg.discrete_layers); +} + +template +bool layeredMapPatchToRosMsg( + const LayeredMapT& map, const std::string& frame_id, + const ros::Time& stamp, wavemap_msgs::LayeredMap& msg, + std::optional> include_blocks, + std::shared_ptr thread_pool = nullptr) { + msg.header.frame_id = frame_id; + msg.header.stamp = stamp; + msg.is_full_update = false; + if (!mapToRosMsg( + map.continuousMap(), frame_id, stamp, msg.continuous_map, + std::move(include_blocks), std::move(thread_pool))) { + return false; + } + return discreteLayerBundlePatchToRosMsg(map.discreteLayers(), + msg.discrete_layers); +} + +template +bool rosMsgToLayeredMap(const wavemap_msgs::LayeredMap& msg, + LayeredMapT& map) { + typename LayeredMapT::ContinuousMap::Ptr continuous_map; + if (!rosMsgToMap(msg.continuous_map, + continuous_map) || + !continuous_map) { + return false; + } + + typename LayeredMapT::DiscreteLayers discrete_layers; + if (!rosMsgToDiscreteLayerBundle(msg.discrete_layers, discrete_layers)) { + return false; + } + + map = LayeredMapT(std::move(continuous_map), std::move(discrete_layers)); + return true; +} +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_LAYERED_MAP_MSG_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h index 8d230c14f..09e4bedf9 100644 --- a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/map_msg_conversions.h @@ -15,6 +15,9 @@ #include #include #include +#include +#include +#include #include namespace wavemap::convert { @@ -50,6 +53,32 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index, const HashedChunkedWaveletOctree::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::HashedWaveletOctreeBlock& msg); + +template +bool mapToRosMsg(const HashedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void mapToRosMsg(const HashedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void blockToRosMsg(const typename HashedWaveletOctreeT::BlockIndex& block_index, const typename HashedWaveletOctreeT::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg); + +template +bool mapToRosMsg(const HashedChunkedWaveletOctreeT& map, const std::string& frame_id, const ros::Time& stamp, wavemap_msgs::Map& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void mapToRosMsg(const HashedChunkedWaveletOctreeT& map, wavemap_msgs::LayeredHashedWaveletOctree& msg, std::optional> include_blocks = std::nullopt, std::shared_ptr thread_pool = nullptr); + +template +void blockToRosMsg(const typename HashedChunkedWaveletOctreeT::BlockIndex& block_index, const typename HashedChunkedWaveletOctreeT::Block& block, FloatingPoint min_log_odds, FloatingPoint max_log_odds, wavemap_msgs::LayeredHashedWaveletOctreeBlock& msg); + +template +void rosMsgToMap(const wavemap_msgs::LayeredHashedWaveletOctree& msg, typename HashedWaveletOctreeT::Ptr& map); + +template +bool rosMsgToMap(const wavemap_msgs::Map& msg, typename HashedWaveletOctreeT::Ptr& map); } // namespace wavemap::convert +#include "wavemap_ros_conversions/impl/layered_map_msg_conversions_inl.h" + #endif // WAVEMAP_ROS_CONVERSIONS_MAP_MSG_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h new file mode 100644 index 000000000..ee46e9111 --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/pointcloud_layer_observation_conversions.h @@ -0,0 +1,218 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_POINTCLOUD_LAYER_OBSERVATION_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_POINTCLOUD_LAYER_OBSERVATION_CONVERSIONS_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace wavemap::convert { + +struct ReflectivityObservationConfig { + std::string field_name = "reflectivity"; + FloatingPoint scale = 1.f / 255.f; + FloatingPoint offset = 0.f; + FloatingPoint min_value = 0.f; + FloatingPoint max_value = 1.f; + size_t point_stride = 1u; +}; +namespace detail { + +inline const sensor_msgs::PointField* findField( + const sensor_msgs::PointCloud2& msg, const std::string& name) { + const auto it = std::find_if( + msg.fields.cbegin(), msg.fields.cend(), + [&](const auto& field) { return field.name == name; }); + return it == msg.fields.cend() ? nullptr : &*it; +} + +template +bool readPointField(const sensor_msgs::PointCloud2& msg, + const sensor_msgs::PointField& field, + size_t point_offset, FloatingPoint& value) { + if (point_offset + field.offset + sizeof(T) > msg.data.size()) { + return false; + } + T stored{}; + std::memcpy(&stored, msg.data.data() + point_offset + field.offset, + sizeof(T)); + value = static_cast(stored); + return true; +} + +inline bool readNumericPointField( + const sensor_msgs::PointCloud2& msg, + const sensor_msgs::PointField& field, size_t point_offset, + FloatingPoint& value) { + if (field.count != 1u) { + return false; + } + switch (field.datatype) { + case sensor_msgs::PointField::INT8: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::UINT8: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::INT16: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::UINT16: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::INT32: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::UINT32: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::FLOAT32: + return readPointField(msg, field, point_offset, value); + case sensor_msgs::PointField::FLOAT64: + return readPointField(msg, field, point_offset, value); + default: + return false; + } +} + +} // namespace detail + +template +struct RosReflectivityEndpointPointcloud { + std_msgs::Header header; + layered::EndpointAttributePointcloud pointcloud; + size_t rejected = 0u; + std::string error; + + explicit operator bool() const { return error.empty(); } +}; + +template +RosReflectivityEndpointPointcloud +pointcloudToReflectivityEndpointPointcloud( + const sensor_msgs::PointCloud2& msg, + const ReflectivityObservationConfig& config = {}) { + static_assert( + std::is_same_v, + FloatingPoint>, + "The reflectivity endpoint layer must store FloatingPoint values."); + RosReflectivityEndpointPointcloud result; + result.header = msg.header; + if (msg.is_bigendian) { + result.error = "Big-endian PointCloud2 messages are not supported."; + return result; + } + if (config.point_stride == 0u || msg.point_step == 0u || msg.width == 0u) { + result.error = + "Point stride, PointCloud2 point_step, and width must be nonzero."; + return result; + } + + const auto* x = detail::findField(msg, "x"); + const auto* y = detail::findField(msg, "y"); + const auto* z = detail::findField(msg, "z"); + const auto* reflectivity = detail::findField(msg, config.field_name); + if (!x || !y || !z || !reflectivity) { + result.error = "PointCloud2 is missing x, y, z, or reflectivity."; + return result; + } + + const size_t point_count = + static_cast(msg.width) * static_cast(msg.height); + const size_t capacity = + (point_count + config.point_stride - 1u) / config.point_stride; + result.pointcloud.resize(capacity); + size_t accepted = 0u; + for (size_t index = 0u; index < point_count; + index += config.point_stride) { + const size_t row = index / msg.width; + const size_t column = index % msg.width; + const size_t point_offset = + row * msg.row_step + column * msg.point_step; + FloatingPoint px = 0.f; + FloatingPoint py = 0.f; + FloatingPoint pz = 0.f; + FloatingPoint raw_reflectivity = 0.f; + if (!detail::readNumericPointField(msg, *x, point_offset, px) || + !detail::readNumericPointField(msg, *y, point_offset, py) || + !detail::readNumericPointField(msg, *z, point_offset, pz) || + !detail::readNumericPointField( + msg, *reflectivity, point_offset, raw_reflectivity) || + !std::isfinite(px) || !std::isfinite(py) || !std::isfinite(pz) || + !std::isfinite(raw_reflectivity)) { + ++result.rejected; + continue; + } + + result.pointcloud.point(accepted) = Point3D(px, py, pz); + result.pointcloud.template attribute(accepted) = + std::clamp(config.offset + config.scale * raw_reflectivity, + config.min_value, config.max_value); + ++accepted; + } + result.pointcloud.truncate(accepted); + return result; +} + +inline RosLayerObservationBatch +pointcloudToReflectivityObservations( + const sensor_msgs::PointCloud2& msg, + const ReflectivityObservationConfig& config = {}) { + RosLayerObservationBatch result; + result.header = msg.header; + if (msg.is_bigendian) { + result.error = "Big-endian PointCloud2 messages are not supported."; + return result; + } + if (config.point_stride == 0u || msg.point_step == 0u) { + result.error = "Point stride and PointCloud2 point_step must be nonzero."; + return result; + } + + const auto* x = detail::findField(msg, "x"); + const auto* y = detail::findField(msg, "y"); + const auto* z = detail::findField(msg, "z"); + const auto* reflectivity = detail::findField(msg, config.field_name); + if (!x || !y || !z || !reflectivity) { + result.error = "PointCloud2 is missing x, y, z, or reflectivity."; + return result; + } + + const size_t point_count = + static_cast(msg.width) * static_cast(msg.height); + result.observations.reserve( + (point_count + config.point_stride - 1u) / config.point_stride); + for (size_t index = 0u; index < point_count; + index += config.point_stride) { + const size_t row = index / msg.width; + const size_t column = index % msg.width; + const size_t point_offset = + row * msg.row_step + column * msg.point_step; + FloatingPoint px = 0.f; + FloatingPoint py = 0.f; + FloatingPoint pz = 0.f; + FloatingPoint raw_reflectivity = 0.f; + if (!detail::readNumericPointField(msg, *x, point_offset, px) || + !detail::readNumericPointField(msg, *y, point_offset, py) || + !detail::readNumericPointField(msg, *z, point_offset, pz) || + !detail::readNumericPointField( + msg, *reflectivity, point_offset, raw_reflectivity) || + !std::isfinite(px) || !std::isfinite(py) || !std::isfinite(pz) || + !std::isfinite(raw_reflectivity)) { + ++result.rejected; + continue; + } + + const FloatingPoint value = std::clamp( + config.offset + config.scale * raw_reflectivity, + config.min_value, config.max_value); + result.observations.emplace_back(Point3D(px, py, pz), value); + } + return result; +} + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_POINTCLOUD_LAYER_OBSERVATION_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h new file mode 100644 index 000000000..b0b9d155c --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/include/wavemap_ros_conversions/rgbd_layer_observation_conversions.h @@ -0,0 +1,124 @@ +#ifndef WAVEMAP_ROS_CONVERSIONS_RGBD_LAYER_OBSERVATION_CONVERSIONS_H_ +#define WAVEMAP_ROS_CONVERSIONS_RGBD_LAYER_OBSERVATION_CONVERSIONS_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace wavemap::convert { + +struct RgbdColorObservationConfig { + FloatingPoint depth_scale_16uc1 = 1e-3f; + FloatingPoint min_depth = 0.1f; + FloatingPoint max_depth = 20.f; + size_t pixel_stride = 1u; +}; + +inline RosLayerObservationBatch +rgbdToColorObservations( + const sensor_msgs::Image& color, const sensor_msgs::Image& aligned_depth, + const sensor_msgs::CameraInfo& camera_info, + const RgbdColorObservationConfig& config = {}) { + RosLayerObservationBatch result; + result.header = color.header; + + const bool rgb = color.encoding == sensor_msgs::image_encodings::RGB8; + const bool bgr = color.encoding == sensor_msgs::image_encodings::BGR8; + const bool depth_u16 = + aligned_depth.encoding == sensor_msgs::image_encodings::TYPE_16UC1; + const bool depth_f32 = + aligned_depth.encoding == sensor_msgs::image_encodings::TYPE_32FC1; + if ((!rgb && !bgr) || (!depth_u16 && !depth_f32)) { + result.error = + "Expected rgb8/bgr8 color and 16UC1/32FC1 aligned depth."; + return result; + } + if (color.is_bigendian || aligned_depth.is_bigendian) { + result.error = "Big-endian RGB-D images are not supported."; + return result; + } + if (config.pixel_stride == 0u || color.width != aligned_depth.width || + color.height != aligned_depth.height || color.width == 0u || + color.height == 0u) { + result.error = "RGB and aligned depth dimensions must match and be nonzero."; + return result; + } + if ((!aligned_depth.header.frame_id.empty() && + aligned_depth.header.frame_id != color.header.frame_id) || + (!camera_info.header.frame_id.empty() && + camera_info.header.frame_id != color.header.frame_id)) { + result.error = "RGB, aligned depth, and CameraInfo frames must match."; + return result; + } + if (color.step < 3u * color.width || + aligned_depth.step < + (depth_u16 ? 2u : 4u) * aligned_depth.width || + color.data.size() < + static_cast(color.step) * color.height || + aligned_depth.data.size() < + static_cast(aligned_depth.step) * aligned_depth.height) { + result.error = "RGB-D image payload is smaller than its declared layout."; + return result; + } + + const FloatingPoint fx = camera_info.K[0]; + const FloatingPoint fy = camera_info.K[4]; + const FloatingPoint cx = camera_info.K[2]; + const FloatingPoint cy = camera_info.K[5]; + if (!(0.f < fx) || !(0.f < fy)) { + result.error = "CameraInfo contains invalid focal lengths."; + return result; + } + + const size_t sampled_width = + (color.width + config.pixel_stride - 1u) / config.pixel_stride; + const size_t sampled_height = + (color.height + config.pixel_stride - 1u) / config.pixel_stride; + result.observations.reserve(sampled_width * sampled_height); + + for (size_t v = 0u; v < color.height; v += config.pixel_stride) { + for (size_t u = 0u; u < color.width; u += config.pixel_stride) { + const size_t depth_offset = v * aligned_depth.step + + u * (depth_u16 ? 2u : 4u); + FloatingPoint depth = 0.f; + if (depth_u16) { + uint16_t raw_depth = 0u; + std::memcpy(&raw_depth, aligned_depth.data.data() + depth_offset, + sizeof(raw_depth)); + depth = config.depth_scale_16uc1 * raw_depth; + } else { + std::memcpy(&depth, aligned_depth.data.data() + depth_offset, + sizeof(depth)); + } + if (!std::isfinite(depth) || depth < config.min_depth || + config.max_depth < depth) { + ++result.rejected; + continue; + } + + const size_t color_offset = v * color.step + 3u * u; + const auto* channels = color.data.data() + color_offset; + const FloatingPoint red = (rgb ? channels[0] : channels[2]) / 255.f; + const FloatingPoint green = channels[1] / 255.f; + const FloatingPoint blue = (rgb ? channels[2] : channels[0]) / 255.f; + const Point3D position((static_cast(u) - cx) * depth / fx, + (static_cast(v) - cy) * depth / fy, + depth); + result.observations.emplace_back( + position, layered::Rgb{red, green, blue}); + } + } + return result; +} + +} // namespace wavemap::convert + +#endif // WAVEMAP_ROS_CONVERSIONS_RGBD_LAYER_OBSERVATION_CONVERSIONS_H_ diff --git a/interfaces/ros1/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml index 9336c2bea..a7e8ea250 100644 --- a/interfaces/ros1/wavemap_ros_conversions/package.xml +++ b/interfaces/ros1/wavemap_ros_conversions/package.xml @@ -16,5 +16,7 @@ wavemap_msgs roscpp + sensor_msgs eigen_conversions + visualization_msgs diff --git a/interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc new file mode 100644 index 000000000..aad77282a --- /dev/null +++ b/interfaces/ros1/wavemap_ros_conversions/src/layered_map_file_conversions.cc @@ -0,0 +1,481 @@ +#include "wavemap_ros_conversions/layered_map_file_conversions.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap::convert { +namespace { +using layered::LayerSchemaEntry; +using layered::LayeredMapSchema; + +constexpr uint64_t kMaxLayerCount = 1024u; +constexpr uint64_t kMaxNameLength = 4096u; +constexpr uint64_t kMaxBlockCount = 10'000'000u; +constexpr uint64_t kMaxNodeCount = 100'000'000u; +constexpr uint64_t kMaxDiscreteCellCount = 100'000'000u; +constexpr uint64_t kMaxOffsetsPerCell = 100'000'000u; + +bool fail(const std::string& message, std::string* error_message) { + if (error_message) { + *error_message = message; + } + return false; +} + +template +bool readPod(std::istream& stream, T& value) { + static_assert(std::is_trivially_copyable_v); + stream.read(reinterpret_cast(&value), sizeof(value)); + return static_cast(stream); +} + +bool readString(std::istream& stream, std::string& value) { + uint64_t size = 0u; + if (!readPod(stream, size) || kMaxNameLength < size) { + return false; + } + value.resize(static_cast(size)); + stream.read(value.data(), static_cast(size)); + return static_cast(stream); +} + +bool readSection(std::istream& stream, const std::string& expected) { + std::string actual; + return readString(stream, actual) && actual == expected; +} + +bool readSchemaEntries(std::istream& stream, + std::vector& entries) { + uint64_t count = 0u; + if (!readPod(stream, count) || kMaxLayerCount < count) { + return false; + } + entries.clear(); + entries.reserve(static_cast(count)); + for (uint64_t index = 0u; index < count; ++index) { + auto& entry = entries.emplace_back(); + if (!readString(stream, entry.name) || !readString(stream, entry.type)) { + return false; + } + } + return true; +} + +bool readSchema(std::istream& stream, LayeredMapSchema& schema) { + return readSchemaEntries(stream, schema.continuous_layers) && + readSchemaEntries(stream, schema.discrete_layers); +} + +std::string rosTypeForStreamType(const std::string& stream_type) { + if (stream_type == "float32_weighted_mean_state") { + return "float32"; + } + return stream_type; +} + +bool isSupportedContinuousType(const std::string& type) { + return type == "float32" || type == "float32_rgb" || + type == "float32_weighted_mean_state"; +} + +std::vector makeLayers( + const std::vector& layers) { + std::vector result; + result.reserve(layers.size()); + for (const auto& layer : layers) { + auto& layer_msg = result.emplace_back(); + layer_msg.name = layer.name; + layer_msg.type = rosTypeForStreamType(layer.type); + } + return result; +} + +bool appendContinuousValue(std::istream& stream, + const LayerSchemaEntry& schema, + wavemap_msgs::Layer& layer) { + if (schema.type == "float32") { + float value = 0.f; + if (!readPod(stream, value)) { + return false; + } + layer.float32_values.emplace_back(value); + return true; + } + if (schema.type == "float32_rgb") { + std::array value{}; + for (float& component : value) { + if (!readPod(stream, component)) { + return false; + } + layer.float32_values.emplace_back(component); + } + return true; + } + if (schema.type == "float32_weighted_mean_state") { + float weighted_sum = 0.f; + float total_weight = 0.f; + if (!readPod(stream, weighted_sum) || !readPod(stream, total_weight)) { + return false; + } + layer.float32_values.emplace_back( + 0.f < total_weight ? weighted_sum / total_weight : 0.f); + return true; + } + return false; +} + +bool readContinuousCell(std::istream& stream, + const std::vector& custom_layers, + float& occupancy, + std::vector& layers) { + if (!readPod(stream, occupancy)) { + return false; + } + for (size_t layer_index = 0u; layer_index < custom_layers.size(); + ++layer_index) { + if (!appendContinuousValue(stream, custom_layers[layer_index], + layers[layer_index])) { + return false; + } + } + return true; +} + +wavemap_msgs::Index3D toRosIndex( + const wavemap::io::streamable::Index3D& index) { + wavemap_msgs::Index3D msg; + msg.x = index.x; + msg.y = index.y; + msg.z = index.z; + return msg; +} + +unsigned int countSetBits(uint8_t value) { + unsigned int count = 0u; + while (value != 0u) { + count += value & 1u; + value >>= 1u; + } + return count; +} + +bool readContinuousMap(std::istream& stream, + const std::vector& custom_layers, + wavemap_msgs::LayeredHashedWaveletOctree& msg, + std::string* error_message) { + const auto storage_format = + wavemap::io::streamable::StorageFormat::read(stream); + if (!stream || storage_format != + wavemap::io::streamable::StorageFormat:: + kLayeredHashedWaveletOctree) { + return fail("The file does not contain a supported layered hashed-wavelet " + "continuous map.", error_message); + } + + const auto header = + wavemap::io::streamable::HashedWaveletOctreeHeader::read(stream); + if (!stream || !std::isfinite(header.min_cell_width) || + header.min_cell_width <= 0.f || !std::isfinite(header.min_log_odds) || + !std::isfinite(header.max_log_odds) || + header.max_log_odds < header.min_log_odds || header.tree_height < 0 || + 30 < header.tree_height || + kMaxBlockCount < header.num_blocks) { + return fail("The layered continuous-map header is invalid.", error_message); + } + msg.min_cell_width = header.min_cell_width; + msg.min_log_odds = header.min_log_odds; + msg.max_log_odds = header.max_log_odds; + msg.tree_height = header.tree_height; + msg.layer_names.reserve(custom_layers.size()); + msg.layer_types.reserve(custom_layers.size()); + for (const auto& layer : custom_layers) { + msg.layer_names.emplace_back(layer.name); + msg.layer_types.emplace_back(rosTypeForStreamType(layer.type)); + } + uint64_t total_node_count = 0u; + for (uint64_t block_index = 0u; block_index < header.num_blocks; + ++block_index) { + const auto serialized_index = + wavemap::io::streamable::Index3D::read(stream); + if (!stream) { + return fail("The layered map ended while reading a block index.", + error_message); + } + const auto ros_index = toRosIndex(serialized_index); + msg.allocated_block_indices.emplace_back(ros_index); + auto& block = msg.blocks.emplace_back(); + block.root_node_offset = ros_index; + block.root_node_layers = makeLayers(custom_layers); + if (!readContinuousCell(stream, custom_layers, + block.root_node_occupancy_scale_coefficient, + block.root_node_layers)) { + return fail("The layered map ended while reading a block root value.", + error_message); + } + + std::vector pending_node_heights{header.tree_height}; + while (!pending_node_heights.empty()) { + if (kMaxNodeCount <= total_node_count) { + return fail("The layered map contains too many wavelet nodes.", + error_message); + } + ++total_node_count; + const int32_t node_height = pending_node_heights.back(); + pending_node_heights.pop_back(); + auto& node = block.nodes.emplace_back(); + node.detail_layers = makeLayers(custom_layers); + for (size_t coefficient_index = 0u; + coefficient_index + 1u < + node.occupancy_detail_coefficients.size(); + ++coefficient_index) { + if (!readContinuousCell( + stream, custom_layers, + node.occupancy_detail_coefficients[coefficient_index], + node.detail_layers)) { + return fail("The layered map ended while reading wavelet " + "coefficients.", error_message); + } + } + if (!readPod(stream, node.allocated_children_bitset)) { + return fail("The layered map ended while reading its wavelet tree.", + error_message); + } + const unsigned int child_count = + countSetBits(node.allocated_children_bitset); + if (node_height == 0 && child_count != 0u) { + return fail("The layered map has children below its minimum cell " + "height.", error_message); + } + pending_node_heights.insert(pending_node_heights.end(), child_count, + node_height - 1); + } + } + return true; +} + +template +bool readDiscreteCells(std::istream& stream, + wavemap_msgs::DiscreteLayer& layer, + std::string* error_message) { + int32_t block_height = 0; + uint64_t cell_count = 0u; + if (!readPod(stream, block_height) || block_height < 0 || + 10 < block_height || !readPod(stream, cell_count) || + kMaxDiscreteCellCount < cell_count) { + return fail("A discrete-layer header is invalid.", error_message); + } + layer.block_height = block_height; + const int32_t max_offset = 1 << (3 * block_height); + for (uint64_t cell_index = 0u; cell_index < cell_count; ++cell_index) { + auto& cell = layer.cells.emplace_back(); + if (!readPod(stream, cell.parent_index.x) || + !readPod(stream, cell.parent_index.y) || + !readPod(stream, cell.parent_index.z)) { + return fail("The file ended while reading a discrete cell index.", + error_message); + } + ValueT dominant{}; + if (!readPod(stream, dominant)) { + return fail("The file ended while reading a discrete cell value.", + error_message); + } + if constexpr (std::is_same_v) { + cell.dominant_int32 = dominant; + } else { + if (1u < dominant) { + return fail("A boolean discrete layer contains a non-boolean value.", + error_message); + } + cell.dominant_uint8 = dominant; + } + + uint64_t observed_count = 0u; + if (!readPod(stream, observed_count) || + kMaxOffsetsPerCell < observed_count) { + return fail("A discrete cell's observed-offset list is invalid.", + error_message); + } + cell.observed_offsets.resize(static_cast(observed_count)); + for (int32_t& offset : cell.observed_offsets) { + if (!readPod(stream, offset) || offset < 0 || max_offset <= offset) { + return fail("A discrete cell contains an invalid observed offset.", + error_message); + } + } + + uint64_t exception_count = 0u; + if (!readPod(stream, exception_count) || + kMaxOffsetsPerCell < exception_count) { + return fail("A discrete cell's exception list is invalid.", + error_message); + } + cell.exception_offsets.resize(static_cast(exception_count)); + if constexpr (std::is_same_v) { + cell.exception_int32_values.reserve(static_cast(exception_count)); + } else { + cell.exception_uint8_values.reserve(static_cast(exception_count)); + } + for (size_t exception_index = 0u; + exception_index < static_cast(exception_count); + ++exception_index) { + ValueT value{}; + if (!readPod(stream, cell.exception_offsets[exception_index]) || + cell.exception_offsets[exception_index] < 0 || + max_offset <= cell.exception_offsets[exception_index] || + !readPod(stream, value)) { + return fail("A discrete cell contains an invalid exception.", + error_message); + } + if constexpr (std::is_same_v) { + cell.exception_int32_values.emplace_back(value); + } else { + if (1u < value) { + return fail("A boolean discrete layer contains a non-boolean value.", + error_message); + } + cell.exception_uint8_values.emplace_back(value); + } + } + } + return true; +} + +bool readDiscreteLayers(std::istream& stream, const LayeredMapSchema& schema, + std::vector& layers, + std::string* error_message) { + uint64_t layer_count = 0u; + if (!readPod(stream, layer_count) || + layer_count != schema.discrete_layers.size()) { + return fail("The discrete-layer data does not match the file schema.", + error_message); + } + layers.reserve(static_cast(layer_count)); + for (size_t layer_index = 0u; layer_index < static_cast(layer_count); + ++layer_index) { + std::string name; + std::string type; + if (!readString(stream, name) || !readString(stream, type) || + name != schema.discrete_layers[layer_index].name || + type != schema.discrete_layers[layer_index].type) { + return fail("The discrete-layer data does not match the file schema.", + error_message); + } + auto& layer = layers.emplace_back(); + layer.name = name; + layer.value_type = type; + if (type == "int") { + if (!readDiscreteCells(stream, layer, error_message)) { + return false; + } + } else if (type == "bool") { + if (!readDiscreteCells(stream, layer, error_message)) { + return false; + } + } else { + return fail("Unsupported discrete codec '" + type + "' for layer '" + + name + "'. Rebuild RViz with that layer's schema and " + "codec registered.", + error_message); + } + } + return true; +} +} // namespace + +bool layeredMapFileToRosMsg(const std::filesystem::path& file_path, + const std::string& frame_id, + const ros::Time& stamp, + wavemap_msgs::LayeredMap& msg, + std::string* error_message) { + std::ifstream stream(file_path, std::ios::binary); + if (!stream.is_open()) { + return fail("Could not open layered map file: " + file_path.string(), + error_message); + } + + std::string magic; + uint32_t version = 0u; + if (!readString(stream, magic) || magic != "LWVMP" || + !readPod(stream, version)) { + return fail("Invalid layered map file header.", error_message); + } + if (version != 1u) { + return fail("Unsupported layered map file version " + + std::to_string(version) + ".", + error_message); + } + + LayeredMapSchema schema; + if (!readSection(stream, "schema") || !readSchema(stream, schema)) { + return fail("Failed to read the layered map schema.", error_message); + } + if (schema.continuous_layers.empty() || + schema.continuous_layers.front() != + LayerSchemaEntry{"occupancy", "float32"}) { + return fail("The file does not have the required occupancy layer.", + error_message); + } + const std::vector custom_layers( + schema.continuous_layers.begin() + 1, + schema.continuous_layers.end()); + for (const auto& layer : custom_layers) { + if (!isSupportedContinuousType(layer.type)) { + return fail("Unsupported continuous codec '" + layer.type + + "' for layer '" + layer.name + + "'. Rebuild RViz with that layer's schema and codec " + "registered.", + error_message); + } + } + for (const auto& layer : schema.discrete_layers) { + if (layer.type != "int" && layer.type != "bool") { + return fail("Unsupported discrete codec '" + layer.type + + "' for layer '" + layer.name + + "'. Rebuild RViz with that layer's schema and codec " + "registered.", + error_message); + } + } + + wavemap_msgs::LayeredMap loaded; + loaded.header.frame_id = frame_id; + loaded.header.stamp = stamp; + loaded.continuous_map.header = loaded.header; + loaded.is_full_update = true; + if (!readSection(stream, "continuous_map")) { + return fail("Failed to find the layered continuous-map section.", + error_message); + } + auto& continuous = + loaded.continuous_map.layered_hashed_wavelet_octree.emplace_back(); + if (!readContinuousMap(stream, custom_layers, continuous, error_message)) { + return false; + } + if (!readSection(stream, "discrete_layers")) { + return fail("Failed to find the layered discrete-layers section.", + error_message); + } + if (!readDiscreteLayers(stream, schema, loaded.discrete_layers, + error_message)) { + return false; + } + msg = std::move(loaded); + return true; +} + +} // namespace wavemap::convert diff --git a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc index d9a66500f..32155692c 100644 --- a/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc +++ b/interfaces/ros1/wavemap_ros_conversions/test/src/test_map_msg_conversions.cc @@ -1,5 +1,7 @@ +#include #include #include +#include #include #include @@ -8,14 +10,69 @@ #include #include #include +#include +#include +#include +#include #include #include #include #include +#include "wavemap_ros_conversions/descriptor_layered_map_conversions.h" +#include "wavemap_ros_conversions/layered_map_file_conversions.h" #include "wavemap_ros_conversions/map_msg_conversions.h" namespace wavemap { +namespace { +struct TestReflectivityLayer + : layered::schema::ContinuousLayer< + FloatingPoint, + layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "reflectivity"; +}; + +using TestSchema = layered::schema::LayerSchema; +using ChunkedLayeredDefinition = layered::LayeredMapDefinition< + TestSchema, layered::HashedChunkedWaveletOctreeBackend>; +using RegularLayeredDefinition = layered::LayeredMapDefinition; +using TestLayerRosConverter = convert::DescriptorContinuousRosConverter< + ChunkedLayeredDefinition::Voxel>; + +struct GenericRgbLayer + : layered::schema::ContinuousLayer< + layered::Rgb, + layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_rgb"; +}; + +struct GenericAverageLayer + : layered::schema::StatefulContinuousLayer< + FloatingPoint, layered::WeightedMeanState, + layered::WeightedMeanLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_average"; +}; + +struct GenericClassLayer + : layered::schema::DiscreteLayer< + int, layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_class"; +}; + +struct GenericMaskLayer + : layered::schema::DiscreteLayer< + bool, layered::ReplaceLayerUpdatePolicy> { + static constexpr std::string_view name = "generic_mask"; +}; + +using GenericFileDefinition = layered::LayeredMapDefinition< + layered::schema::LayerSchema>; +using GenericFileRosConverter = convert::DescriptorContinuousRosConverter< + GenericFileDefinition::Voxel>; +} // namespace + template class MapMsgConversionsTest : public FixtureBase, public GeometryGenerator, @@ -166,4 +223,108 @@ TYPED_TEST(MapMsgConversionsTest, InsertionAndLeafVisitor) { } } } + +TEST(LayeredMapMsgConversionsTest, + ChunkedMapUsesLogicalHashedWaveletRepresentation) { + ros::Time::init(); + + ChunkedLayeredDefinition::ContinuousMap::Config config; + config.min_cell_width = 0.2f; + config.min_log_odds = -2.f; + config.max_log_odds = 4.f; + config.tree_height = 4; + + ChunkedLayeredDefinition::ContinuousMap chunked_map(config); + const Index3D cell_index{3, -2, 5}; + ChunkedLayeredDefinition::Voxel voxel; + voxel.occupancy = 0.7f; + voxel.data.get() = 0.35f; + chunked_map.setVoxelValue(cell_index, voxel); + + wavemap_msgs::Map map_msg; + ASSERT_TRUE((convert::mapToRosMsg( + chunked_map, "odom", ros::Time(42), map_msg))); + ASSERT_EQ(map_msg.layered_hashed_wavelet_octree.size(), 1u); + const auto& layered_msg = map_msg.layered_hashed_wavelet_octree.front(); + EXPECT_EQ(layered_msg.layer_names, + std::vector({"reflectivity"})); + EXPECT_EQ(layered_msg.layer_types, + std::vector({"float32"})); + EXPECT_FALSE(layered_msg.blocks.empty()); + + RegularLayeredDefinition::ContinuousMap::Ptr regular_map; + ASSERT_TRUE((convert::rosMsgToMap(map_msg, + regular_map))); + ASSERT_TRUE(regular_map); + const auto reconstructed = regular_map->getVoxelValue(cell_index); + EXPECT_NEAR(reconstructed.occupancy, voxel.occupancy, 1e-5f); + EXPECT_NEAR(reconstructed.data.get(), + voxel.data.get(), 1e-5f); +} + +TEST(LayeredMapFileConversionsTest, + LoadsBuiltInCodecsWithoutARegisteredRuntimeSchema) { + ros::Time::init(); + + GenericFileDefinition::Config config; + config.continuous_map.min_cell_width = 0.125f; + config.continuous_map.min_log_odds = -3.f; + config.continuous_map.max_log_odds = 5.f; + config.continuous_map.tree_height = 3; + config.discrete_compression.block_height = 2; + GenericFileDefinition::Map map(config); + + const Index3D first_index{1, 2, 3}; + const Index3D second_index{2, 2, 3}; + GenericFileDefinition::Voxel voxel; + voxel.occupancy = 0.8f; + voxel.data.get() = 0.35f; + voxel.data.get() = {0.2f, 0.4f, 0.6f}; + voxel.data.get() = {2.4f, 3.f}; + map.continuousMap().setVoxelValue(first_index, voxel); + map.discreteLayers().get().setValue(first_index, 4); + map.discreteLayers().get().setValue(second_index, 7); + map.discreteLayers().get().setValue(first_index, true); + map.discreteLayers().get().setValue(second_index, false); + + const auto file_path = std::filesystem::temp_directory_path() / + "wavemap_runtime_schema_conversion_test.lwvmp"; + ASSERT_TRUE(GenericFileDefinition::MapIo::save(file_path, map)); + + const ros::Time stamp(42); + wavemap_msgs::LayeredMap expected; + ASSERT_TRUE((convert::layeredMapToRosMsg( + map, "test_frame", stamp, expected))); + wavemap_msgs::LayeredMap actual; + std::string error_message; + const bool loaded = convert::layeredMapFileToRosMsg( + file_path, "test_frame", stamp, actual, &error_message); + std::filesystem::remove(file_path); + ASSERT_TRUE(loaded) << error_message; + + ASSERT_EQ(actual.continuous_map.layered_hashed_wavelet_octree.size(), 1u); + ASSERT_EQ(expected.continuous_map.layered_hashed_wavelet_octree.size(), 1u); + const auto& actual_continuous = + actual.continuous_map.layered_hashed_wavelet_octree.front(); + const auto& expected_continuous = + expected.continuous_map.layered_hashed_wavelet_octree.front(); + EXPECT_EQ(actual.header, expected.header); + EXPECT_TRUE(actual.is_full_update); + EXPECT_FLOAT_EQ(actual_continuous.min_cell_width, + expected_continuous.min_cell_width); + EXPECT_FLOAT_EQ(actual_continuous.min_log_odds, + expected_continuous.min_log_odds); + EXPECT_FLOAT_EQ(actual_continuous.max_log_odds, + expected_continuous.max_log_odds); + EXPECT_EQ(actual_continuous.tree_height, expected_continuous.tree_height); + EXPECT_EQ(actual_continuous.layer_names, expected_continuous.layer_names); + EXPECT_EQ(actual_continuous.layer_types, expected_continuous.layer_types); + EXPECT_EQ(actual_continuous.allocated_block_indices, + expected_continuous.allocated_block_indices); + EXPECT_EQ(actual_continuous.blocks, expected_continuous.blocks); + EXPECT_EQ(actual.discrete_layers, expected.discrete_layers); +} } // namespace wavemap diff --git a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt index d316cf978..baf6d395e 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt +++ b/interfaces/ros1/wavemap_rviz_plugin/CMakeLists.txt @@ -7,14 +7,17 @@ project(wavemap_rviz_plugin) # Dependencies find_package(catkin REQUIRED - COMPONENTS rviz wavemap wavemap_msgs wavemap_ros_conversions) + COMPONENTS pluginlib rviz wavemap wavemap_msgs wavemap_ros_conversions) # Setup catkin package catkin_package(CATKIN_DEPENDS - wavemap wavemap_msgs wavemap_ros_conversions) + pluginlib wavemap wavemap_msgs wavemap_ros_conversions) # For all targets -include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}) +include_directories( + include + ${CMAKE_CURRENT_SOURCE_DIR}/../wavemap_ros/app/ouster_layered + SYSTEM ${catkin_INCLUDE_DIRS}) # This setting causes Qt's "MOC" generation to happen automatically set(CMAKE_AUTOMOC ON) @@ -22,6 +25,7 @@ set(CMAKE_AUTOMOC ON) # This plugin includes Qt widgets, so we must include Qt. # We'll use the version that rviz used so they are compatible. set(HEADERS_TO_MOC + include/wavemap_rviz_plugin/layered_map_display.h include/wavemap_rviz_plugin/wavemap_map_display.h include/wavemap_rviz_plugin/visuals/voxel_visual.h include/wavemap_rviz_plugin/visuals/slice_visual.h @@ -52,6 +56,8 @@ add_library(${PROJECT_NAME} src/visuals/voxel_visual.cc src/visuals/cell_layer.cc src/visuals/cell_selector.cc + src/layered_map_display.cc + src/layered_map_factories.cc src/wavemap_map_display.cc ${QT_MOC}) set_wavemap_target_properties(${PROJECT_NAME}) diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h index 2a9093a3b..3840906da 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/common.h @@ -2,12 +2,24 @@ #define WAVEMAP_RVIZ_PLUGIN_COMMON_H_ #include +#include +#include +#include #include +#include +#include + +#include "wavemap_rviz_plugin/layered_map_interface.h" namespace wavemap::rviz_plugin { struct MapAndMutex { MapBase::Ptr map; + std::shared_ptr layered_map; + std::optional layered_map_msg; + std::optional discrete_layer_msg; + FloatingPoint discrete_layer_min_cell_width = 0.1f; + std::string selected_layer_name = "occupancy"; std::mutex mutex; }; } // namespace wavemap::rviz_plugin diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h new file mode 100644 index 000000000..2b608ebd8 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_display.h @@ -0,0 +1,136 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_DISPLAY_H_ +#define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_DISPLAY_H_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap_rviz_plugin/common.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" +#include "wavemap_rviz_plugin/utils/button_property.h" +#include "wavemap_rviz_plugin/visuals/voxel_visual.h" +#endif + +namespace Ogre { +class OverlayElement; +class TextAreaOverlayElement; +} + +namespace wavemap::rviz_plugin { +struct LayeredMapSourceMode : public TypeSelector { + using TypeSelector::TypeSelector; + + enum Id : TypeId { kFromTopic, kFromFile }; + + static constexpr std::array names = {"Topic", "File"}; +}; + +class LayeredMapDisplay + : public rviz::MessageFilterDisplay { + Q_OBJECT + public: // NOLINT + LayeredMapDisplay(); + ~LayeredMapDisplay() override; + + protected: + void onInitialize() override; + void reset() override; + + private Q_SLOTS: // NOLINT + void updateSourceModeCallback(); + void loadMapFromDiskCallback(); + void updateLayerSelectionCallback(); + void updateLegendVisibilityCallback(); + + private: + struct DisplayLayer { + enum class Source { kContinuous, kDiscrete }; + + std::string name; + std::string type; + Source source = Source::kContinuous; + std::vector min_values; + std::vector max_values; + }; + + std::vector available_layers_; + LayeredMapSourceMode source_mode_ = LayeredMapSourceMode::kFromTopic; + std::string selected_layer_name_ = "occupancy"; + int continuous_termination_height_ = 0; + int discrete_termination_height_ = 1; + std::optional latest_msg_; + std::unique_ptr> + layered_map_factory_loader_; + std::vector> layered_map_factories_; + + const std::shared_ptr map_and_mutex_ = + std::make_shared(); + std::unique_ptr voxel_visual_; + + rviz::EnumProperty source_mode_property_{"Source", "", + "Where to load the layered map from.", + this, + SLOT(updateSourceModeCallback())}; + ButtonProperty load_map_from_disk_property_{ + "Loaded map", + "Choose file", + "Open a file dialog to choose and load a layered map from disk.", + this, + SLOT(loadMapFromDiskCallback()), + this}; + rviz::Property voxel_visual_properties_{"Render voxels", QVariant(), + "Properties for voxel rendering.", + this}; + rviz::EnumProperty layer_property_{ + "Layer", "occupancy", + "Layer to display. Populated from the received LayeredMap message.", + &voxel_visual_properties_, SLOT(updateLayerSelectionCallback()), this}; + rviz::BoolProperty show_legend_property_{ + "Show Legend", true, "Whether to show the selected layer legend.", + this, SLOT(updateLegendVisibilityCallback()), this}; + void processMessage(const wavemap_msgs::LayeredMap::ConstPtr& msg) override; + bool loadMapFromDisk(const std::filesystem::path& filepath, + std::string* error_message = nullptr); + void displayMessage(const wavemap_msgs::LayeredMap& msg); + void clearStoredMap(); + void updateAvailableLayers(const wavemap_msgs::LayeredMap& msg); + void updateStoredMapForSelectedLayer(const wavemap_msgs::LayeredMap& msg); + void configureSelectedLayerAppearance( + const wavemap_msgs::LayeredMap& msg); + void loadLayeredMapFactories(); + void updateLegendOverlay(const wavemap_msgs::LayeredMap* msg = nullptr); + void initializeLegendOverlay(); + void destroyLegendOverlay(); + void clearLegendOverlayElements(); + std::string makeLegendMaterial(const Ogre::ColourValue& color, + const std::string& name_hint); + void addLegendText(const std::string& text, float x, float y, + float char_height, const Ogre::ColourValue& color); + void addLegendRow(const Ogre::ColourValue& color, const std::string& text, + float y); + const DisplayLayer* selectedLayer() const; + + Ogre::Overlay* legend_overlay_ = nullptr; + Ogre::OverlayContainer* legend_panel_ = nullptr; + std::vector legend_elements_; + std::vector legend_material_names_; + std::vector legend_texture_names_; + size_t legend_element_counter_ = 0u; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_DISPLAY_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h new file mode 100644 index 000000000..64ec1517d --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_factory.h @@ -0,0 +1,108 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ +#define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "wavemap_rviz_plugin/layered_map_interface.h" + +namespace wavemap::rviz_plugin { +inline std::vector extractLayerMetadata( + const wavemap_msgs::LayeredHashedWaveletOctree& msg) { + std::vector layers; + const size_t num_layers = + std::min(msg.layer_names.size(), msg.layer_types.size()); + layers.reserve(num_layers); + for (size_t layer_idx = 0u; layer_idx < num_layers; ++layer_idx) { + LayerMetadata metadata{msg.layer_names[layer_idx], + msg.layer_types[layer_idx], {}, {}}; + if (layer_idx < msg.layer_min_values.size()) { + metadata.min_values = msg.layer_min_values[layer_idx].float32_values; + } + if (layer_idx < msg.layer_max_values.size()) { + metadata.max_values = msg.layer_max_values[layer_idx].float32_values; + } + layers.emplace_back(std::move(metadata)); + } + return layers; +} + +// Runtime factory interface. The display owns factories through this base class, +// while each concrete factory knows how to rebuild one specific user voxel type. +class LayeredMapFactory { + public: + virtual ~LayeredMapFactory() = default; + + virtual std::shared_ptr tryCreate( + const wavemap_msgs::LayeredHashedWaveletOctree& msg) const = 0; + + virtual bool tryLoad(const std::filesystem::path& /*filepath*/, + const std::string& /*frame_id*/, + wavemap_msgs::LayeredMap& /*msg*/, + std::string* /*error_message*/) const { + return false; + } +}; + +template +class TypedLayeredMapFactory : public LayeredMapFactory { + public: + std::shared_ptr tryCreate( + const wavemap_msgs::LayeredHashedWaveletOctree& msg) const override { + // Never invoke a typed converter on a different schema. Besides producing + // invalid values, converters commonly index their expected layer arrays + // directly, so a mismatch can otherwise become an out-of-bounds access. + if (msg.layer_names != CellDataRosConverterT::layerNames() || + msg.layer_types != CellDataRosConverterT::layerTypes()) { + return nullptr; + } + typename LayeredMapT::Ptr map; + convert::rosMsgToMap(msg, map); + if (!map) { + ROS_WARN("Failed to create typed layered map for RViz display."); + return nullptr; + } + + return std::make_shared< + HashedWaveletOctreeLayeredMapAdapter>( + map, extractLayerMetadata(msg)); + } +}; + +// Adds file loading to a typed topic factory. A user registering a new schema +// only supplies their map, existing IO codec, and config factory; ROS message +// construction remains shared infrastructure. +template +class TypedLayeredMapFileFactory + : public TypedLayeredMapFactory { + public: + bool tryLoad(const std::filesystem::path& filepath, + const std::string& frame_id, wavemap_msgs::LayeredMap& msg, + std::string* error_message) const override { + FullLayeredMapT map(MakeConfigT()); + if (!LayeredMapIoT::load(filepath, map, error_message)) { + return false; + } + return convert::layeredMapToRosMsg( + map, frame_id, ros::Time(0), msg); + } +}; + +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_FACTORY_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h new file mode 100644 index 000000000..29f930cd3 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/layered_map_interface.h @@ -0,0 +1,149 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ +#define WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "wavemap_rviz_plugin/occupancy_query.h" + +namespace wavemap::rviz_plugin { +struct LayerMetadata { + std::string name; + std::string type; + std::vector min_values; + std::vector max_values; +}; + +// Runtime interface used by the RViz plugin for layered maps. Concrete user +// voxel types remain hidden behind this API, keeping the display code modular. +class LayeredMapInterface : public OccupancyQuery { + public: + using IndexedOccupancyVisitor = + std::function; + + ~LayeredMapInterface() override = default; + + virtual IndexElement getTreeHeight() const = 0; + virtual FloatingPoint getMinCellWidth() const = 0; + virtual const std::vector& getLayers() const = 0; + + virtual std::vector getBlockIndices() const = 0; + virtual bool hasBlock(const Index3D& block_index) const = 0; + virtual void forEachLeaf(const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const = 0; + virtual void forEachBlockLeaf(const Index3D& block_index, + const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const = 0; + virtual bool getLayerColor(const std::string& layer_name, + const OctreeIndex& index, + FloatingPoint occupancy, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + Ogre::ColourValue& color) const = 0; +}; + +struct NoLayerColorProvider { + template + static bool getLayerColor(const std::string& /*layer_name*/, + const CellDataT& /*voxel*/, + FloatingPoint /*occupancy*/, + FloatingPoint /*scalar_display_min*/, + FloatingPoint /*scalar_display_max*/, + const Ogre::ColourValue& /*scalar_low_color*/, + const Ogre::ColourValue& /*scalar_high_color*/, + const LayerMetadata& /*metadata*/, + Ogre::ColourValue& /*color*/) { + return false; + } +}; + +template +class HashedWaveletOctreeLayeredMapAdapter : public LayeredMapInterface { + public: + HashedWaveletOctreeLayeredMapAdapter( + std::shared_ptr map, + std::vector layers) + : map_(std::move(map)), layers_(std::move(layers)) {} + + IndexElement getTreeHeight() const override { return map_->getTreeHeight(); } + + FloatingPoint getMinCellWidth() const override { + return map_->getMinCellWidth(); + } + + const std::vector& getLayers() const override { + return layers_; + } + + FloatingPoint getCellValue(const OctreeIndex& index) override { + return map_->getCellValue(index); + } + + std::vector getBlockIndices() const override { + std::vector block_indices; + block_indices.reserve(map_->getHashMap().size()); + map_->forEachBlock([&block_indices](const Index3D& block_index, + const auto& /*block*/) { + block_indices.push_back(block_index); + }); + return block_indices; + } + + bool hasBlock(const Index3D& block_index) const override { + return map_->hasBlock(block_index); + } + + void forEachLeaf(const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const override { + map_->forEachBlock([&visitor_fn, termination_height]( + const Index3D& block_index, const auto& block) { + block.forEachLeaf(block_index, visitor_fn, termination_height); + }); + } + + void forEachBlockLeaf(const Index3D& block_index, + const IndexedOccupancyVisitor& visitor_fn, + IndexElement termination_height = 0) const override { + if (const auto* block = map_->getBlock(block_index); block) { + block->forEachLeaf(block_index, visitor_fn, termination_height); + } + } + + bool getLayerColor(const std::string& layer_name, const OctreeIndex& index, + FloatingPoint occupancy, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + Ogre::ColourValue& color) const override { + const auto voxel = map_->getVoxelValue(index); + const auto metadata_it = + std::find_if(layers_.begin(), layers_.end(), + [&](const LayerMetadata& layer) { + return layer.name == layer_name; + }); + if (metadata_it == layers_.end()) { + return false; + } + return LayerColorProviderT::getLayerColor( + layer_name, voxel, occupancy, scalar_display_min, scalar_display_max, + scalar_low_color, scalar_high_color, *metadata_it, color); + } + + private: + std::shared_ptr map_; + std::vector layers_; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_LAYERED_MAP_INTERFACE_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h new file mode 100644 index 000000000..bbe7c78b0 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/occupancy_query.h @@ -0,0 +1,38 @@ +#ifndef WAVEMAP_RVIZ_PLUGIN_OCCUPANCY_QUERY_H_ +#define WAVEMAP_RVIZ_PLUGIN_OCCUPANCY_QUERY_H_ + +#include + +#include +#include +#include +#include + +namespace wavemap::rviz_plugin { +// Small runtime interface for map occupancy queries. The cell selector only +// needs this API, so it does not need to know whether the backing map is a +// legacy wavemap map or a future layered map adapter. +class OccupancyQuery { + public: + virtual ~OccupancyQuery() = default; + + virtual FloatingPoint getCellValue(const OctreeIndex& index) = 0; +}; + +class HashedWaveletOctreeOccupancyQuery : public OccupancyQuery { + public: + explicit HashedWaveletOctreeOccupancyQuery( + std::shared_ptr map) + : map_(std::move(map)), query_accelerator_(*map_) {} + + FloatingPoint getCellValue(const OctreeIndex& index) override { + return query_accelerator_.getCellValue(index); + } + + private: + std::shared_ptr map_; + QueryAccelerator query_accelerator_; +}; +} // namespace wavemap::rviz_plugin + +#endif // WAVEMAP_RVIZ_PLUGIN_OCCUPANCY_QUERY_H_ diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h index 41ca0d023..02a5771c8 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/cell_selector.h @@ -7,7 +7,8 @@ #include #include #include -#include + +#include "wavemap_rviz_plugin/occupancy_query.h" #endif namespace wavemap::rviz_plugin { @@ -28,6 +29,8 @@ class CellSelector : public QObject { void initializePropertyMenu(); void setMap(const MapBase::ConstPtr& map); + void setOccupancyQuery(std::shared_ptr occupancy_query); + void clearOccupancyQuery(); bool shouldBeDrawn(const OctreeIndex& cell_index, FloatingPoint cell_log_odds) const; @@ -44,8 +47,7 @@ class CellSelector : public QObject { private: std::function redraw_map_; - mutable std::optional> - query_accelerator_; + std::shared_ptr occupancy_query_; // Selection mode and thresholds CellSelectionMode cell_selection_mode_ = CellSelectionMode::kSurface; diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h index 863b9fa57..030e6026b 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/visuals/voxel_visual.h @@ -2,15 +2,25 @@ #define WAVEMAP_RVIZ_PLUGIN_VISUALS_VOXEL_VISUAL_H_ #ifndef Q_MOC_RUN +#include +#include +#include #include +#include #include +#include #include +#include +#include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -48,14 +58,47 @@ class VoxelVisual : public QObject { VoxelVisual(Ogre::SceneManager* scene_manager, rviz::ViewManager* view_manager, Ogre::SceneNode* parent_node, rviz::Property* submenu_root_property, - std::shared_ptr map_and_mutex); + std::shared_ptr map_and_mutex, + std::function layer_color_changed_callback = {}); // Destructor. Removes the visual elements from the scene. ~VoxelVisual() override; + void applyDiscreteLayerPatch( + const wavemap_msgs::DiscreteLayer& patch, + FloatingPoint min_cell_width); + void updateMap(bool redraw_all = false); - void clear() { block_voxel_layers_map_.clear(); } + void clear(); + + int terminationHeight() const { + return termination_height_property_.getInt(); + } + void setTerminationHeight(int max_height, int height) { + termination_height_property_.setMax(max_height); + termination_height_property_.setInt(std::clamp(height, 0, max_height)); + } + FloatingPoint scalarDisplayMin() const { + return scalar_min_property_.getFloat(); + } + FloatingPoint scalarDisplayMax() const { + return scalar_max_property_.getFloat(); + } + Ogre::ColourValue scalarLowColor() const { return scalar_low_color_; } + VoxelColorMode colorMode() const { return voxel_color_mode_; } + Ogre::ColourValue flatColor() const { return voxel_flat_color_; } + Ogre::ColourValue scalarHighColor() const { return scalar_high_color_; } + void configureLayerAppearance( + const std::string& layer_name, const std::string& layer_type, + bool is_discrete, FloatingPoint scalar_min, FloatingPoint scalar_max, + bool has_low_color, const Ogre::ColourValue& low_color, + bool has_high_color, const Ogre::ColourValue& high_color, + const std::vector& categories = {}); + Ogre::ColourValue categoryColor(int value) const; + bool categoryVisible(int value) const; + Ogre::ColourValue boolColor(bool value) const; + bool boolVisible(bool value) const; // Set the pose of the coordinate frame the message refers to void setFramePosition(const Ogre::Vector3& position); @@ -69,13 +112,24 @@ class VoxelVisual : public QObject { void opacityUpdateCallback(); void colorModeUpdateCallback(); void flatColorUpdateCallback(); + void layerColorUpdateCallback(); private: VoxelColorMode voxel_color_mode_ = VoxelColorMode::kHeight; Ogre::ColourValue voxel_flat_color_ = Ogre::ColourValue::Blue; + Ogre::ColourValue scalar_low_color_ = Ogre::ColourValue(0.f, 0.f, 1.f); + Ogre::ColourValue scalar_high_color_ = Ogre::ColourValue(1.f, 1.f, 0.f); + Ogre::ColourValue bool_true_color_ = Ogre::ColourValue(1.f, 0.05f, 0.05f); + Ogre::ColourValue bool_false_color_ = + Ogre::ColourValue(0.35f, 0.35f, 0.35f); + + std::string selected_color_layer_; + std::unordered_map category_colors_; + static Ogre::ColourValue defaultCategoryColor(int value); // Shared pointer to the map, owned by WavemapMapDisplay const std::shared_ptr map_and_mutex_; + const std::function layer_color_changed_callback_; // The SceneManager, kept here only so the destructor can ask it to // destroy the `frame_node_`. @@ -95,6 +149,8 @@ class VoxelVisual : public QObject { rviz::FloatProperty opacity_property_; rviz::EnumProperty color_mode_property_; rviz::ColorProperty flat_color_property_; + rviz::FloatProperty scalar_min_property_; + rviz::FloatProperty scalar_max_property_; // Frame-rate stats rviz::Property frame_rate_properties_; rviz::IntProperty num_queued_blocks_indicator_; @@ -103,6 +159,7 @@ class VoxelVisual : public QObject { // The objects implementing the voxel visuals using VoxelLayers = std::vector>; std::unordered_map block_voxel_layers_map_; + void detachVoxelLayers(VoxelLayers& voxel_layers); // Material handling Ogre::MaterialPtr voxel_material_; @@ -128,12 +185,47 @@ class VoxelVisual : public QObject { const OctreeIndex& cell_index, FloatingPoint cell_log_odds, VoxelsPerLevel& voxels_per_level); + void appendLayeredLeafCenterAndColor(const LayeredMapInterface& layered_map, + const std::string& selected_layer_name, + int tree_height, + FloatingPoint min_cell_width, + const OctreeIndex& cell_index, + FloatingPoint cell_log_odds, + VoxelsPerLevel& voxels_per_level); void drawMultiResolutionVoxels(IndexElement tree_height, FloatingPoint min_cell_width, const Index3D& block_index, FloatingPoint alpha, VoxelsPerLevel& voxels_per_level, VoxelLayers& voxel_layer_visuals); + void drawLayeredMapOccupancy(const LayeredMapInterface& layered_map); + void drawLayeredMapOccupancy( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg); + bool getGenericContinuousLayerColor( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, + const std::vector>& layer_values, + Ogre::ColourValue& color) const; + void drawDiscreteLayer(const wavemap_msgs::DiscreteLayer& discrete_layer_msg, + FloatingPoint min_cell_width); + void rebuildDiscreteBatches( + const std::unordered_set& batch_indices, + FloatingPoint min_cell_width); + static Index3D discreteRenderBatchIndex(const Index3D& parent_index); + + std::optional discrete_layer_metadata_; + std::unordered_map + discrete_parent_cells_; + std::unordered_map, Index3DHash> + discrete_batch_parents_; + bool getDiscreteCellColor(const wavemap_msgs::DiscreteLayer& layer_msg, + const wavemap_msgs::DiscreteLayerCell& cell_msg, + int exception_index, bool is_exception, + Ogre::ColourValue& color) const; + void appendLayeredBlockOccupancy( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, + const wavemap_msgs::LayeredHashedWaveletOctreeBlock& block_msg, + IndexElement termination_height, VoxelsPerLevel& voxels_per_level); // Block update queue // NOTE: Instead of performing all the block updates at once whenever the map diff --git a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h index 43da57d65..4eb826d04 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h +++ b/interfaces/ros1/wavemap_rviz_plugin/include/wavemap_rviz_plugin/wavemap_map_display.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -51,9 +52,12 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { void requestWavemapServerResetCallback(); void requestWholeMapCallback(); void loadMapFromDiskCallback(); + void updateLayerSelectionCallback(); private: SourceMode source_mode_ = SourceMode::kFromTopic; + std::vector available_layers_; + std::string selected_layer_name_ = "occupancy"; bool hasMap(); void clearMap(); @@ -67,6 +71,7 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { const std::shared_ptr map_and_mutex_ = std::make_shared(); void updateMapFromRosMsg(const wavemap_msgs::Map& map_msg); + void updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& map_msg); // Submenus for each visual's properties rviz::EnumProperty source_mode_property_{"Source", "", @@ -90,6 +95,10 @@ class WavemapMapDisplay : public rviz::MessageFilterDisplay { rviz::Property voxel_visual_properties_{ "Render voxels", QVariant(), "Properties for the voxel-based visualization.", this}; + rviz::EnumProperty layer_property_{ + "Layer", "occupancy", + "Layer to use for voxel coloring when a layered map is received.", + &voxel_visual_properties_, SLOT(updateLayerSelectionCallback()), this}; rviz::Property slice_visual_properties_{ "Render slice", QVariant(), "Properties for the slice visualization.", this}; diff --git a/interfaces/ros1/wavemap_rviz_plugin/package.xml b/interfaces/ros1/wavemap_rviz_plugin/package.xml index 6504135cf..8063ef7fb 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/package.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/package.xml @@ -17,11 +17,13 @@ wavemap wavemap_msgs wavemap_ros_conversions + pluginlib rviz qtbase5-dev + diff --git a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml index b4df01c54..6c0d88762 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml +++ b/interfaces/ros1/wavemap_rviz_plugin/plugin_description.xml @@ -7,4 +7,37 @@ wavemap_msgs/Map + + + Displays layered maps with continuous and discrete voxel layers. + + wavemap_msgs/LayeredMap + + + Schema handler for layered occupancy-only maps. + + + Schema handler for occupancy and reflectivity maps. + + + Schema handler for occupancy, reflectivity, and class maps. + + + Schema handler for occupancy, reflectivity, signal, near-IR, and class maps with discrete block height 2. + + + Schema handler for occupancy and direct RGB color maps. + diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc new file mode 100644 index 000000000..574e643d4 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_display.cc @@ -0,0 +1,1014 @@ +#include "wavemap_rviz_plugin/layered_map_display.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wavemap_rviz_plugin/utils/alert_dialog.h" + +namespace wavemap::rviz_plugin { +namespace { +using IndexTuple = std::tuple; + +template +IndexTuple indexTuple(const IndexMsgT& index) { + return {index.x, index.y, index.z}; +} + +void mergeContinuousPatch(const wavemap_msgs::Map& patch, + wavemap_msgs::Map& stored) { + if (patch.layered_hashed_wavelet_octree.size() != 1u) { + return; + } + if (stored.layered_hashed_wavelet_octree.size() != 1u) { + stored = patch; + return; + } + const auto& patch_map = patch.layered_hashed_wavelet_octree.front(); + auto& stored_map = stored.layered_hashed_wavelet_octree.front(); + std::set allocated; + for (const auto& block_index : patch_map.allocated_block_indices) { + allocated.emplace(indexTuple(block_index)); + } + stored_map.allocated_block_indices = patch_map.allocated_block_indices; + stored_map.min_cell_width = patch_map.min_cell_width; + stored_map.min_log_odds = patch_map.min_log_odds; + stored_map.max_log_odds = patch_map.max_log_odds; + stored_map.tree_height = patch_map.tree_height; + stored_map.layer_names = patch_map.layer_names; + stored_map.layer_types = patch_map.layer_types; + stored_map.layer_min_values = patch_map.layer_min_values; + stored_map.layer_max_values = patch_map.layer_max_values; + stored_map.layer_visualizations = patch_map.layer_visualizations; + + stored_map.blocks.erase( + std::remove_if(stored_map.blocks.begin(), stored_map.blocks.end(), + [&](const auto& block) { + return allocated.find(indexTuple(block.root_node_offset)) == + allocated.end(); + }), + stored_map.blocks.end()); + for (const auto& patch_block : patch_map.blocks) { + const auto patch_index = indexTuple(patch_block.root_node_offset); + const auto stored_it = std::find_if( + stored_map.blocks.begin(), stored_map.blocks.end(), + [&](const auto& block) { + return indexTuple(block.root_node_offset) == patch_index; + }); + if (stored_it == stored_map.blocks.end()) { + stored_map.blocks.emplace_back(patch_block); + } else { + *stored_it = patch_block; + } + } + stored.header = patch.header; +} + +void mergeDiscretePatch(const wavemap_msgs::DiscreteLayer& patch, + wavemap_msgs::DiscreteLayer& stored) { + stored.name = patch.name; + stored.value_type = patch.value_type; + stored.block_height = patch.block_height; + stored.categories = patch.categories; + + std::set deleted_keys; + for (const auto& deleted_index : patch.deleted_parent_indices) { + deleted_keys.insert(indexTuple(deleted_index)); + } + if (!deleted_keys.empty()) { + stored.cells.erase( + std::remove_if(stored.cells.begin(), stored.cells.end(), + [&](const auto& cell) { + return deleted_keys.count( + indexTuple(cell.parent_index)) != 0u; + }), + stored.cells.end()); + } + + std::map stored_positions; + for (size_t index = 0u; index < stored.cells.size(); ++index) { + stored_positions[indexTuple(stored.cells[index].parent_index)] = index; + } + for (const auto& patch_cell : patch.cells) { + const auto key = indexTuple(patch_cell.parent_index); + const auto position_it = stored_positions.find(key); + if (position_it == stored_positions.end()) { + stored_positions[key] = stored.cells.size(); + stored.cells.emplace_back(patch_cell); + } else { + stored.cells[position_it->second] = patch_cell; + } + } + stored.deleted_parent_indices.clear(); +} + +void mergeLayeredPatch(const wavemap_msgs::LayeredMap& patch, + wavemap_msgs::LayeredMap& stored) { + stored.header = patch.header; + mergeContinuousPatch(patch.continuous_map, stored.continuous_map); + for (const auto& layer_patch : patch.discrete_layers) { + const auto stored_it = std::find_if( + stored.discrete_layers.begin(), stored.discrete_layers.end(), + [&](const auto& layer) { return layer.name == layer_patch.name; }); + if (stored_it == stored.discrete_layers.end()) { + stored.discrete_layers.emplace_back(layer_patch); + stored.discrete_layers.back().deleted_parent_indices.clear(); + } else { + mergeDiscretePatch(layer_patch, *stored_it); + } + } + stored.is_full_update = true; +} + +Ogre::ColourValue stableIntColor(int value) { + const uint32_t hash = static_cast(value) * 2654435761u; + const float r = 0.25f + + 0.75f * static_cast((hash >> 16u) & 0xffu) / 255.f; + const float g = 0.25f + + 0.75f * static_cast((hash >> 8u) & 0xffu) / 255.f; + const float b = 0.25f + 0.75f * static_cast(hash & 0xffu) / 255.f; + return Ogre::ColourValue(r, g, b, 1.f); +} + +std::set collectIntValues(const wavemap_msgs::DiscreteLayer& layer) { + std::set values; + for (const auto& cell : layer.cells) { + values.insert(cell.dominant_int32); + values.insert(cell.exception_int32_values.begin(), + cell.exception_int32_values.end()); + } + return values; +} + +std::set collectBoolValues(const wavemap_msgs::DiscreteLayer& layer) { + std::set values; + for (const auto& cell : layer.cells) { + values.insert(cell.dominant_uint8 != 0u); + for (const uint8_t exception_value : cell.exception_uint8_values) { + values.insert(exception_value != 0u); + } + } + return values; +} +} // namespace + +LayeredMapDisplay::LayeredMapDisplay() { + source_mode_property_.clearOptions(); + for (const auto& name : LayeredMapSourceMode::names) { + source_mode_property_.addOption(name); + } + source_mode_property_.setStringStd(source_mode_.toStr()); + load_map_from_disk_property_.setHidden(true); + + layer_property_.clearOptions(); + layer_property_.addOption(QString::fromStdString(selected_layer_name_)); + layer_property_.setStringStd(selected_layer_name_); +} + +LayeredMapDisplay::~LayeredMapDisplay() { + // Destroy Ogre visuals before Qt-owned RViz properties start tearing down. + voxel_visual_.reset(); + destroyLegendOverlay(); +} + +void LayeredMapDisplay::onInitialize() { + ProfilerZoneScoped; + MFDClass::onInitialize(); + + const int source_row = source_mode_property_.rowNumberInParent(); + const int topic_row = topic_property_->rowNumberInParent(); + if (0 <= topic_row && topic_row < source_row) { + moveChild(source_row, topic_row); + } + + if (topic_property_->getStdString().empty()) { + topic_property_->setString("/wavemap/layered_map"); + } + voxel_visual_ = std::make_unique( + scene_manager_, context_->getViewManager(), scene_node_, + &voxel_visual_properties_, map_and_mutex_, [this]() { + updateLegendOverlay(latest_msg_ ? &latest_msg_.value() : nullptr); + }); + loadLayeredMapFactories(); + initializeLegendOverlay(); +} + +void LayeredMapDisplay::loadLayeredMapFactories() { + layered_map_factories_.clear(); + try { + layered_map_factory_loader_ = std::make_unique< + pluginlib::ClassLoader>( + "wavemap_rviz_plugin", "wavemap::rviz_plugin::LayeredMapFactory"); + for (const std::string& class_name : + layered_map_factory_loader_->getDeclaredClasses()) { + try { + layered_map_factories_.emplace_back( + layered_map_factory_loader_->createInstance(class_name)); + } catch (const pluginlib::PluginlibException& exception) { + ROS_WARN_STREAM("Could not load layered map schema handler '" + << class_name << "': " << exception.what()); + } + } + } catch (const pluginlib::PluginlibException& exception) { + ROS_ERROR_STREAM("Could not initialize layered map schema handlers: " + << exception.what()); + } +} + +void LayeredMapDisplay::reset() { + ProfilerZoneScoped; + MFDClass::reset(); + latest_msg_.reset(); + clearStoredMap(); + if (voxel_visual_) { + voxel_visual_->clear(); + } + updateLegendOverlay(nullptr); +} + + +void LayeredMapDisplay::processMessage( + const wavemap_msgs::LayeredMap::ConstPtr& msg) { + ProfilerZoneScoped; + if (!msg) { + ROS_WARN("Ignoring request to process non-existent layered map msg."); + return; + } + + displayMessage(*msg); +} + +void LayeredMapDisplay::displayMessage( + const wavemap_msgs::LayeredMap& incoming_msg) { + const bool replace_all = incoming_msg.is_full_update || !latest_msg_; + if (replace_all) { + latest_msg_ = incoming_msg; + } else { + mergeLayeredPatch(incoming_msg, *latest_msg_); + } + const wavemap_msgs::LayeredMap& msg = *latest_msg_; + updateAvailableLayers(msg); + configureSelectedLayerAppearance(msg); + updateLegendOverlay(&msg); + const DisplayLayer* selected_layer = selectedLayer(); + const bool selected_is_discrete = + selected_layer && + selected_layer->source == DisplayLayer::Source::kDiscrete; + if (!selected_is_discrete || replace_all) { + updateStoredMapForSelectedLayer(msg); + } + + if (!voxel_visual_) { + ROS_WARN("Voxel visual not initialized yet, skipping layered map message."); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(msg.header.frame_id, + msg.header.stamp, + position, orientation)) { + ROS_WARN("Error transforming from frame '%s' to frame '%s'", + msg.header.frame_id.c_str(), qPrintable(fixed_frame_)); + return; + } + voxel_visual_->setFramePosition(position); + voxel_visual_->setFrameOrientation(orientation); + + if (selected_is_discrete && !replace_all) { + const auto patch_it = std::find_if( + incoming_msg.discrete_layers.begin(), + incoming_msg.discrete_layers.end(), + [this](const wavemap_msgs::DiscreteLayer& layer) { + return layer.name == selected_layer_name_; + }); + if (patch_it != incoming_msg.discrete_layers.end()) { + FloatingPoint min_cell_width = 0.1f; + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + min_cell_width = msg.continuous_map.layered_hashed_wavelet_octree + .front() + .min_cell_width; + } + voxel_visual_->applyDiscreteLayerPatch(*patch_it, min_cell_width); + } + return; + } + voxel_visual_->updateMap(true); +} + +void LayeredMapDisplay::configureSelectedLayerAppearance( + const wavemap_msgs::LayeredMap& msg) { + if (!voxel_visual_) { + return; + } + const DisplayLayer* layer = selectedLayer(); + if (!layer) { + return; + } + + bool has_low_color = false; + bool has_high_color = false; + Ogre::ColourValue low_color; + FloatingPoint scalar_min = voxel_visual_->scalarDisplayMin(); + FloatingPoint scalar_max = voxel_visual_->scalarDisplayMax(); + Ogre::ColourValue high_color; + std::vector categories; + + if (layer->source == DisplayLayer::Source::kContinuous && + !msg.continuous_map.layered_hashed_wavelet_octree.empty()) { + const auto& continuous = + msg.continuous_map.layered_hashed_wavelet_octree.front(); + const auto layer_name_it = + std::find(continuous.layer_names.begin(), continuous.layer_names.end(), + selected_layer_name_); + if (layer_name_it != continuous.layer_names.end()) { + const size_t layer_index = static_cast( + std::distance(continuous.layer_names.begin(), layer_name_it)); + if (layer_index < continuous.layer_min_values.size() && + layer_index < continuous.layer_max_values.size() && + !continuous.layer_min_values[layer_index].float32_values.empty() && + !continuous.layer_max_values[layer_index].float32_values.empty()) { + scalar_min = continuous.layer_min_values[layer_index].float32_values[0]; + scalar_max = continuous.layer_max_values[layer_index].float32_values[0]; + } + } + const auto visualization_it = std::find_if( + continuous.layer_visualizations.begin(), + continuous.layer_visualizations.end(), [&](const auto& visualization) { + return visualization.name == selected_layer_name_; + }); + if (visualization_it != continuous.layer_visualizations.end()) { + has_low_color = visualization_it->has_low_color; + has_high_color = visualization_it->has_high_color; + low_color = Ogre::ColourValue(visualization_it->low_r, + visualization_it->low_g, + visualization_it->low_b, 1.f); + high_color = Ogre::ColourValue(visualization_it->high_r, + visualization_it->high_g, + visualization_it->high_b, 1.f); + } + } else if (layer->source == DisplayLayer::Source::kDiscrete) { + const auto layer_it = std::find_if( + msg.discrete_layers.begin(), msg.discrete_layers.end(), + [this](const auto& discrete_layer) { + return discrete_layer.name == selected_layer_name_; + }); + if (layer_it != msg.discrete_layers.end()) { + categories = layer_it->categories; + } + } + + voxel_visual_->configureLayerAppearance( + layer->name, layer->type, + layer->source == DisplayLayer::Source::kDiscrete, + scalar_min, scalar_max, has_low_color, low_color, + has_high_color, high_color, categories); +} + + +bool LayeredMapDisplay::loadMapFromDisk( + const std::filesystem::path& filepath, std::string* error_message) { + const std::string frame_id = + fixed_frame_.isEmpty() ? "map" : fixed_frame_.toStdString(); + for (const auto& factory : layered_map_factories_) { + wavemap_msgs::LayeredMap msg; + std::string factory_error; + if (factory->tryLoad(filepath, frame_id, msg, &factory_error)) { + displayMessage(msg); + return true; + } + if (error_message && !factory_error.empty()) { + *error_message = std::move(factory_error); + } + } + + wavemap_msgs::LayeredMap msg; + std::string generic_error; + if (convert::layeredMapFileToRosMsg(filepath, frame_id, ros::Time(0), msg, + &generic_error)) { + displayMessage(msg); + return true; + } + if (error_message) { + *error_message = std::move(generic_error); + } + return false; +} + +void LayeredMapDisplay::clearStoredMap() { + updateLegendOverlay(nullptr); + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->map.reset(); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); + map_and_mutex_->discrete_layer_msg.reset(); +} + +void LayeredMapDisplay::updateAvailableLayers( + const wavemap_msgs::LayeredMap& msg) { + std::vector new_layers; + new_layers.push_back( + {"occupancy", "float32", DisplayLayer::Source::kContinuous, {}, {}}); + + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + const auto& continuous_msg = + msg.continuous_map.layered_hashed_wavelet_octree.front(); + const size_t num_layers = std::min(continuous_msg.layer_names.size(), + continuous_msg.layer_types.size()); + for (size_t layer_i = 0u; layer_i < num_layers; ++layer_i) { + DisplayLayer layer{continuous_msg.layer_names[layer_i], + continuous_msg.layer_types[layer_i], + DisplayLayer::Source::kContinuous, {}, {}}; + if (layer_i < continuous_msg.layer_min_values.size()) { + layer.min_values = + continuous_msg.layer_min_values[layer_i].float32_values; + } + if (layer_i < continuous_msg.layer_max_values.size()) { + layer.max_values = + continuous_msg.layer_max_values[layer_i].float32_values; + } + new_layers.emplace_back(std::move(layer)); + } + } + + for (const auto& discrete_layer : msg.discrete_layers) { + new_layers.push_back({discrete_layer.name, discrete_layer.value_type, + DisplayLayer::Source::kDiscrete, {}, {}}); + } + + const bool selected_still_exists = + std::any_of(new_layers.begin(), new_layers.end(), + [this](const DisplayLayer& layer) { + return layer.name == selected_layer_name_; + }); + if (!selected_still_exists) { + selected_layer_name_ = + new_layers.empty() ? "occupancy" : new_layers.front().name; + } + + const auto sameLayer = [](const DisplayLayer& lhs, const DisplayLayer& rhs) { + return lhs.name == rhs.name && lhs.type == rhs.type && + lhs.source == rhs.source && lhs.min_values == rhs.min_values && + lhs.max_values == rhs.max_values; + }; + const bool schema_unchanged = + available_layers_.size() == new_layers.size() && + std::equal(available_layers_.begin(), available_layers_.end(), + new_layers.begin(), sameLayer); + available_layers_ = std::move(new_layers); + if (schema_unchanged) { + return; + } + + // Rebuilding an EnumProperty emits selection-change signals. Block them + // here because displayMessage() performs exactly one map update below. + const QSignalBlocker signal_blocker(&layer_property_); + layer_property_.clearOptions(); + for (const DisplayLayer& layer : available_layers_) { + layer_property_.addOption(QString::fromStdString(layer.name)); + } + layer_property_.setStringStd(selected_layer_name_); +} + +const LayeredMapDisplay::DisplayLayer* LayeredMapDisplay::selectedLayer() + const { + const auto layer_it = std::find_if( + available_layers_.begin(), available_layers_.end(), + [this](const DisplayLayer& layer) { return layer.name == selected_layer_name_; }); + return layer_it == available_layers_.end() ? nullptr : &*layer_it; +} + +void LayeredMapDisplay::updateStoredMapForSelectedLayer( + const wavemap_msgs::LayeredMap& msg) { + const DisplayLayer* layer = selectedLayer(); + const bool selected_is_discrete = + layer && layer->source == DisplayLayer::Source::kDiscrete; + + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->map.reset(); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); + map_and_mutex_->discrete_layer_msg.reset(); + map_and_mutex_->selected_layer_name = selected_layer_name_; + + if (selected_is_discrete) { + const auto discrete_it = std::find_if( + msg.discrete_layers.begin(), msg.discrete_layers.end(), + [this](const wavemap_msgs::DiscreteLayer& discrete_layer) { + return discrete_layer.name == selected_layer_name_; + }); + if (discrete_it != msg.discrete_layers.end()) { + map_and_mutex_->discrete_layer_msg = *discrete_it; + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + map_and_mutex_->discrete_layer_min_cell_width = + msg.continuous_map.layered_hashed_wavelet_octree.front().min_cell_width; + } + } + return; + } + + if (msg.continuous_map.layered_hashed_wavelet_octree.size() == 1u) { + const auto& continuous_msg = + msg.continuous_map.layered_hashed_wavelet_octree.front(); + for (const auto& factory : layered_map_factories_) { + if (auto typed_map = factory->tryCreate(continuous_msg)) { + map_and_mutex_->layered_map = std::move(typed_map); + return; + } + } + + map_and_mutex_->layered_map_msg = continuous_msg; + return; + } + + MapBase::Ptr map; + if (!convert::rosMsgToMap(msg.continuous_map, map)) { + ROS_WARN("Failed to parse continuous map inside LayeredMap message."); + return; + } + map_and_mutex_->map = std::move(map); +} + +void LayeredMapDisplay::initializeLegendOverlay() { + if (legend_overlay_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + const std::string name_suffix = std::to_string(reinterpret_cast(this)); + legend_overlay_ = overlay_manager.create("LayeredMapLegendOverlay_" + name_suffix); + legend_overlay_->setZOrder(650); + + legend_panel_ = static_cast( + overlay_manager.createOverlayElement( + "Panel", "LayeredMapLegendPanel_" + name_suffix)); + legend_panel_->setMetricsMode(Ogre::GMM_PIXELS); + legend_panel_->setHorizontalAlignment(Ogre::GHA_RIGHT); + legend_panel_->setVerticalAlignment(Ogre::GVA_TOP); + legend_panel_->setPosition(-430.f, 18.f); + legend_panel_->setDimensions(410.f, 150.f); + legend_panel_->setMaterialName(makeLegendMaterial( + Ogre::ColourValue(0.92f, 0.94f, 0.96f, 0.88f), "background")); + + legend_overlay_->add2D(legend_panel_); + legend_overlay_->show(); + updateLegendOverlay(nullptr); +} + +void LayeredMapDisplay::clearLegendOverlayElements() { + if (!legend_panel_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + for (Ogre::OverlayElement* element : legend_elements_) { + if (!element) { + continue; + } + legend_panel_->removeChild(element->getName()); + overlay_manager.destroyOverlayElement(element); + } + legend_elements_.clear(); +} + +void LayeredMapDisplay::destroyLegendOverlay() { + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + clearLegendOverlayElements(); + + if (legend_overlay_ && legend_panel_) { + legend_overlay_->remove2D(legend_panel_); + overlay_manager.destroyOverlayElement(legend_panel_); + legend_panel_ = nullptr; + } + + if (legend_overlay_) { + overlay_manager.destroy(legend_overlay_); + legend_overlay_ = nullptr; + } + + auto& material_manager = Ogre::MaterialManager::getSingleton(); + for (const std::string& material_name : legend_material_names_) { + if (material_manager.resourceExists(material_name)) { + material_manager.remove(material_name); + } + } + legend_material_names_.clear(); + + auto& texture_manager = Ogre::TextureManager::getSingleton(); + for (const std::string& texture_name : legend_texture_names_) { + if (texture_manager.resourceExists(texture_name)) { + texture_manager.remove(texture_name); + } + } + legend_texture_names_.clear(); +} + +std::string LayeredMapDisplay::makeLegendMaterial( + const Ogre::ColourValue& color, const std::string& name_hint) { + std::ostringstream material_name; + material_name << "LayeredMapLegend_" << name_hint << "_" + << reinterpret_cast(this) << "_" + << legend_element_counter_++; + + Ogre::TexturePtr texture; + std::string texture_name; + if (name_hint != "background") { + std::ostringstream texture_name_stream; + texture_name_stream << "LayeredMapLegendTexture_" << name_hint << "_" + << reinterpret_cast(this) << "_" + << legend_element_counter_++; + texture_name = texture_name_stream.str(); + texture = Ogre::TextureManager::getSingleton().createManual( + texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, 1u, 1u, 0u, Ogre::PF_A8R8G8B8, + Ogre::TU_DYNAMIC_WRITE_ONLY); + + const auto to_byte = [](float value) { + return static_cast( + std::clamp(static_cast(std::round(value * 255.f)), 0, 255)); + }; + std::array pixel = {to_byte(color.b), to_byte(color.g), + to_byte(color.r), to_byte(color.a)}; + Ogre::PixelBox pixel_box(1u, 1u, 1u, Ogre::PF_A8R8G8B8, pixel.data()); + texture->getBuffer()->blitFromMemory(pixel_box); + legend_texture_names_.push_back(texture_name); + } + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create( + material_name.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setLightingEnabled(false); + material->setDepthCheckEnabled(false); + material->setDepthWriteEnabled(false); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + Ogre::Pass* pass = material->getTechnique(0)->getPass(0); + pass->setAmbient(color); + pass->setDiffuse(color); + pass->setSelfIllumination(color); + if (!texture.isNull()) { + pass->createTextureUnitState(texture_name); + } + + legend_material_names_.push_back(material_name.str()); + return material_name.str(); +} + +void LayeredMapDisplay::addLegendText(const std::string& text, float x, float y, + float char_height, + const Ogre::ColourValue& color) { + if (!legend_panel_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + std::ostringstream element_name; + element_name << "LayeredMapLegendText_" << reinterpret_cast(this) + << "_" << legend_element_counter_++; + + auto* text_element = static_cast( + overlay_manager.createOverlayElement("TextArea", element_name.str())); + text_element->setMetricsMode(Ogre::GMM_PIXELS); + text_element->setPosition(x, y); + text_element->setDimensions(380.f - x, 38.f); + text_element->setCharHeight(char_height); + text_element->setFontName("Liberation Sans"); + text_element->setColour(color); + text_element->setCaption(text); + + legend_panel_->addChild(text_element); + legend_elements_.push_back(text_element); +} + +void LayeredMapDisplay::addLegendRow(const Ogre::ColourValue& color, + const std::string& text, float y) { + if (!legend_panel_) { + return; + } + + auto& overlay_manager = Ogre::OverlayManager::getSingleton(); + std::ostringstream swatch_name; + swatch_name << "LayeredMapLegendSwatch_" << reinterpret_cast(this) + << "_" << legend_element_counter_++; + + auto* swatch = static_cast( + overlay_manager.createOverlayElement("Panel", swatch_name.str())); + swatch->setMetricsMode(Ogre::GMM_PIXELS); + constexpr float kRowHeight = 24.f; + constexpr float kSwatchSize = 24.f; + swatch->setPosition(24.f, y + 0.5f * (kRowHeight - kSwatchSize)); + swatch->setDimensions(kSwatchSize, kSwatchSize); + swatch->setMaterialName(makeLegendMaterial(color, "swatch")); + + legend_panel_->addChild(swatch); + legend_elements_.push_back(swatch); + + addLegendText(text, 62.f, y + 2.f, 24.f, + Ogre::ColourValue(0.05f, 0.06f, 0.07f, 1.f)); +} + +void LayeredMapDisplay::updateLegendOverlay(const wavemap_msgs::LayeredMap* msg) { + if (!legend_panel_ || !legend_overlay_) { + return; + } + + if (!show_legend_property_.getBool()) { + legend_overlay_->hide(); + return; + } + + clearLegendOverlayElements(); + + struct LegendRow { + Ogre::ColourValue color; + std::string text; + }; + + const DisplayLayer* layer = selectedLayer(); + if (!layer) { + legend_panel_->setDimensions(410.f, 128.f); + addLegendText("Layered map", 22.f, 18.f, 28.f, + Ogre::ColourValue(0.02f, 0.025f, 0.03f, 1.f)); + addLegendText("No layer selected", 22.f, 68.f, 22.f, + Ogre::ColourValue(0.18f, 0.20f, 0.24f, 1.f)); + legend_overlay_->show(); + return; + } + + std::vector rows; + std::string subtitle; + + if (layer->source == DisplayLayer::Source::kContinuous) { + if (layer->name == "occupancy") { + const VoxelColorMode color_mode = + voxel_visual_ ? voxel_visual_->colorMode() + : VoxelColorMode{VoxelColorMode::kHeight}; + switch (color_mode.toTypeId()) { + case VoxelColorMode::kProbability: + subtitle = "occupancy / probability"; + rows.push_back({logOddsToColor(-20.f), "0% probability"}); + rows.push_back({logOddsToColor(0.f), "50% probability"}); + rows.push_back({logOddsToColor(20.f), "100% probability"}); + break; + case VoxelColorMode::kFlat: + subtitle = "occupancy / flat color"; + rows.push_back( + {voxel_visual_ ? voxel_visual_->flatColor() + : Ogre::ColourValue::Blue, + "all displayed voxels"}); + break; + case VoxelColorMode::kHeight: + default: + subtitle = "Color based on height"; + break; + } + } else if (layer->type == "float32_rgb" || layer->type == "float32_vec3") { + subtitle = "direct RGB color"; + rows.push_back({Ogre::ColourValue(1.f, 0.f, 0.f, 1.f), "red channel"}); + rows.push_back({Ogre::ColourValue(0.f, 1.f, 0.f, 1.f), "green channel"}); + rows.push_back({Ogre::ColourValue(0.f, 0.f, 1.f, 1.f), "blue channel"}); + } else if (layer->type == "float32") { + subtitle = "scalar value / RViz display range"; + const FloatingPoint min_value = + voxel_visual_ ? voxel_visual_->scalarDisplayMin() : 0.f; + const FloatingPoint max_value = + voxel_visual_ ? voxel_visual_->scalarDisplayMax() : 1.f; + std::ostringstream min_label; + std::ostringstream max_label; + min_label << min_value; + max_label << max_value; + rows.push_back( + {voxel_visual_ ? voxel_visual_->scalarLowColor() + : Ogre::ColourValue(0.f, 0.f, 1.f, 1.f), + min_label.str()}); + rows.push_back( + {voxel_visual_ ? voxel_visual_->scalarHighColor() + : Ogre::ColourValue(1.f, 1.f, 0.f, 1.f), + max_label.str()}); + } else { + subtitle = "No legend rule for type " + layer->type; + } + } else if (!msg) { + subtitle = "No discrete data loaded"; + } else { + const auto layer_it = std::find_if( + msg->discrete_layers.begin(), msg->discrete_layers.end(), + [this](const wavemap_msgs::DiscreteLayer& discrete_layer) { + return discrete_layer.name == selected_layer_name_; + }); + if (layer_it == msg->discrete_layers.end()) { + subtitle = "Layer missing in map"; + } else if (layer_it->value_type == "bool") { + subtitle = "boolean values"; + const std::set values = collectBoolValues(*layer_it); + if (values.count(true)) { + rows.push_back({voxel_visual_ ? voxel_visual_->boolColor(true) + : Ogre::ColourValue(1.f, 0.05f, 0.05f), + "true"}); + } + if (values.count(false)) { + rows.push_back({voxel_visual_ ? voxel_visual_->boolColor(false) + : Ogre::ColourValue(0.35f, 0.35f, 0.35f), + "false"}); + } + if (values.empty()) { + subtitle = "No observed values"; + } + } else if (layer_it->value_type == "int") { + subtitle = "integer values"; + const std::set values = collectIntValues(*layer_it); + std::map labels; + for (const auto& category : layer_it->categories) { + labels[category.value] = category.label; + } + constexpr size_t kMaxLegendEntries = 10u; + const size_t category_limit = values.size() > kMaxLegendEntries + ? kMaxLegendEntries - 1u + : kMaxLegendEntries; + size_t entry_count = 0u; + for (const int value : values) { + if (category_limit <= entry_count) { + rows.push_back({Ogre::ColourValue(0.70f, 0.70f, 0.70f, 1.f), + "+" + std::to_string(values.size() - entry_count) + + " more"}); + break; + } + const auto label_it = labels.find(value); + const std::string label = + label_it == labels.end() || label_it->second.empty() + ? "Value " + std::to_string(value) + : label_it->second + " (" + std::to_string(value) + ")"; + rows.push_back( + {voxel_visual_ ? voxel_visual_->categoryColor(value) + : stableIntColor(value), + label}); + ++entry_count; + } + if (values.empty()) { + subtitle = "No observed values"; + } + } else { + subtitle = "No legend rule for type " + layer_it->value_type; + } + } + + constexpr size_t kMaxVisibleRows = 10u; + const size_t num_visible_rows = std::min(rows.size(), kMaxVisibleRows); + constexpr float kLegendRowHeight = 40.f; + const float panel_height = std::max(136.f + kLegendRowHeight * + static_cast(num_visible_rows), + 170.f); + legend_panel_->setDimensions(410.f, panel_height); + + addLegendText("Layer: " + layer->name, 22.f, 20.f, 28.f, + Ogre::ColourValue(0.02f, 0.025f, 0.03f, 1.f)); + addLegendText(subtitle, 22.f, 70.f, 21.f, + Ogre::ColourValue(0.22f, 0.24f, 0.28f, 1.f)); + + float row_y = 108.f; + size_t visible_row_count = 0u; + for (const LegendRow& row : rows) { + if (kMaxVisibleRows <= visible_row_count) { + break; + } + addLegendRow(row.color, row.text, row_y); + row_y += kLegendRowHeight; + ++visible_row_count; + } + + legend_overlay_->show(); +} + +void LayeredMapDisplay::updateLegendVisibilityCallback() { + updateLegendOverlay(latest_msg_ ? &latest_msg_.value() : nullptr); +} + +void LayeredMapDisplay::updateSourceModeCallback() { + ProfilerZoneScoped; + const LayeredMapSourceMode old_source_mode = source_mode_; + source_mode_ = LayeredMapSourceMode(source_mode_property_.getStdString()); + + unreliable_property_->setHidden(source_mode_ != LayeredMapSourceMode::kFromTopic); + queue_size_property_->setHidden(source_mode_ != LayeredMapSourceMode::kFromTopic); + topic_property_->setHidden(source_mode_ != LayeredMapSourceMode::kFromTopic); + + load_map_from_disk_property_.setHidden(source_mode_ != LayeredMapSourceMode::kFromFile); + load_map_from_disk_property_.resetAllValues(); + + if (source_mode_ != old_source_mode) { + if (source_mode_ == LayeredMapSourceMode::kFromTopic) { + updateTopic(); + } else { + unsubscribe(); + } + latest_msg_.reset(); + clearStoredMap(); + if (voxel_visual_) { + voxel_visual_->clear(); + } + } +} + +void LayeredMapDisplay::loadMapFromDiskCallback() { + ProfilerZoneScoped; + const auto filepath_qt = QFileDialog::getOpenFileName( + nullptr, "Open layered wavemap", QString(), + "Layered wavemap files (*.lwvmp *.lwmp);;All files (*)"); + + if (filepath_qt.isEmpty()) { + load_map_from_disk_property_.resetAllValues(); + return; + } + + const std::filesystem::path filepath{filepath_qt.toStdString()}; + std::string load_error; + if (!loadMapFromDisk(filepath, &load_error)) { + if (load_error.empty()) { + load_error = "The file could not be decoded as a layered map."; + } + AlertDialog alert{ + "Could not load layered map", + std::string("Failed to load:\n\"") + filepath.string() + + "\"\n\n" + load_error}; + alert.exec(); + load_map_from_disk_property_.resetAllValues(); + return; + } + + load_map_from_disk_property_.setAtRestValue(filepath.filename().string()); +} + +void LayeredMapDisplay::updateLayerSelectionCallback() { + ProfilerZoneScoped; + const DisplayLayer* previous_layer = selectedLayer(); + if (voxel_visual_ && previous_layer) { + if (previous_layer->source == DisplayLayer::Source::kDiscrete) { + discrete_termination_height_ = voxel_visual_->terminationHeight(); + } else { + continuous_termination_height_ = voxel_visual_->terminationHeight(); + } + } + selected_layer_name_ = layer_property_.getStdString(); + if (latest_msg_) { + configureSelectedLayerAppearance(latest_msg_.value()); + updateLegendOverlay(&latest_msg_.value()); + updateStoredMapForSelectedLayer(latest_msg_.value()); + } else { + updateLegendOverlay(nullptr); + } + if (voxel_visual_) { + const DisplayLayer* current_layer = selectedLayer(); + if (current_layer && latest_msg_) { + if (current_layer->source == DisplayLayer::Source::kDiscrete) { + const auto layer_it = std::find_if( + latest_msg_->discrete_layers.begin(), + latest_msg_->discrete_layers.end(), + [this](const auto& layer) { + return layer.name == selected_layer_name_; + }); + const int max_height = + layer_it == latest_msg_->discrete_layers.end() + ? 1 + : layer_it->block_height; + voxel_visual_->setTerminationHeight(max_height, + discrete_termination_height_); + } else if (latest_msg_->continuous_map.layered_hashed_wavelet_octree.size() == + 1u) { + voxel_visual_->setTerminationHeight( + latest_msg_->continuous_map.layered_hashed_wavelet_octree.front() + .tree_height, + continuous_termination_height_); + } + } + voxel_visual_->clear(); + voxel_visual_->updateMap(true); + } +} +} // namespace wavemap::rviz_plugin + +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::LayeredMapDisplay, rviz::Display) diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc new file mode 100644 index 000000000..cb7638ac3 --- /dev/null +++ b/interfaces/ros1/wavemap_rviz_plugin/src/layered_map_factories.cc @@ -0,0 +1,207 @@ +#include +#include + +#include +#include +#include + +#include "occupancy_map_layers.h" +#include "reflectivity_map_layers.h" +#include "reflectivity_only_map_layers.h" +#include "rgb_map_layers.h" +#include "signal_map_layers.h" +#include "wavemap_rviz_plugin/layered_map_factory.h" + +namespace wavemap::rviz_plugin { +namespace occupancy_map = wavemap::examples::occupancy_map; +namespace reflectivity_map = wavemap::examples::reflectivity_map; +namespace reflectivity_only_map = wavemap::examples::reflectivity_only_map; +namespace rgb_map = wavemap::examples::rgb_map; +namespace signal_map = wavemap::examples::signal_map; + +FloatingPoint normalizeScalar(FloatingPoint value, + const LayerMetadata& metadata) { + if (metadata.min_values.size() != 1u || + metadata.max_values.size() != 1u || + metadata.max_values.front() <= metadata.min_values.front()) { + return std::clamp(value, 0.f, 1.f); + } + return std::clamp((value - metadata.min_values.front()) / + (metadata.max_values.front() - + metadata.min_values.front()), + 0.f, 1.f); +} + +Ogre::ColourValue scalarGradient(FloatingPoint normalized_value, + const Ogre::ColourValue& low, + const Ogre::ColourValue& high) { + const FloatingPoint value = std::clamp(normalized_value, 0.f, 1.f); + return Ogre::ColourValue(low.r + value * (high.r - low.r), + low.g + value * (high.g - low.g), + low.b + value * (high.b - low.b), 1.f); +} + +struct ReflectivityContinuousLayerColorProvider { + template + static bool getLayerColor(const std::string& layer_name, + const VoxelT& voxel, + FloatingPoint /*occupancy*/, + FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, + Ogre::ColourValue& color) { + if (layer_name != "reflectivity") { + return false; + } + LayerMetadata display_metadata; + display_metadata.min_values = {scalar_display_min}; + display_metadata.max_values = {scalar_display_max}; + const auto& state = + voxel.data.template get(); + const FloatingPoint value = normalizeScalar( + layered::LayerStateConversion< + reflectivity_map::ReflectivityLayer>::value(state), + display_metadata); + color = scalarGradient(value, scalar_low_color, scalar_high_color); + return true; + } +}; + +struct SignalContinuousLayerColorProvider { + template + static Ogre::ColourValue colorFor( + const VoxelT& voxel, FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color) { + LayerMetadata display_metadata; + display_metadata.min_values = {scalar_display_min}; + display_metadata.max_values = {scalar_display_max}; + const auto& state = voxel.data.template get(); + const FloatingPoint value = normalizeScalar( + layered::LayerStateConversion::value(state), + display_metadata); + return scalarGradient(value, scalar_low_color, scalar_high_color); + } + + template + static bool getLayerColor( + const std::string& layer_name, const VoxelT& voxel, + FloatingPoint /*occupancy*/, FloatingPoint scalar_display_min, + FloatingPoint scalar_display_max, + const Ogre::ColourValue& scalar_low_color, + const Ogre::ColourValue& scalar_high_color, + const LayerMetadata& /*metadata*/, Ogre::ColourValue& color) { + if (layer_name == "reflectivity") { + color = colorFor( + voxel, scalar_display_min, scalar_display_max, scalar_low_color, + scalar_high_color); + } else if (layer_name == "signal") { + color = colorFor( + voxel, scalar_display_min, scalar_display_max, scalar_low_color, + scalar_high_color); + } else if (layer_name == "near_ir") { + color = colorFor( + voxel, scalar_display_min, scalar_display_max, scalar_low_color, + scalar_high_color); + } else { + return false; + } + return true; + } +}; + +struct RgbContinuousLayerColorProvider { + template + static bool getLayerColor( + const std::string& layer_name, const VoxelT& voxel, + FloatingPoint /*occupancy*/, FloatingPoint /*scalar_display_min*/, + FloatingPoint /*scalar_display_max*/, + const Ogre::ColourValue& /*scalar_low_color*/, + const Ogre::ColourValue& /*scalar_high_color*/, + const LayerMetadata& /*metadata*/, Ogre::ColourValue& color) { + if (layer_name != "rgb") { + return false; + } + const layered::Rgb value = layered::LayerStateConversion< + rgb_map::RgbLayer>::value( + voxel.data.template get()); + color = Ogre::ColourValue(std::clamp(value.r, 0.f, 1.f), + std::clamp(value.g, 0.f, 1.f), + std::clamp(value.b, 0.f, 1.f), 1.f); + return true; + } +}; + +signal_map::NearIrClassDefinition::Config makeAllLayersH2MapConfig() { + return signal_map::makeMapConfigFor< + signal_map::NearIrClassDefinition>(2); +} + +reflectivity_map::Definition::Config makeReflectivityClassMapConfig() { + return reflectivity_map::makeMapConfig< + reflectivity_map::Definition>(0); +} + +class OccupancyLayeredMapFactory final + : public TypedLayeredMapFileFactory< + occupancy_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + occupancy_map::Definition::Voxel>, + NoLayerColorProvider, occupancy_map::Definition::Map, + occupancy_map::Definition::MapIo, + occupancy_map::makeMapConfig> {}; + +class ReflectivityLayeredMapFactory final + : public TypedLayeredMapFileFactory< + reflectivity_only_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + reflectivity_only_map::Definition::Voxel>, + ReflectivityContinuousLayerColorProvider, + reflectivity_only_map::Definition::Map, + reflectivity_only_map::Definition::MapIo, + reflectivity_only_map::makeMapConfig< + reflectivity_only_map::Definition>> {}; + +class ReflectivityClassLayeredMapFactory final + : public TypedLayeredMapFileFactory< + reflectivity_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + reflectivity_map::Definition::Voxel>, + ReflectivityContinuousLayerColorProvider, + reflectivity_map::Definition::Map, + reflectivity_map::Definition::MapIo, + makeReflectivityClassMapConfig> {}; + +class SignalNearIrClassLayeredMapFactory final + : public TypedLayeredMapFileFactory< + signal_map::NearIrClassDefinition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + signal_map::NearIrClassDefinition::Voxel>, + SignalContinuousLayerColorProvider, + signal_map::NearIrClassDefinition::Map, + signal_map::NearIrClassDefinition::MapIo, + makeAllLayersH2MapConfig> {}; + +class RgbLayeredMapFactory final + : public TypedLayeredMapFileFactory< + rgb_map::Definition::ContinuousMap, + wavemap::convert::DescriptorContinuousRosConverter< + rgb_map::Definition::Voxel>, + RgbContinuousLayerColorProvider, rgb_map::Definition::Map, + rgb_map::Definition::MapIo, rgb_map::makeMapConfig> {}; +} // namespace wavemap::rviz_plugin + +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::OccupancyLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::ReflectivityClassLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS( + wavemap::rviz_plugin::SignalNearIrClassLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) +PLUGINLIB_EXPORT_CLASS(wavemap::rviz_plugin::RgbLayeredMapFactory, + wavemap::rviz_plugin::LayeredMapFactory) diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc index 9188a7e8e..cb7b8bbc3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/cell_selector.cc @@ -56,15 +56,26 @@ void CellSelector::setMap(const MapBase::ConstPtr& map) { const auto hashed_map = std::dynamic_pointer_cast(map); if (hashed_map) { - query_accelerator_.emplace(*hashed_map); + setOccupancyQuery( + std::make_shared(hashed_map)); } else { - if (cell_selection_mode_ == CellSelectionMode::kSurface) { - ROS_WARN( - "Cell selection mode 'Surface' only supports HashedWaveletOctree " - "maps. Falling back to mode 'Band'."); - cell_selection_mode_ = CellSelectionMode::kBand; - initializePropertyMenu(); - } + clearOccupancyQuery(); + } +} + +void CellSelector::setOccupancyQuery( + std::shared_ptr occupancy_query) { + occupancy_query_ = std::move(occupancy_query); +} + +void CellSelector::clearOccupancyQuery() { + occupancy_query_.reset(); + if (cell_selection_mode_ == CellSelectionMode::kSurface) { + ROS_WARN( + "Cell selection mode 'Surface' requires an occupancy query. Falling " + "back to mode 'Band'."); + cell_selection_mode_ = CellSelectionMode::kBand; + initializePropertyMenu(); } } @@ -95,11 +106,15 @@ bool CellSelector::shouldBeDrawn(const OctreeIndex& cell_index, } bool CellSelector::hasFreeNeighbor(const OctreeIndex& cell_index) const { + if (!occupancy_query_) { + return true; + } + for (const auto& offset : kNeighborOffsets) { // NOLINT const OctreeIndex neighbor_index = {cell_index.height, cell_index.position + offset}; const FloatingPoint neighbor_log_odds = - query_accelerator_->getCellValue(neighbor_index); + occupancy_query_->getCellValue(neighbor_index); // Check if the neighbor is free and observed if (neighbor_log_odds < surface_occupancy_threshold_ && !isUnknown(neighbor_log_odds)) { diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc index 1b7fe412c..e93bbadf3 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/visuals/voxel_visual.cc @@ -1,24 +1,35 @@ #include "wavemap_rviz_plugin/visuals/voxel_visual.h" #include +#include #include +#include #include #include +#include +#include #include +#include + #include #include #include #include +#include #include +#include namespace wavemap::rviz_plugin { VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, rviz::ViewManager* view_manager, Ogre::SceneNode* parent_node, rviz::Property* submenu_root_property, - std::shared_ptr map_and_mutex) + std::shared_ptr map_and_mutex, + std::function layer_color_changed_callback) : map_and_mutex_(std::move(map_and_mutex)), + layer_color_changed_callback_( + std::move(layer_color_changed_callback)), scene_manager_(CHECK_NOTNULL(scene_manager)), frame_node_(CHECK_NOTNULL(parent_node)->createChildSceneNode()), visibility_property_( @@ -43,6 +54,14 @@ VoxelVisual::VoxelVisual(Ogre::SceneManager* scene_manager, "Flat color", rviz::ogreToQt(voxel_flat_color_), R"(Solid color to use when "Color Mode" is set to "Flat")", submenu_root_property, SLOT(flatColorUpdateCallback()), this), + scalar_min_property_("Scalar min", 0.0, + "Lower value used when coloring scalar continuous layers.", + submenu_root_property, + SLOT(layerColorUpdateCallback()), this), + scalar_max_property_("Scalar max", 1.0, + "Upper value used when coloring scalar continuous layers.", + submenu_root_property, + SLOT(layerColorUpdateCallback()), this), frame_rate_properties_("Frame rate", QVariant(), "Properties to control the frame rate.", submenu_root_property), @@ -94,10 +113,35 @@ VoxelVisual::~VoxelVisual() { // NOTE: This must be done before any of the objects that are used in the // prerender callback (incl. members of VoxelVisual) are destroyed. prerender_listener_.reset(); + clear(); // Destroy the frame node scene_manager_->destroySceneNode(frame_node_); } +void VoxelVisual::clear() { + block_update_queue_.clear(); + for (auto& [block_index, voxel_layers] : block_voxel_layers_map_) { + (void)block_index; + detachVoxelLayers(voxel_layers); + } + block_voxel_layers_map_.clear(); + discrete_layer_metadata_.reset(); + discrete_parent_cells_.clear(); + discrete_batch_parents_.clear(); +} + +void VoxelVisual::detachVoxelLayers(VoxelLayers& voxel_layers) { + for (auto& voxel_layer : voxel_layers) { + if (!voxel_layer) { + continue; + } + voxel_layer->clear(); + if (voxel_layer->getParentSceneNode()) { + voxel_layer->getParentSceneNode()->detachObject(voxel_layer.get()); + } + } +} + void VoxelVisual::updateMap(bool redraw_all) { ProfilerZoneScoped; if (!visibility_property_.getBool()) { @@ -109,6 +153,15 @@ void VoxelVisual::updateMap(bool redraw_all) { std::scoped_lock lock(map_and_mutex_->mutex); MapBase::ConstPtr map = map_and_mutex_->map; if (!map) { + if (map_and_mutex_->discrete_layer_msg) { + drawDiscreteLayer(map_and_mutex_->discrete_layer_msg.value(), + map_and_mutex_->discrete_layer_min_cell_width); + } else if (map_and_mutex_->layered_map) { + cell_selector_.setOccupancyQuery(map_and_mutex_->layered_map); + drawLayeredMapOccupancy(*map_and_mutex_->layered_map); + } else if (map_and_mutex_->layered_map_msg) { + drawLayeredMapOccupancy(map_and_mutex_->layered_map_msg.value()); + } return; } cell_selector_.setMap(map); @@ -136,6 +189,7 @@ void VoxelVisual::updateMap(bool redraw_all) { it != block_voxel_layers_map_.end();) { const auto block_idx = it->first; if (!hashed_map->hasBlock(block_idx)) { + detachVoxelLayers(it->second); it = block_voxel_layers_map_.erase(it); } else { ++it; @@ -316,6 +370,9 @@ void VoxelVisual::colorModeUpdateCallback() { // Update the map if the color mode changed if (voxel_color_mode_ != old_color_mode) { updateMap(true); + if (layer_color_changed_callback_) { + layer_color_changed_callback_(); + } } } @@ -328,9 +385,89 @@ void VoxelVisual::flatColorUpdateCallback() { // Update the map if the color changed if (voxel_flat_color_ != old_flat_color) { updateMap(true); + if (layer_color_changed_callback_) { + layer_color_changed_callback_(); + } + } +} + +void VoxelVisual::layerColorUpdateCallback() { + ProfilerZoneScoped; + updateMap(true); + if (layer_color_changed_callback_) { + layer_color_changed_callback_(); + } +} + +Ogre::ColourValue VoxelVisual::defaultCategoryColor(int value) { + const uint32_t hash = static_cast(value) * 2654435761u; + const float r = 0.25f + + 0.75f * static_cast((hash >> 16u) & 0xffu) / 255.f; + const float g = 0.25f + + 0.75f * static_cast((hash >> 8u) & 0xffu) / 255.f; + const float b = + 0.25f + 0.75f * static_cast(hash & 0xffu) / 255.f; + return Ogre::ColourValue(r, g, b, 1.f); +} + +void VoxelVisual::configureLayerAppearance( + const std::string& layer_name, const std::string& layer_type, + bool is_discrete, FloatingPoint scalar_min, FloatingPoint scalar_max, + bool has_low_color, const Ogre::ColourValue& low_color, + bool has_high_color, const Ogre::ColourValue& high_color, + const std::vector& categories) { + const bool layer_changed = selected_color_layer_ != layer_name; + selected_color_layer_ = layer_name; + const bool is_occupancy = layer_name == "occupancy"; + const bool is_scalar = + !is_discrete && !is_occupancy && layer_type == "float32"; + + color_mode_property_.setHidden(!is_occupancy); + flat_color_property_.setHidden(!is_occupancy || + voxel_color_mode_ != VoxelColorMode::kFlat); + scalar_min_property_.setHidden(!is_scalar); + scalar_max_property_.setHidden(!is_scalar); + + if (is_scalar && layer_changed) { + const QSignalBlocker min_blocker(&scalar_min_property_); + const QSignalBlocker max_blocker(&scalar_max_property_); + scalar_min_property_.setFloat(scalar_min); + scalar_max_property_.setFloat(scalar_max); + const int layer_hash = + static_cast(std::hash{}(layer_name)); + scalar_low_color_ = + has_low_color ? low_color : defaultCategoryColor(layer_hash); + scalar_high_color_ = has_high_color + ? high_color + : defaultCategoryColor(layer_hash ^ 0x5bd1e995); + } + + bool_false_color_ = defaultCategoryColor(0); + bool_true_color_ = defaultCategoryColor(1); + category_colors_.clear(); + for (const auto& category : categories) { + category_colors_[category.value] = + category.has_color + ? Ogre::ColourValue(category.r, category.g, category.b, 1.f) + : defaultCategoryColor(category.value); } } +Ogre::ColourValue VoxelVisual::categoryColor(int value) const { + const auto it = category_colors_.find(value); + return it == category_colors_.end() ? defaultCategoryColor(value) + : it->second; +} + +bool VoxelVisual::categoryVisible(int /*value*/) const { return true; } + +Ogre::ColourValue VoxelVisual::boolColor(bool value) const { + return value ? bool_true_color_ : bool_false_color_; +} + +bool VoxelVisual::boolVisible(bool /*value*/) const { return true; } + + void VoxelVisual::appendLeafCenterAndColor(int tree_height, FloatingPoint min_cell_width, const OctreeIndex& cell_index, @@ -369,6 +506,45 @@ void VoxelVisual::appendLeafCenterAndColor(int tree_height, } } +void VoxelVisual::appendLayeredLeafCenterAndColor( + const LayeredMapInterface& layered_map, + const std::string& selected_layer_name, int tree_height, + FloatingPoint min_cell_width, const OctreeIndex& cell_index, + FloatingPoint cell_log_odds, VoxelsPerLevel& voxels_per_level) { + if (selected_layer_name == "occupancy") { + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, voxels_per_level); + return; + } + + if (!cell_selector_.shouldBeDrawn(cell_index, cell_log_odds)) { + return; + } + + Ogre::ColourValue layer_color; + if (!layered_map.getLayerColor( + selected_layer_name, cell_index, cell_log_odds, + scalar_min_property_.getFloat(), scalar_max_property_.getFloat(), + scalar_low_color_, scalar_high_color_, + layer_color)) { + appendLeafCenterAndColor(tree_height, min_cell_width, cell_index, + cell_log_odds, voxels_per_level); + return; + } + + const IndexElement depth = tree_height - cell_index.height; + CHECK_GE(depth, 0); + CHECK_LT(depth, voxels_per_level.size()); + const Point3D cell_center = + convert::nodeIndexToCenterPoint(cell_index, min_cell_width); + + auto& point = voxels_per_level[depth].emplace_back(); + point.center.x = cell_center[0]; + point.center.y = cell_center[1]; + point.center.z = cell_center[2]; + point.color = layer_color; +} + void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, FloatingPoint min_cell_width, const Index3D& block_index, @@ -406,6 +582,530 @@ void VoxelVisual::drawMultiResolutionVoxels(IndexElement tree_height, } } + +void VoxelVisual::drawLayeredMapOccupancy(const LayeredMapInterface& layered_map) { + ProfilerZoneScoped; + const IndexElement tree_height = layered_map.getTreeHeight(); + const FloatingPoint min_cell_width = layered_map.getMinCellWidth(); + const FloatingPoint alpha = opacity_property_.getFloat(); + const std::string selected_layer_name = map_and_mutex_->selected_layer_name; + const IndexElement termination_height = + std::min(termination_height_property_.getInt(), tree_height); + + termination_height_property_.setMax(tree_height); + block_update_queue_.clear(); + + // Remove visuals for blocks that no longer exist in the layered map. + const std::vector block_indices = layered_map.getBlockIndices(); + std::unordered_set allocated_blocks( + block_indices.begin(), block_indices.end()); + for (auto it = block_voxel_layers_map_.begin(); + it != block_voxel_layers_map_.end();) { + if (!allocated_blocks.count(it->first)) { + detachVoxelLayers(it->second); + it = block_voxel_layers_map_.erase(it); + } else { + ++it; + } + } + + // Draw occupancy through the runtime layered map interface. Layer-specific + // color mapping will be added on top of this path. + for (const auto& block_index : block_indices) { + const int num_levels = tree_height + 1 - termination_height; + VoxelsPerLevel voxels_per_level(num_levels); + layered_map.forEachBlockLeaf( + block_index, + [this, &layered_map, &selected_layer_name, tree_height, min_cell_width, + &voxels_per_level](const OctreeIndex& cell_index, + FloatingPoint cell_log_odds) { + appendLayeredLeafCenterAndColor(layered_map, selected_layer_name, + tree_height, min_cell_width, + cell_index, cell_log_odds, + voxels_per_level); + }, + termination_height); + drawMultiResolutionVoxels(tree_height, min_cell_width, block_index, alpha, + voxels_per_level, + block_voxel_layers_map_[block_index]); + } + + num_queued_blocks_indicator_.setInt(0); +} + + +void VoxelVisual::drawLayeredMapOccupancy(const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg) { + ProfilerZoneScoped; + const IndexElement tree_height = layered_map_msg.tree_height; + const FloatingPoint min_cell_width = layered_map_msg.min_cell_width; + const FloatingPoint alpha = opacity_property_.getFloat(); + const IndexElement termination_height = std::min( + termination_height_property_.getInt(), tree_height); + + termination_height_property_.setMax(tree_height); + block_update_queue_.clear(); + + // Remove visuals for blocks that no longer exist in the layered map message. + std::unordered_set allocated_blocks; + for (const auto& block_index_msg : layered_map_msg.allocated_block_indices) { + allocated_blocks.emplace(block_index_msg.x, block_index_msg.y, block_index_msg.z); + } + for (auto it = block_voxel_layers_map_.begin(); it != block_voxel_layers_map_.end();) { + if (!allocated_blocks.count(it->first)) { + detachVoxelLayers(it->second); + it = block_voxel_layers_map_.erase(it); + } else { + ++it; + } + } + + for (const auto& block_msg : layered_map_msg.blocks) { + const Index3D block_index{block_msg.root_node_offset.x, block_msg.root_node_offset.y, block_msg.root_node_offset.z}; + const int num_levels = tree_height + 1 - termination_height; + VoxelsPerLevel voxels_per_level(num_levels); + appendLayeredBlockOccupancy(layered_map_msg, block_msg, termination_height, voxels_per_level); + drawMultiResolutionVoxels(tree_height, min_cell_width, block_index, alpha, voxels_per_level, block_voxel_layers_map_[block_index]); + } + + num_queued_blocks_indicator_.setInt(0); +} + +namespace { +int continuousLayerComponentCount(const std::string& type) { + if (type == "float32_rgb" || type == "float32_vec3") { + return 3; + } + if (type == "float32") { + return 1; + } + return 0; +} + +FloatingPoint readFloatLayerValue(const wavemap_msgs::Layer& layer, + size_t value_index, + int component_count, int component_index) { + const size_t array_index = value_index * static_cast(component_count) + + static_cast(component_index); + if (array_index < layer.float32_values.size()) { + return layer.float32_values[array_index]; + } + return 0.f; +} + +Ogre::ColourValue interpolateColor(const Ogre::ColourValue& low, + const Ogre::ColourValue& high, + FloatingPoint t) { + t = std::clamp(t, 0.f, 1.f); + return Ogre::ColourValue(low.r + t * (high.r - low.r), + low.g + t * (high.g - low.g), + low.b + t * (high.b - low.b), 1.f); +} +} // namespace + +bool VoxelVisual::getGenericContinuousLayerColor( + const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, + const std::vector>& layer_values, + Ogre::ColourValue& color) const { + const std::string selected_layer_name = map_and_mutex_->selected_layer_name; + const auto layer_name_it = std::find(layered_map_msg.layer_names.begin(), + layered_map_msg.layer_names.end(), + selected_layer_name); + if (layer_name_it == layered_map_msg.layer_names.end()) { + return false; + } + + const size_t layer_index = static_cast( + std::distance(layered_map_msg.layer_names.begin(), layer_name_it)); + if (layer_values.size() <= layer_index || + layered_map_msg.layer_types.size() <= layer_index) { + return false; + } + + const std::string& layer_type = layered_map_msg.layer_types[layer_index]; + const auto& values = layer_values[layer_index]; + if ((layer_type == "float32_rgb" || layer_type == "float32_vec3") && + values.size() >= 3u) { + color = Ogre::ColourValue(std::clamp(values[0], 0.f, 1.f), + std::clamp(values[1], 0.f, 1.f), + std::clamp(values[2], 0.f, 1.f), 1.f); + return true; + } + + if (layer_type == "float32" && !values.empty()) { + const FloatingPoint min_value = scalar_min_property_.getFloat(); + const FloatingPoint max_value = scalar_max_property_.getFloat(); + const FloatingPoint range = max_value - min_value; + const FloatingPoint t = std::abs(range) < kEpsilon + ? 0.f + : (values.front() - min_value) / range; + color = interpolateColor(scalar_low_color_, scalar_high_color_, t); + return true; + } + + return false; +} + +Index3D VoxelVisual::discreteRenderBatchIndex(const Index3D& parent_index) { + constexpr int kParentsPerRenderBatchSide = 16; + const auto floor_divide = [](int value, int divisor) { + const int quotient = value / divisor; + const int remainder = value % divisor; + return quotient - (remainder < 0 ? 1 : 0); + }; + return {floor_divide(parent_index.x(), kParentsPerRenderBatchSide), + floor_divide(parent_index.y(), kParentsPerRenderBatchSide), + floor_divide(parent_index.z(), kParentsPerRenderBatchSide)}; +} + +void VoxelVisual::drawDiscreteLayer( + const wavemap_msgs::DiscreteLayer& discrete_layer_msg, + FloatingPoint min_cell_width) { + ProfilerZoneScoped; + termination_height_property_.setMax(0); + block_update_queue_.clear(); + + for (auto& [batch_index, voxel_layers] : block_voxel_layers_map_) { + (void)batch_index; + detachVoxelLayers(voxel_layers); + } + block_voxel_layers_map_.clear(); + discrete_parent_cells_.clear(); + discrete_batch_parents_.clear(); + discrete_layer_metadata_ = discrete_layer_msg; + discrete_layer_metadata_->cells.clear(); + discrete_layer_metadata_->deleted_parent_indices.clear(); + + std::unordered_set affected_batches; + for (const auto& cell : discrete_layer_msg.cells) { + const Index3D parent_index{cell.parent_index.x, cell.parent_index.y, + cell.parent_index.z}; + const Index3D batch_index = discreteRenderBatchIndex(parent_index); + discrete_parent_cells_[parent_index] = cell; + discrete_batch_parents_[batch_index].insert(parent_index); + affected_batches.insert(batch_index); + } + rebuildDiscreteBatches(affected_batches, min_cell_width); +} + +void VoxelVisual::applyDiscreteLayerPatch( + const wavemap_msgs::DiscreteLayer& patch, FloatingPoint min_cell_width) { + ProfilerZoneScoped; + if (!discrete_layer_metadata_ || + discrete_layer_metadata_->name != patch.name) { + return; + } + discrete_layer_metadata_->value_type = patch.value_type; + discrete_layer_metadata_->block_height = patch.block_height; + discrete_layer_metadata_->categories = patch.categories; + + std::unordered_set affected_batches; + const auto erase_parent = [&](const Index3D& parent_index) { + const Index3D batch_index = discreteRenderBatchIndex(parent_index); + affected_batches.insert(batch_index); + discrete_parent_cells_.erase(parent_index); + const auto batch_it = discrete_batch_parents_.find(batch_index); + if (batch_it != discrete_batch_parents_.end()) { + batch_it->second.erase(parent_index); + if (batch_it->second.empty()) { + discrete_batch_parents_.erase(batch_it); + } + } + }; + + for (const auto& deleted : patch.deleted_parent_indices) { + erase_parent(Index3D{deleted.x, deleted.y, deleted.z}); + } + for (const auto& cell : patch.cells) { + const Index3D parent_index{cell.parent_index.x, cell.parent_index.y, + cell.parent_index.z}; + const Index3D batch_index = discreteRenderBatchIndex(parent_index); + affected_batches.insert(batch_index); + discrete_parent_cells_[parent_index] = cell; + discrete_batch_parents_[batch_index].insert(parent_index); + } + rebuildDiscreteBatches(affected_batches, min_cell_width); +} + +void VoxelVisual::rebuildDiscreteBatches( + const std::unordered_set& batch_indices, + FloatingPoint min_cell_width) { + if (!discrete_layer_metadata_) { + return; + } + const auto& layer_msg = *discrete_layer_metadata_; + const FloatingPoint alpha = opacity_property_.getFloat(); + const int side_length = 1 << layer_msg.block_height; + + for (const Index3D& batch_index : batch_indices) { + std::vector cells; + const auto batch_it = discrete_batch_parents_.find(batch_index); + if (batch_it != discrete_batch_parents_.end()) { + for (const Index3D& parent_index : batch_it->second) { + const auto parent_it = discrete_parent_cells_.find(parent_index); + if (parent_it == discrete_parent_cells_.end()) { + continue; + } + const auto& cell_msg = parent_it->second; + std::unordered_map exception_indices; + for (size_t exception_i = 0u; + exception_i < cell_msg.exception_offsets.size(); ++exception_i) { + exception_indices[cell_msg.exception_offsets[exception_i]] = + static_cast(exception_i); + } + for (const int offset : cell_msg.observed_offsets) { + const auto exception_it = exception_indices.find(offset); + const bool is_exception = exception_it != exception_indices.end(); + const int exception_index = + is_exception ? exception_it->second : -1; + Ogre::ColourValue color; + if (!getDiscreteCellColor(layer_msg, cell_msg, exception_index, + is_exception, color)) { + continue; + } + const int local_x = offset % side_length; + const int local_y = (offset / side_length) % side_length; + const int local_z = offset / (side_length * side_length); + auto& cell = cells.emplace_back(); + cell.center = Ogre::Vector3( + (parent_index.x() * side_length + local_x + 0.5f) * + min_cell_width, + (parent_index.y() * side_length + local_y + 0.5f) * + min_cell_width, + (parent_index.z() * side_length + local_z + 0.5f) * + min_cell_width); + cell.color = color; + } + } + } + + if (cells.empty()) { + const auto visual_it = block_voxel_layers_map_.find(batch_index); + if (visual_it != block_voxel_layers_map_.end()) { + detachVoxelLayers(visual_it->second); + block_voxel_layers_map_.erase(visual_it); + } + continue; + } + + auto& voxel_layers = block_voxel_layers_map_[batch_index]; + if (voxel_layers.empty()) { + auto& voxel_layer = voxel_layers.emplace_back( + std::make_unique(voxel_material_)); + voxel_layer->setName("discrete_layer_" + layer_msg.name + "_" + + std::to_string(Index3DHash()(batch_index))); + frame_node_->attachObject(voxel_layer.get()); + } + auto& voxel_layer = voxel_layers.front(); + voxel_layer->setCellDimensions(min_cell_width, min_cell_width, + min_cell_width); + voxel_layer->setAlpha(alpha); + voxel_layer->setCells(cells); + } + num_queued_blocks_indicator_.setInt(0); +} + +bool VoxelVisual::getDiscreteCellColor( + const wavemap_msgs::DiscreteLayer& layer_msg, + const wavemap_msgs::DiscreteLayerCell& cell_msg, int exception_index, + bool is_exception, Ogre::ColourValue& color) const { + if (layer_msg.value_type == "bool") { + const bool value = is_exception + ? cell_msg.exception_uint8_values[exception_index] != 0u + : cell_msg.dominant_uint8 != 0u; + if (!boolVisible(value)) { + return false; + } + color = boolColor(value); + return true; + } + + if (layer_msg.value_type == "int") { + const int value = is_exception ? cell_msg.exception_int32_values[exception_index] + : cell_msg.dominant_int32; + if (!categoryVisible(value)) { + return false; + } + color = categoryColor(value); + return true; + } + + color = Ogre::ColourValue(0.7f, 0.7f, 0.7f, 1.f); + return true; +} + +void VoxelVisual::appendLayeredBlockOccupancy(const wavemap_msgs::LayeredHashedWaveletOctree& layered_map_msg, const wavemap_msgs::LayeredHashedWaveletOctreeBlock& block_msg, IndexElement termination_height, VoxelsPerLevel& voxels_per_level) { + ProfilerZoneScoped; + if (block_msg.nodes.empty()) { + return; + } + + using Coefficients = HashedWaveletOctreeBlock::Coefficients; + using Transform = HashedWaveletOctreeBlock::Transform; + using LayerValues = std::vector>; + + struct StackElement { + OctreeIndex node_index; + FloatingPoint occupancy_scale; + LayerValues layer_scales; + bool render_descendants; + }; + + LayerValues root_layer_scales; + root_layer_scales.reserve(layered_map_msg.layer_types.size()); + for (size_t layer_i = 0u; layer_i < layered_map_msg.layer_types.size(); ++layer_i) { + const int component_count = continuousLayerComponentCount(layered_map_msg.layer_types[layer_i]); + std::vector values; + values.reserve(component_count); + const wavemap_msgs::Layer* root_layer = + layer_i < block_msg.root_node_layers.size() ? &block_msg.root_node_layers[layer_i] : nullptr; + for (int component_i = 0; component_i < component_count; ++component_i) { + values.push_back(root_layer ? readFloatLayerValue(*root_layer, 0u, component_count, component_i) : 0.f); + } + root_layer_scales.push_back(std::move(values)); + } + + auto append_leaf = [this, &layered_map_msg, &voxels_per_level]( + const OctreeIndex& node_index, + FloatingPoint occupancy_scale, + const LayerValues& layer_scales) { + if (map_and_mutex_->selected_layer_name == "occupancy") { + appendLeafCenterAndColor(layered_map_msg.tree_height, + layered_map_msg.min_cell_width, node_index, + occupancy_scale, voxels_per_level); + return; + } + + if (!cell_selector_.shouldBeDrawn(node_index, occupancy_scale)) { + return; + } + + Ogre::ColourValue layer_color; + if (!getGenericContinuousLayerColor(layered_map_msg, layer_scales, + layer_color)) { + appendLeafCenterAndColor(layered_map_msg.tree_height, + layered_map_msg.min_cell_width, node_index, + occupancy_scale, voxels_per_level); + return; + } + + const IndexElement depth = layered_map_msg.tree_height - node_index.height; + CHECK_GE(depth, 0); + CHECK_LT(depth, voxels_per_level.size()); + const Point3D cell_center = convert::nodeIndexToCenterPoint( + node_index, layered_map_msg.min_cell_width); + + auto& point = voxels_per_level[depth].emplace_back(); + point.center.x = cell_center[0]; + point.center.y = cell_center[1]; + point.center.z = cell_center[2]; + point.color = layer_color; + }; + + size_t next_node_msg_index = 0u; + std::stack stack; + stack.emplace(StackElement{ + OctreeIndex{layered_map_msg.tree_height, + Index3D{block_msg.root_node_offset.x, + block_msg.root_node_offset.y, + block_msg.root_node_offset.z}}, + block_msg.root_node_occupancy_scale_coefficient, + std::move(root_layer_scales), true}); + + while (!stack.empty()) { + StackElement stack_element = stack.top(); + stack.pop(); + + if (block_msg.nodes.size() <= next_node_msg_index) { + ROS_WARN("Layered map block ended before all queued nodes were read."); + return; + } + + // Nodes are serialized in depth-first preorder. Consume the next message + // when its corresponding stack entry is popped instead of assigning + // indices while siblings are discovered. A sibling's message only follows + // after the complete subtree of its preceding sibling. + const auto& node_msg = block_msg.nodes[next_node_msg_index++]; + Coefficients::Details occupancy_details; + std::copy_n(node_msg.occupancy_detail_coefficients.begin(), + occupancy_details.size(), occupancy_details.begin()); + const auto child_occupancy_scales = Transform::backward( + {stack_element.occupancy_scale, occupancy_details}); + + std::array child_layer_scales; + for (size_t layer_i = 0u; layer_i < layered_map_msg.layer_types.size(); ++layer_i) { + const int component_count = continuousLayerComponentCount(layered_map_msg.layer_types[layer_i]); + if (component_count == 0) { + continue; + } + const wavemap_msgs::Layer* detail_layer = + layer_i < node_msg.detail_layers.size() ? &node_msg.detail_layers[layer_i] : nullptr; + for (int component_i = 0; component_i < component_count; ++component_i) { + Coefficients::Details detail_values; + for (size_t coefficient_i = 0u; coefficient_i < detail_values.size(); ++coefficient_i) { + detail_values[coefficient_i] = detail_layer ? readFloatLayerValue(*detail_layer, coefficient_i, component_count, component_i) : 0.f; + } + const FloatingPoint parent_value = + stack_element.layer_scales.size() > layer_i && + stack_element.layer_scales[layer_i].size() > static_cast(component_i) + ? stack_element.layer_scales[layer_i][component_i] + : 0.f; + const auto child_values = Transform::backward({parent_value, detail_values}); + for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (child_layer_scales[child_idx].size() <= layer_i) { + child_layer_scales[child_idx].resize(layer_i + 1u); + } + child_layer_scales[child_idx][layer_i].push_back(child_values[child_idx]); + } + } + } + + struct ChildToVisit { + OctreeIndex node_index; + FloatingPoint occupancy_scale; + LayerValues layer_scales; + bool render_descendants; + }; + std::vector children_to_visit; + + for (NdtreeIndexRelativeChild child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = stack_element.node_index.computeChildIndex(child_idx); + const FloatingPoint child_occupancy_scale = child_occupancy_scales[child_idx]; + const bool child_exists = bit_ops::is_bit_set(node_msg.allocated_children_bitset, child_idx); + + if (child_exists) { + const bool render_child_descendants = + stack_element.render_descendants && + termination_height < child_node_index.height; + children_to_visit.push_back( + {child_node_index, child_occupancy_scale, + std::move(child_layer_scales[child_idx]), + render_child_descendants}); + if (stack_element.render_descendants && + !render_child_descendants) { + append_leaf(child_node_index, child_occupancy_scale, + children_to_visit.back().layer_scales); + } + } else if (stack_element.render_descendants) { + append_leaf(child_node_index, child_occupancy_scale, + child_layer_scales[child_idx]); + } + } + + // The serialized node array follows the same DFS order as the stack traversal. + // Push in reverse so the next serialized node is popped first. + for (auto child_it = children_to_visit.rbegin(); child_it != children_to_visit.rend(); ++child_it) { + stack.emplace(StackElement{child_it->node_index, + child_it->occupancy_scale, + std::move(child_it->layer_scales), + child_it->render_descendants}); + } + } + + if (next_node_msg_index != block_msg.nodes.size()) { + ROS_WARN("Layered map block contains unread serialized nodes."); + } +} + void VoxelVisual::processBlockUpdateQueue(const Point3D& camera_position) { ProfilerZoneScoped; if (!visibility_property_.getBool()) { diff --git a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc index a9452c92e..21e76b8c6 100644 --- a/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc +++ b/interfaces/ros1/wavemap_rviz_plugin/src/wavemap_map_display.cc @@ -1,10 +1,12 @@ #include "wavemap_rviz_plugin/wavemap_map_display.h" +#include #include #include #include #include +#include #include #include #include @@ -15,6 +17,7 @@ #include "wavemap_rviz_plugin/utils/alert_dialog.h" + namespace wavemap::rviz_plugin { WavemapMapDisplay::WavemapMapDisplay() { // Initialize the property menu @@ -23,6 +26,12 @@ WavemapMapDisplay::WavemapMapDisplay() { source_mode_property_.addOption(name); } source_mode_property_.setStringStd(source_mode_.toStr()); + + // The layered map selector is only useful after receiving a layered map message. + layer_property_.clearOptions(); + layer_property_.addOption(QString::fromStdString(selected_layer_name_)); + layer_property_.setStringStd(selected_layer_name_); + layer_property_.setHidden(true); } // After the top-level rviz::Display::initialize() does its own setup, @@ -51,7 +60,7 @@ void WavemapMapDisplay::reset() { bool WavemapMapDisplay::hasMap() { ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); - return static_cast(map_and_mutex_->map); + return static_cast(map_and_mutex_->map) || static_cast(map_and_mutex_->layered_map) || map_and_mutex_->layered_map_msg.has_value(); } void WavemapMapDisplay::clearMap() { @@ -60,11 +69,15 @@ void WavemapMapDisplay::clearMap() { if (map_and_mutex_->map) { map_and_mutex_->map->clear(); } + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); } bool WavemapMapDisplay::loadMapFromDisk(const std::filesystem::path& filepath) { ProfilerZoneScoped; std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); return io::fileToMap(filepath, map_and_mutex_->map); } @@ -117,12 +130,76 @@ void WavemapMapDisplay::processMessage( void WavemapMapDisplay::updateMapFromRosMsg(const wavemap_msgs::Map& map_msg) { ProfilerZoneScoped; + updateLayerMetadataFromRosMsg(map_msg); + + const bool has_layered_map = map_msg.layered_hashed_wavelet_octree.size() == 1u; + const bool has_legacy_map = !map_msg.hashed_blocks.empty() || !map_msg.wavelet_octree.empty() || !map_msg.hashed_wavelet_octree.empty(); + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->selected_layer_name = selected_layer_name_; + if (has_layered_map && !has_legacy_map) { + map_and_mutex_->map.reset(); + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); + return; + } + + map_and_mutex_->layered_map.reset(); + map_and_mutex_->layered_map_msg.reset(); if (!convert::rosMsgToMap(map_msg, map_and_mutex_->map)) { ROS_WARN("Failed to parse map message."); } } + +void WavemapMapDisplay::updateLayerMetadataFromRosMsg(const wavemap_msgs::Map& map_msg) { + ProfilerZoneScoped; + available_layers_.clear(); + layer_property_.clearOptions(); + layer_property_.addOption("occupancy"); + + if (map_msg.layered_hashed_wavelet_octree.empty()) { + selected_layer_name_ = "occupancy"; + layer_property_.setStringStd(selected_layer_name_); + layer_property_.setHidden(true); + return; + } + + const auto& layered_map_msg = map_msg.layered_hashed_wavelet_octree.front(); + const size_t num_layers = std::min(layered_map_msg.layer_names.size(), layered_map_msg.layer_types.size()); + for (size_t layer_idx = 0u; layer_idx < num_layers; ++layer_idx) { + available_layers_.push_back({layered_map_msg.layer_names[layer_idx], + layered_map_msg.layer_types[layer_idx], + {}, {}}); + layer_property_.addOption(QString::fromStdString(layered_map_msg.layer_names[layer_idx])); + } + + const bool selected_layer_still_exists = + selected_layer_name_ == "occupancy" || + std::any_of(available_layers_.begin(), available_layers_.end(), + [this](const LayerMetadata& layer) { + return layer.name == selected_layer_name_; + }); + if (!selected_layer_still_exists) { + selected_layer_name_ = "occupancy"; + } + + layer_property_.setStringStd(selected_layer_name_); + layer_property_.setHidden(false); + + ROS_DEBUG_STREAM("Received layered map metadata with " << available_layers_.size() << " custom layers."); +} + +void WavemapMapDisplay::updateLayerSelectionCallback() { + ProfilerZoneScoped; + selected_layer_name_ = layer_property_.getStdString(); + { + std::scoped_lock lock(map_and_mutex_->mutex); + map_and_mutex_->selected_layer_name = selected_layer_name_; + } + updateVisuals(true); +} + void WavemapMapDisplay::updateSourceModeCallback() { ProfilerZoneScoped; // Update the cached source mode value diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt index 1a94b0a8d..d5528d554 100644 --- a/library/cpp/CMakeLists.txt +++ b/library/cpp/CMakeLists.txt @@ -54,6 +54,10 @@ if (GENERATE_WAVEMAP_INSTALL_RULES) install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/3rd_party/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/3rd_party FILES_MATCHING PATTERN "*.h") + # Mark layered map headers for installation + install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/wavemap/layered/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/wavemap/layered + FILES_MATCHING PATTERN "*.h") # Install the CMake exports file install(EXPORT wavemap-targets FILE wavemap-targets.cmake diff --git a/library/cpp/include/wavemap/core/data_structure/pointcloud.h b/library/cpp/include/wavemap/core/data_structure/pointcloud.h index fcf731555..79d8c464a 100644 --- a/library/cpp/include/wavemap/core/data_structure/pointcloud.h +++ b/library/cpp/include/wavemap/core/data_structure/pointcloud.h @@ -38,6 +38,10 @@ class Pointcloud { CHECK_LT(n_points, std::numeric_limits::max()); data_.resize(kDim, n_points); } + void conservativeResize(size_t n_points) { + CHECK_LT(n_points, std::numeric_limits::max()); + data_.conservativeResize(Eigen::NoChange, n_points); + } void clear() { data_.resize(kDim, 0); } typename Data::ColXpr operator[](Eigen::Index point_index) { diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h index a8ebabc56..a2ed8b552 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_CHUNKED_WAVELET_INTEGRATOR_H_ +#include #include #include #include @@ -13,15 +14,20 @@ #include "wavemap/core/utils/thread_pool.h" namespace wavemap { -class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { +template +class HashedChunkedWaveletIntegratorT : public ProjectiveIntegrator { public: - HashedChunkedWaveletIntegrator( + using MapType = HashedChunkedWaveletOctreeT; + using MapPtr = typename MapType::Ptr; + using Block = typename MapType::Block; + using BlockIndex = typename MapType::BlockIndex; + HashedChunkedWaveletIntegratorT( const ProjectiveIntegratorConfig& config, ProjectorBase::ConstPtr projection_model, PosedImage<>::Ptr posed_range_image, Image::Ptr beam_offset_image, MeasurementModelBase::ConstPtr measurement_model, - HashedChunkedWaveletOctree::Ptr occupancy_map, + MapPtr occupancy_map, std::shared_ptr thread_pool = nullptr) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), @@ -31,10 +37,10 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { : std::make_shared()) {} private: - using BlockList = std::vector; - using OctreeType = HashedChunkedWaveletOctreeBlock::OctreeType; + using BlockList = std::vector; + using OctreeType = typename Block::OctreeType; - const HashedChunkedWaveletOctree::Ptr occupancy_map_; + const MapPtr occupancy_map_; const std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; @@ -56,23 +62,38 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator { std::log2(config_.max_update_resolution / min_cell_width_))) : 0; + static FloatingPoint occupancyOf(const CellDataT& value) { + return static_cast(value); + } + + static void addClampedOccupancyUpdate(CellDataT& value, + FloatingPoint update, + FloatingPoint min_value, + FloatingPoint max_value) { + value += update; + value = std::clamp(occupancyOf(value), min_value, max_value); + } + std::pair getFovMinMaxIndices( const Point3D& sensor_origin) const; void recursiveTester(const OctreeIndex& node_index, BlockList& update_job_list); void updateMap() override; - void updateBlock(HashedChunkedWaveletOctree::Block& block, - const HashedChunkedWaveletOctree::BlockIndex& block_index); + void updateBlock(Block& block, + const BlockIndex& block_index); - void updateNodeRecursive(OctreeType::NodeRefType node, + void updateNodeRecursive(typename OctreeType::NodeRefType node, const OctreeIndex& node_index, - FloatingPoint& node_value, + CellDataT& node_value, bool& block_needs_thresholding); void updateLeavesBatch(const OctreeIndex& parent_index, - FloatingPoint& parent_value, - OctreeType::NodeDataType& parent_details); + CellDataT& parent_value, + typename OctreeType::NodeDataType& parent_details); }; + +using HashedChunkedWaveletIntegrator = + HashedChunkedWaveletIntegratorT; } // namespace wavemap #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h" diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h index aac70459a..2d216f2c4 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h @@ -1,6 +1,7 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_HASHED_WAVELET_INTEGRATOR_H_ +#include #include #include #include @@ -12,15 +13,20 @@ #include "wavemap/core/utils/thread_pool.h" namespace wavemap { -class HashedWaveletIntegrator : public ProjectiveIntegrator { +template +class HashedWaveletIntegratorT : public ProjectiveIntegrator { public: - HashedWaveletIntegrator(const ProjectiveIntegratorConfig& config, - ProjectorBase::ConstPtr projection_model, - PosedImage<>::Ptr posed_range_image, - Image::Ptr beam_offset_image, - MeasurementModelBase::ConstPtr measurement_model, - HashedWaveletOctree::Ptr occupancy_map, - std::shared_ptr thread_pool = nullptr) + using MapType = HashedWaveletOctreeT; + using MapPtr = typename MapType::Ptr; + + HashedWaveletIntegratorT( + const ProjectiveIntegratorConfig& config, + ProjectorBase::ConstPtr projection_model, + PosedImage<>::Ptr posed_range_image, + Image::Ptr beam_offset_image, + MeasurementModelBase::ConstPtr measurement_model, + MapPtr occupancy_map, + std::shared_ptr thread_pool = nullptr) : ProjectiveIntegrator( config, std::move(projection_model), std::move(posed_range_image), std::move(beam_offset_image), std::move(measurement_model)), @@ -29,10 +35,13 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { : std::make_shared()) {} private: - using BlockList = std::vector; - using OctreeType = HashedWaveletOctreeBlock::OctreeType; + using BlockList = std::vector; + using Block = typename MapType::Block; + using BlockIndex = typename MapType::BlockIndex; + using OctreeType = typename Block::OctreeType; + using Coefficients = typename Block::Coefficients; - const HashedWaveletOctree::Ptr occupancy_map_; + const MapPtr occupancy_map_; const std::shared_ptr thread_pool_; std::shared_ptr range_image_intersector_; @@ -51,6 +60,18 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { static constexpr auto kUnitCubeHalfDiagonal = constants::kSqrt3 / 2.f; + static FloatingPoint occupancyOf(const CellDataT& value) { + return static_cast(value); + } + + static void addClampedOccupancyUpdate(CellDataT& value, + FloatingPoint update, + FloatingPoint min_log_odds, + FloatingPoint max_log_odds) { + value += update; + value = std::clamp(occupancyOf(value), min_log_odds, max_log_odds); + } + std::pair getFovMinMaxIndices( const Point3D& sensor_origin) const; @@ -58,9 +79,10 @@ class HashedWaveletIntegrator : public ProjectiveIntegrator { BlockList& update_job_list); void updateMap() override; - void updateBlock(HashedWaveletOctree::Block& block, - const HashedWaveletOctree::BlockIndex& block_index); + void updateBlock(Block& block, const BlockIndex& block_index); }; + +using HashedWaveletIntegrator = HashedWaveletIntegratorT; } // namespace wavemap #include "wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h" diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h index f6159ce93..d057e77a1 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h @@ -1,12 +1,173 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_CHUNKED_WAVELET_INTEGRATOR_INL_H_ +#include +#include #include +#include + namespace wavemap { -inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT +template +void HashedChunkedWaveletIntegratorT::updateMap() { + ProfilerZoneScoped; + // Update the range image intersector + { + ProfilerZoneScopedN("updateRangeImageIntersector"); + range_image_intersector_ = std::make_shared( + posed_range_image_, projection_model_, *measurement_model_, + config_.min_range, config_.max_range); + } + + // Find all the indices of blocks that need updating + BlockList blocks_to_update; + { + ProfilerZoneScopedN("selectBlocksToUpdate"); + const auto [fov_min_idx, fov_max_idx] = + getFovMinMaxIndices(posed_range_image_->getOrigin()); + for (const auto& block_index : + Grid(fov_min_idx.position, fov_max_idx.position)) { + recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, + blocks_to_update); + } + } + + // Make sure the to-be-updated blocks are allocated + for (const auto& block_index : blocks_to_update) { + occupancy_map_->getOrAllocateBlock(block_index); + } + + // Update it with the threadpool + for (const auto& block_index : blocks_to_update) { + thread_pool_->add_task([this, block_index]() { + if (auto* block = occupancy_map_->getBlock(block_index); block) { + updateBlock(*block, block_index); + } + }); + } + thread_pool_->wait_all(); +} + +template +std::pair +HashedChunkedWaveletIntegratorT::getFovMinMaxIndices( + const Point3D& sensor_origin) const { + const int height = 1 + std::max(static_cast(std::ceil(std::log2( + config_.max_range / min_cell_width_))), + tree_height_); + const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToFloorIndex<3>( + sensor_origin - Vector3D::Constant(config_.max_range), + min_cell_width_inv_) - + occupancy_map_->getBlockSize(), + height); + const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToCeilIndex<3>( + sensor_origin + Vector3D::Constant(config_.max_range), + min_cell_width_inv_) + + occupancy_map_->getBlockSize(), + height); + return {fov_min_idx, fov_max_idx}; +} + +template +void HashedChunkedWaveletIntegratorT::updateBlock( + Block& block, + const BlockIndex& block_index) { + ProfilerZoneScoped; + block.setNeedsPruning(); + block.setLastUpdatedStamp(); + + bool block_needs_thresholding = block.getNeedsThresholding(); + const OctreeIndex root_node_index{tree_height_, block_index}; + updateNodeRecursive(block.getRootNode(), root_node_index, + block.getRootScale(), block_needs_thresholding); + block.setNeedsThresholding(block_needs_thresholding); +} + +template +void HashedChunkedWaveletIntegratorT::updateNodeRecursive( // NOLINT + typename OctreeType::NodeRefType node, + const OctreeIndex& node_index, CellDataT& node_value, + bool& block_needs_thresholding) { + // Decompress child values + auto& node_details = node.data(); + auto child_values = Block::Transform::backward( + {node_value, node_details}); + + // Handle each child + for (NdtreeIndexRelativeChild relative_child_idx = 0; + relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { + const OctreeIndex child_index = + node_index.computeChildIndex(relative_child_idx); + CellDataT& child_value = child_values[relative_child_idx]; + + // Test whether it is fully occupied; free or unknown; or fully unknown + const AABB W_child_aabb = + convert::nodeIndexToAABB(child_index, min_cell_width_); + const UpdateType update_type = + range_image_intersector_->determineUpdateType( + W_child_aabb, posed_range_image_->getRotationMatrixInverse(), + posed_range_image_->getOrigin()); + + // If we're fully in unknown space, + // there's no need to evaluate this node or its children + if (update_type == UpdateType::kFullyUnobserved) { + continue; + } + + // We can also stop here if the cell will result in a free space update + // (or zero) and the map is already saturated free + if (update_type != UpdateType::kPossiblyOccupied && + occupancyOf(child_value) < min_log_odds_shrunk_) { + continue; + } + + // Test if the worst-case error for the intersection type at the current + // resolution falls within the acceptable approximation error + const FloatingPoint child_width = W_child_aabb.width<0>(); + const Point3D W_child_center = + W_child_aabb.min + Vector3D::Constant(child_width / 2.f); + const Point3D C_child_center = + posed_range_image_->getPoseInverse() * W_child_center; + const FloatingPoint d_C_child = + projection_model_->cartesianToSensorZ(C_child_center); + const FloatingPoint bounding_sphere_radius = + kUnitCubeHalfDiagonal * child_width; + if (measurement_model_->computeWorstCaseApproximationError( + update_type, d_C_child, bounding_sphere_radius) < + config_.termination_update_error) { + const FloatingPoint sample = computeUpdate(C_child_center); + child_value += sample; + block_needs_thresholding = true; + continue; + } + + // Since the approximation error would still be too big, refine + auto child_node = node.getOrAllocateChild(relative_child_idx); + auto& child_details = child_node.data(); + + // If we're at the leaf level, directly compute the update + if (child_index.height <= termination_height_ + 1) { + updateLeavesBatch(child_index, child_value, child_details); + } else { + // Otherwise, recurse + DCHECK_GE(child_index.height, 0); + updateNodeRecursive(child_node, child_index, child_value, + block_needs_thresholding); + } + } + + // Compress + const auto [new_value, new_details] = + Block::Transform::forward(child_values); + node_details = new_details; + node_value = new_value; +} +template +inline void HashedChunkedWaveletIntegratorT::recursiveTester( // NOLINT const OctreeIndex& node_index, - HashedChunkedWaveletIntegrator::BlockList& update_job_list) { + HashedChunkedWaveletIntegratorT::BlockList& update_job_list) { const AABB block_aabb = convert::nodeIndexToAABB(node_index, min_cell_width_); const UpdateType update_type = range_image_intersector_->determineUpdateType( @@ -24,7 +185,7 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT } if (const auto* block = occupancy_map_->getBlock(node_index.position); block) { - if (min_log_odds_shrunk_ <= block->getRootScale()) { + if (min_log_odds_shrunk_ <= occupancyOf(block->getRootScale())) { // Add the block to the job list update_job_list.emplace_back(node_index.position); } @@ -37,11 +198,12 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester( // NOLINT } } -inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( - const OctreeIndex& parent_index, FloatingPoint& parent_value, - HashedChunkedWaveletIntegrator::OctreeType::NodeDataType& parent_details) { +template +inline void HashedChunkedWaveletIntegratorT::updateLeavesBatch( + const OctreeIndex& parent_index, CellDataT& parent_value, + typename OctreeType::NodeDataType& parent_details) { // Decompress - auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( + auto child_values = Block::Transform::backward( {parent_value, parent_details}); // Get child center points in world frame W @@ -61,22 +223,14 @@ inline void HashedChunkedWaveletIntegrator::updateLeavesBatch( // Compute updated values for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { const FloatingPoint sample = computeUpdate(child_centers.col(child_idx)); - FloatingPoint& child_value = child_values[child_idx]; - child_value = sample + child_value; - } - - // Threshold - for (int child_idx = 0; child_idx < OctreeIndex::kNumChildren; ++child_idx) { - FloatingPoint& child_value = child_values[child_idx]; - child_value = (child_value < min_log_odds_padded_) ? min_log_odds_padded_ - : child_value; - child_value = (max_log_odds_padded_ < child_value) ? max_log_odds_padded_ - : child_value; + CellDataT& child_value = child_values[child_idx]; + addClampedOccupancyUpdate(child_value, sample, + min_log_odds_padded_, max_log_odds_padded_); } // Compress const auto [new_value, new_details] = - HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); + Block::Transform::forward(child_values); parent_details = new_details; parent_value = new_value; } diff --git a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h index 279c90099..e95abb00e 100644 --- a/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h +++ b/library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/impl/hashed_wavelet_integrator_inl.h @@ -1,12 +1,79 @@ #ifndef WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ #define WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ +#include +#include #include +#include + namespace wavemap { -inline void HashedWaveletIntegrator::recursiveTester( // NOLINT +template +void HashedWaveletIntegratorT::updateMap() { + ProfilerZoneScoped; + // Update the range image intersector + { + ProfilerZoneScopedN("updateRangeImageIntersector"); + range_image_intersector_ = std::make_shared( + posed_range_image_, projection_model_, *measurement_model_, + config_.min_range, config_.max_range); + } + + // Find all the indices of blocks that need updating + BlockList blocks_to_update; + { + ProfilerZoneScopedN("selectBlocksToUpdate"); + const auto [fov_min_idx, fov_max_idx] = + getFovMinMaxIndices(posed_range_image_->getOrigin()); + for (const auto& block_index : + Grid(fov_min_idx.position, fov_max_idx.position)) { + recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, + blocks_to_update); + } + } + + // Make sure the to-be-updated blocks are allocated + for (const auto& block_index : blocks_to_update) { + occupancy_map_->getOrAllocateBlock(block_index); + } + + // Update it with the threadpool + for (const auto& block_index : blocks_to_update) { + thread_pool_->add_task([this, block_index]() { + if (auto* block = occupancy_map_->getBlock(block_index); block) { + updateBlock(*block, block_index); + } + }); + } + thread_pool_->wait_all(); +} + +template +std::pair +HashedWaveletIntegratorT::getFovMinMaxIndices( + const Point3D& sensor_origin) const { + const int height = 1 + std::max(static_cast(std::ceil(std::log2( + config_.max_range / min_cell_width_))), + tree_height_); + const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToFloorIndex<3>( + sensor_origin - Vector3D::Constant(config_.max_range), + min_cell_width_inv_) - + occupancy_map_->getBlockSize(), + height); + const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( + convert::pointToCeilIndex<3>( + sensor_origin + Vector3D::Constant(config_.max_range), + min_cell_width_inv_) + + occupancy_map_->getBlockSize(), + height); + return {fov_min_idx, fov_max_idx}; +} + +template +void HashedWaveletIntegratorT::recursiveTester( // NOLINT const OctreeIndex& node_index, - HashedWaveletIntegrator::BlockList& update_job_list) { + HashedWaveletIntegratorT::BlockList& update_job_list) { const AABB block_aabb = convert::nodeIndexToAABB(node_index, min_cell_width_); const UpdateType update_type = range_image_intersector_->determineUpdateType( @@ -24,7 +91,8 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT } if (const auto* block = occupancy_map_->getBlock(node_index.position); block) { - if (min_log_odds_ + kNoiseThreshold / 10.f <= block->getRootScale()) { + if (min_log_odds_ + kNoiseThreshold / 10.f <= + occupancyOf(block->getRootScale())) { // Add the block to the job list update_job_list.emplace_back(node_index.position); } @@ -36,6 +104,136 @@ inline void HashedWaveletIntegrator::recursiveTester( // NOLINT recursiveTester(child_index, update_job_list); } } + +template +void HashedWaveletIntegratorT::updateBlock( + Block& block, const BlockIndex& block_index) { + ProfilerZoneScoped; + block.setNeedsPruning(); + block.setLastUpdatedStamp(); + + struct StackElement { + typename OctreeType::NodeRefType parent_node; + const OctreeIndex parent_node_index; + NdtreeIndexRelativeChild next_child_idx; + typename Coefficients::CoefficientsArray child_scale_coefficients; + }; + std::stack stack; + + typename OctreeType::NodeRefType root_node = block.getRootNode(); + typename Coefficients::Scale& root_node_scale = block.getRootScale(); + stack.emplace(StackElement{root_node, + {tree_height_, block_index}, + 0, + Block::Transform::backward( + {root_node_scale, root_node.data()})}); + + while (!stack.empty()) { + // If the current stack element has fully been processed, propagate upward + if (OctreeIndex::kNumChildren <= stack.top().next_child_idx) { + const auto [scale, details] = + Block::Transform::forward(stack.top().child_scale_coefficients); + stack.top().parent_node.data() = details; + stack.pop(); + if (stack.empty()) { + root_node_scale = scale; + return; + } else { + const NdtreeIndexRelativeChild current_child_idx = + stack.top().next_child_idx - 1; + stack.top().child_scale_coefficients[current_child_idx] = scale; + continue; + } + } + + // Evaluate stack element's active child + const NdtreeIndexRelativeChild current_child_idx = + stack.top().next_child_idx; + ++stack.top().next_child_idx; + DCHECK_GE(current_child_idx, 0); + DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); + + typename OctreeType::NodeRefType parent_node = stack.top().parent_node; + CellDataT& node_value = + stack.top().child_scale_coefficients[current_child_idx]; + const OctreeIndex node_index = + stack.top().parent_node_index.computeChildIndex(current_child_idx); + DCHECK_GE(node_index.height, 0); + + // If we're at the leaf level, directly update the node + if (node_index.height <= termination_height_) { + const Point3D W_node_center = + convert::nodeIndexToCenterPoint(node_index, min_cell_width_); + const Point3D C_node_center = + posed_range_image_->getPoseInverse() * W_node_center; + const FloatingPoint sample = computeUpdate(C_node_center); + addClampedOccupancyUpdate(node_value, sample, + min_log_odds_ - kNoiseThreshold, + max_log_odds_ + kNoiseThreshold); + continue; + } + + // Otherwise, test whether the current node is fully occupied; + // free or unknown; or fully unknown + const AABB W_cell_aabb = + convert::nodeIndexToAABB(node_index, min_cell_width_); + const UpdateType update_type = + range_image_intersector_->determineUpdateType( + W_cell_aabb, posed_range_image_->getRotationMatrixInverse(), + posed_range_image_->getOrigin()); + + // If we're fully in unknown space, + // there's no need to evaluate this node or its children + if (update_type == UpdateType::kFullyUnobserved) { + continue; + } + + // We can also stop here if the cell will result in a free space update + // (or zero) and the map is already saturated free + if (update_type != UpdateType::kPossiblyOccupied && + occupancyOf(node_value) < min_log_odds_ + kNoiseThreshold / 10.f) { + continue; + } + + // Test if the worst-case error for the intersection type at the current + // resolution falls within the acceptable approximation error + const FloatingPoint node_width = W_cell_aabb.width<0>(); + const Point3D W_node_center = + W_cell_aabb.min + Vector3D::Constant(node_width / 2.f); + const Point3D C_node_center = + posed_range_image_->getPoseInverse() * W_node_center; + const FloatingPoint d_C_cell = + projection_model_->cartesianToSensorZ(C_node_center); + const FloatingPoint bounding_sphere_radius = + kUnitCubeHalfDiagonal * node_width; + typename OctreeType::NodePtrType node = + parent_node.getChild(node_index.computeRelativeChildIndex()); + if (measurement_model_->computeWorstCaseApproximationError( + update_type, d_C_cell, bounding_sphere_radius) < + config_.termination_update_error) { + const FloatingPoint sample = computeUpdate(C_node_center); + if (!node || !node->hasAtLeastOneChild()) { + addClampedOccupancyUpdate(node_value, sample, + min_log_odds_ - kNoiseThreshold, + max_log_odds_ + kNoiseThreshold); + } else { + node_value += sample; + block.setNeedsThresholding(); + } + continue; + } + + // Since the approximation error would still be too big, refine + if (!node) { + // Allocate the current node if it has not yet been allocated + node = &parent_node.getOrAllocateChild( + node_index.computeRelativeChildIndex()); + } + stack.emplace(StackElement{*node, node_index, 0, + Block::Transform::backward( + {node_value, node->data()})}); + } +} } // namespace wavemap #endif // WAVEMAP_CORE_INTEGRATOR_PROJECTIVE_COARSE_TO_FINE_IMPL_HASHED_WAVELET_INTEGRATOR_INL_H_ diff --git a/library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h b/library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h new file mode 100644 index 000000000..bf0a5149e --- /dev/null +++ b/library/cpp/include/wavemap/core/map/cell_types/cell_data_traits.h @@ -0,0 +1,134 @@ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_CELL_DATA_TRAITS_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_CELL_DATA_TRAITS_H_ + +#include +#include +#include + +#include "wavemap/core/common.h" +#include "wavemap/core/map/cell_types/voxel_data.h" + +namespace wavemap { +namespace detail { +template +inline constexpr bool kAlwaysFalse = false; +} // namespace detail + +struct EmptyCellDataConfig {}; + +struct FieldPruningConfig { + FloatingPoint scale = 1e-3f; + FloatingPoint weight = 1.f; +}; + +inline FloatingPoint weightedPruningScore( + FloatingPoint magnitude, const FieldPruningConfig& config) { + if (config.weight <= 0.f || magnitude <= 0.f) { + return 0.f; + } + if (config.scale <= 0.f) { + return std::numeric_limits::infinity(); + } + return config.weight * magnitude / config.scale; +} + +template +struct CellDataTraits { + using ThresholdConfig = EmptyCellDataConfig; + using PruningConfig = EmptyCellDataConfig; + + static void threshold(CellDataT&, const ThresholdConfig&) { + static_assert(detail::kAlwaysFalse, + "CellDataTraits::threshold must be specialized for this " + "custom cell data type."); + } + + static FloatingPoint pruningScore(const CellDataT&, const PruningConfig&) { + static_assert(detail::kAlwaysFalse, + "CellDataTraits::pruningScore must be specialized for " + "this custom cell data type."); + return 0.f; + } + + static bool isNonzero(const CellDataT& value, const PruningConfig& config) { + return 1.f < pruningScore(value, config); + } +}; + +template <> +struct CellDataTraits { + using ThresholdConfig = EmptyCellDataConfig; + using PruningConfig = FieldPruningConfig; + + static void threshold(FloatingPoint&, const ThresholdConfig&) {} + + static FloatingPoint pruningScore(FloatingPoint value, + const PruningConfig& config) { + return weightedPruningScore(std::abs(value), config); + } + + static bool isNonzero(FloatingPoint value, const PruningConfig& config) { + return 1.f < pruningScore(value, config); + } +}; + +template +struct CellDataTraits> { + using AdditionalTraits = CellDataTraits; + + struct ThresholdConfig { + typename AdditionalTraits::ThresholdConfig data{}; + }; + + struct PruningConfig { + FieldPruningConfig occupancy{}; + typename AdditionalTraits::PruningConfig data{}; + FloatingPoint combined_threshold = 1.f; + }; + + static void threshold( + VoxelData& voxel, + const ThresholdConfig& config) { + AdditionalTraits::threshold(voxel.data, config.data); + } + + static FloatingPoint pruningScore( + const VoxelData& voxel, + const PruningConfig& config) { + return weightedPruningScore(std::abs(voxel.occupancy), config.occupancy) + + AdditionalTraits::pruningScore(voxel.data, config.data); + } + + static bool isNonzero( + const VoxelData& voxel, + const PruningConfig& config) { + return config.combined_threshold < pruningScore(voxel, config); + } +}; + +template +struct CellDataTraits> { + using ThresholdConfig = EmptyCellDataConfig; + + struct PruningConfig { + FieldPruningConfig occupancy{}; + FloatingPoint combined_threshold = 1.f; + }; + + static void threshold(VoxelData&, + const ThresholdConfig&) {} + + static FloatingPoint pruningScore( + const VoxelData& voxel, + const PruningConfig& config) { + return weightedPruningScore(std::abs(voxel.occupancy), config.occupancy); + } + + static bool isNonzero(const VoxelData& voxel, + const PruningConfig& config) { + return config.combined_threshold < pruningScore(voxel, config); + } +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_CELL_DATA_TRAITS_H_ diff --git a/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h b/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h new file mode 100644 index 000000000..5aacbe662 --- /dev/null +++ b/library/cpp/include/wavemap/core/map/cell_types/voxel_data.h @@ -0,0 +1,259 @@ +#ifndef WAVEMAP_CORE_MAP_CELL_TYPES_VOXEL_DATA_H_ +#define WAVEMAP_CORE_MAP_CELL_TYPES_VOXEL_DATA_H_ + +#include "wavemap/core/common.h" + +namespace wavemap { +template +struct DefaultVoxelDataPolicy { + static AdditionalDataT add(const AdditionalDataT& lhs, + const AdditionalDataT& rhs) { + return lhs + rhs; + } + + static AdditionalDataT subtract(const AdditionalDataT& lhs, + const AdditionalDataT& rhs) { + return lhs - rhs; + } + + static AdditionalDataT scale(const AdditionalDataT& data, + FloatingPoint factor) { + return data * factor; + } +}; + +template <> +struct DefaultVoxelDataPolicy {}; + +/** + * @brief Voxel data with mandatory occupancy and optional user data. + * + * AdditionalDataPolicyT defines the arithmetic of the additional data when this + * type is used in numerical algorithms such as wavelet transforms. + */ +template > +struct VoxelData { + using AdditionalData = AdditionalDataT; + using AdditionalDataPolicy = AdditionalDataPolicyT; + + FloatingPoint occupancy = 0.f; + AdditionalDataT data{}; + + VoxelData() = default; + explicit VoxelData(FloatingPoint occupancy_value) + : occupancy(occupancy_value) {} + VoxelData(FloatingPoint occupancy_value, const AdditionalDataT& data_value) + : occupancy(occupancy_value), data(data_value) {} + + operator FloatingPoint() const { return occupancy; } // NOLINT + + VoxelData& operator=(FloatingPoint value) { + occupancy = value; + return *this; + } + + bool operator==(const VoxelData& other) const { + return occupancy == other.occupancy && data == other.data; + } + bool operator!=(const VoxelData& other) const { return !(*this == other); } + + VoxelData& operator+=(FloatingPoint value) { + occupancy += value; + return *this; + } + + VoxelData& operator+=(const VoxelData& other) { + occupancy += other.occupancy; + data = AdditionalDataPolicyT::add(data, other.data); + return *this; + } + + VoxelData& operator-=(const VoxelData& other) { + occupancy -= other.occupancy; + data = AdditionalDataPolicyT::subtract(data, other.data); + return *this; + } + + VoxelData& operator*=(FloatingPoint factor) { + occupancy *= factor; + data = AdditionalDataPolicyT::scale(data, factor); + return *this; + } + + VoxelData& operator/=(FloatingPoint factor) { + occupancy /= factor; + data = AdditionalDataPolicyT::scale(data, 1.f / factor); + return *this; + } + + VoxelData& operator*=(const VoxelData& scalar) { + return *this *= scalar.occupancy; + } + + VoxelData& operator/=(const VoxelData& scalar) { + return *this /= scalar.occupancy; + } + + VoxelData operator+(FloatingPoint value) const { + VoxelData result = *this; + result.occupancy += value; + return result; + } + + friend VoxelData operator+(VoxelData lhs, const VoxelData& rhs) { + lhs += rhs; + return lhs; + } + + friend VoxelData operator-(VoxelData lhs, const VoxelData& rhs) { + lhs -= rhs; + return lhs; + } + + friend VoxelData operator+(VoxelData value) { return value; } + + friend VoxelData operator-(VoxelData value) { + value.occupancy = -value.occupancy; + value.data = AdditionalDataPolicyT::scale(value.data, -1.f); + return value; + } + + friend VoxelData operator*(FloatingPoint factor, VoxelData value) { + value *= factor; + return value; + } + + friend VoxelData operator*(VoxelData value, FloatingPoint factor) { + value *= factor; + return value; + } + + // Supports generic numeric code that represents scalars as ValueT objects, + // e.g. static_cast(0.5f) * voxel in Haar transforms. + friend VoxelData operator*(const VoxelData& scalar, VoxelData value) { + value *= scalar.occupancy; + return value; + } + + friend VoxelData operator/(VoxelData value, FloatingPoint factor) { + value /= factor; + return value; + } + + friend VoxelData operator/(VoxelData value, const VoxelData& scalar) { + value /= scalar.occupancy; + return value; + } +}; + +/** + * @brief Occupancy-only specialization. + */ +template +struct VoxelData { + FloatingPoint occupancy = 0.f; + + VoxelData() = default; + explicit VoxelData(FloatingPoint occupancy_value) + : occupancy(occupancy_value) {} + + operator FloatingPoint() const { return occupancy; } // NOLINT + + VoxelData& operator=(FloatingPoint value) { + occupancy = value; + return *this; + } + + bool operator==(const VoxelData& other) const { + return occupancy == other.occupancy; + } + bool operator!=(const VoxelData& other) const { return !(*this == other); } + + VoxelData& operator+=(FloatingPoint value) { + occupancy += value; + return *this; + } + + VoxelData& operator+=(const VoxelData& other) { + occupancy += other.occupancy; + return *this; + } + + VoxelData& operator-=(const VoxelData& other) { + occupancy -= other.occupancy; + return *this; + } + + VoxelData& operator*=(FloatingPoint factor) { + occupancy *= factor; + return *this; + } + + VoxelData& operator/=(FloatingPoint factor) { + occupancy /= factor; + return *this; + } + + VoxelData& operator*=(const VoxelData& scalar) { + occupancy *= scalar.occupancy; + return *this; + } + + VoxelData& operator/=(const VoxelData& scalar) { + occupancy /= scalar.occupancy; + return *this; + } + + VoxelData operator+(FloatingPoint value) const { + VoxelData result = *this; + result.occupancy += value; + return result; + } + + friend VoxelData operator+(VoxelData lhs, const VoxelData& rhs) { + lhs += rhs; + return lhs; + } + + friend VoxelData operator-(VoxelData lhs, const VoxelData& rhs) { + lhs -= rhs; + return lhs; + } + + friend VoxelData operator+(VoxelData value) { return value; } + + friend VoxelData operator-(VoxelData value) { + value.occupancy = -value.occupancy; + return value; + } + + friend VoxelData operator*(FloatingPoint factor, VoxelData value) { + value *= factor; + return value; + } + + friend VoxelData operator*(VoxelData value, FloatingPoint factor) { + value *= factor; + return value; + } + + friend VoxelData operator*(const VoxelData& lhs, VoxelData rhs) { + rhs *= lhs.occupancy; + return rhs; + } + + friend VoxelData operator/(VoxelData value, FloatingPoint factor) { + value /= factor; + return value; + } + + friend VoxelData operator/(VoxelData value, const VoxelData& scalar) { + value /= scalar.occupancy; + return value; + } +}; +} // namespace wavemap + +#endif // WAVEMAP_CORE_MAP_CELL_TYPES_VOXEL_DATA_H_ diff --git a/library/cpp/include/wavemap/core/map/hashed_blocks.h b/library/cpp/include/wavemap/core/map/hashed_blocks.h index 703badbb1..3c92339f2 100644 --- a/library/cpp/include/wavemap/core/map/hashed_blocks.h +++ b/library/cpp/include/wavemap/core/map/hashed_blocks.h @@ -25,6 +25,8 @@ class HashedBlocks : public MapBase, FloatingPoint default_value = 0.f) : MapBase(config.checkValid()), DenseBlockHash(default_value) {} + MapType getMapType() const override { return MapType::kHashedBlocks; } + bool empty() const override { return DenseBlockHash::empty(); } size_t size() const override { return DenseBlockHash::size(); } void threshold() override { diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h index 3d4ee2a43..3404cebf2 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h @@ -2,6 +2,7 @@ #define WAVEMAP_CORE_MAP_HASHED_CHUNKED_WAVELET_OCTREE_H_ #include +#include #include #include "wavemap/core/common.h" @@ -14,14 +15,11 @@ namespace wavemap { /** - * Config struct for the hashed chunked wavelet octree volumetric data - * structure. + * Config struct for the hashed wavelet octree volumetric data structure. */ -struct HashedChunkedWaveletOctreeConfig - : ConfigBase { +struct HashedChunkedWaveletOctreeConfig : ConfigBase { static constexpr IndexElement kMaxSupportedTreeHeight = HashedChunkedWaveletOctreeBlock::kMaxSupportedTreeHeight; - //! Maximum resolution of the map, set as the width of the smallest cell that //! it can represent. Meters min_cell_width = 0.1f; @@ -42,10 +40,11 @@ struct HashedChunkedWaveletOctreeConfig // Constructors HashedChunkedWaveletOctreeConfig() = default; - HashedChunkedWaveletOctreeConfig( - FloatingPoint min_cell_width, FloatingPoint min_log_odds, - FloatingPoint max_log_odds, IndexElement tree_height, - FloatingPoint only_prune_blocks_if_unused_for) + HashedChunkedWaveletOctreeConfig(FloatingPoint min_cell_width, + FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + IndexElement tree_height, + FloatingPoint only_prune_blocks_if_unused_for) : min_cell_width(min_cell_width), min_log_odds(min_log_odds), max_log_odds(max_log_odds), @@ -60,24 +59,42 @@ struct HashedChunkedWaveletOctreeConfig bool isValid(bool verbose) const override; }; -class HashedChunkedWaveletOctree : public MapBase { +template +class HashedChunkedWaveletOctreeT : public MapBase { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; using Config = HashedChunkedWaveletOctreeConfig; + using CellDataType = CellDataT; static constexpr bool kRequiresExplicitThresholding = true; using BlockIndex = Index3D; using CellIndex = OctreeIndex; - using Block = HashedChunkedWaveletOctreeBlock; + using Block = HashedChunkedWaveletOctreeBlockT; using BlockHashMap = SpatialHash; - - explicit HashedChunkedWaveletOctree( - const HashedChunkedWaveletOctreeConfig& config) - : MapBase(config), config_(config.checkValid()) {} + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedChunkedWaveletOctreeT( + const HashedChunkedWaveletOctreeConfig& config, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) + : MapBase(config), + config_(config.checkValid()), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} // Copy construction is not supported - HashedChunkedWaveletOctree(const HashedChunkedWaveletOctree&) = delete; + HashedChunkedWaveletOctreeT(const HashedChunkedWaveletOctreeT&) = delete; + + MapType getMapType() const override { + if constexpr (std::is_same_v) { + return MapType::kHashedChunkedWaveletOctree; + } else { + return MapType::kLayeredHashedWaveletOctree; + } + } bool empty() const override { return block_map_.empty(); } size_t size() const override; @@ -97,12 +114,19 @@ class HashedChunkedWaveletOctree : public MapBase { Index3D getBlockSize() const { return Index3D::Constant(cells_per_block_side_); } - const HashedChunkedWaveletOctreeConfig& getConfig() { return config_; } + const HashedChunkedWaveletOctreeConfig& getConfig() const { return config_; } + const ThresholdConfig& getThresholdConfig() const { + return threshold_config_; + } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const Index3D& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const Index3D& index, FloatingPoint new_value) override; + void setVoxelValue(const Index3D& index, const CellDataT& new_value); void addToCellValue(const Index3D& index, FloatingPoint update) override; + void addToVoxelValue(const Index3D& index, const CellDataT& update); bool hasBlock(const Index3D& block_index) const; bool eraseBlock(const BlockIndex& block_index); @@ -124,6 +148,9 @@ class HashedChunkedWaveletOctree : public MapBase { void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; + template + void forEachVoxelLeaf(IndexedVoxelLeafVisitorFunction visitor_fn) const; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; CellIndex indexToCellIndex(OctreeIndex index) const; @@ -131,9 +158,13 @@ class HashedChunkedWaveletOctree : public MapBase { const HashedChunkedWaveletOctreeConfig config_; const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; BlockHashMap block_map_; }; + +using HashedChunkedWaveletOctree = HashedChunkedWaveletOctreeT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h" diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h index 5c457414e..74d8485a3 100644 --- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h @@ -3,28 +3,39 @@ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h" +#include "wavemap/core/map/cell_types/cell_data_traits.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" #include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/cell_types/voxel_data.h" #include "wavemap/core/map/map_base.h" #include "wavemap/core/utils/time/time.h" namespace wavemap { -class HashedChunkedWaveletOctreeBlock { +template +class HashedChunkedWaveletOctreeBlockT { public: static constexpr int kDim = 3; static constexpr int kChunkHeight = 3; static constexpr int kMaxSupportedTreeHeight = 9; + using CellDataType = CellDataT; using BlockIndex = Index3D; - using Coefficients = HaarCoefficients; - using Transform = HaarTransform; - using OctreeType = ChunkedOctree; - - explicit HashedChunkedWaveletOctreeBlock(IndexElement tree_height, - FloatingPoint min_log_odds, - FloatingPoint max_log_odds) + using Coefficients = HaarCoefficients; + using Transform = HaarTransform; + using OctreeType = ChunkedOctree; + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedChunkedWaveletOctreeBlockT( + IndexElement tree_height, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) : tree_height_(tree_height), min_log_odds_(min_log_odds), - max_log_odds_(max_log_odds) {} + max_log_odds_(max_log_odds), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} bool empty() const; size_t size() const { return ndtree_.size(); } @@ -33,33 +44,42 @@ class HashedChunkedWaveletOctreeBlock { void clear(); FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const OctreeIndex& index, FloatingPoint new_value); + void setVoxelValue(const OctreeIndex& index, const CellDataT& new_value); void addToCellValue(const OctreeIndex& index, FloatingPoint update); + void addToVoxelValue(const OctreeIndex& index, const CellDataT& update); void forEachLeaf(const BlockIndex& block_index, typename MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height = 0) const; - Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } - const Coefficients::Scale& getRootScale() const { + template + void forEachVoxelLeaf(const BlockIndex& block_index, + IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; + + typename Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } - OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } - OctreeType::NodeConstRefType getRootNode() const { + const typename Coefficients::Scale& getRootScale() const { + return root_scale_coefficient_; + } + typename OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } - OctreeType::ChunkType& getRootChunk() { return ndtree_.getRootChunk(); } - const OctreeType::ChunkType& getRootChunk() const { + typename OctreeType::NodeConstRefType getRootNode() const { + return ndtree_.getRootNode(); + } + typename OctreeType::ChunkType& getRootChunk() { return ndtree_.getRootChunk(); } + const typename OctreeType::ChunkType& getRootChunk() const { return ndtree_.getRootChunk(); } void setNeedsPruning(bool value = true) { needs_pruning_ = value; } bool getNeedsPruning() const { return needs_pruning_; } - bool& getNeedsPruning() { return needs_pruning_; } - void setNeedsThresholding(bool value = true) { needs_thresholding_ = value; } bool getNeedsThresholding() const { return needs_thresholding_; } - void setLastUpdatedStamp(Timestamp stamp = Time::now()) { last_updated_stamp_ = stamp; } @@ -68,11 +88,11 @@ class HashedChunkedWaveletOctreeBlock { template auto getChunkIterator() { - return ndtree_.getChunkIterator(); + return ndtree_.template getChunkIterator(); } template auto getChunkIterator() const { - return ndtree_.getChunkIterator(); + return ndtree_.template getChunkIterator(); } size_t getMemoryUsage() const { return ndtree_.getMemoryUsage(); } @@ -81,19 +101,22 @@ class HashedChunkedWaveletOctreeBlock { const IndexElement tree_height_; const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; OctreeType ndtree_{tree_height_ - 1}; - Coefficients::Scale root_scale_coefficient_{}; + typename Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - void recursiveThreshold(OctreeType::NodeRefType node, - Coefficients::Scale& node_scale_coefficient); - void recursivePrune( - HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node); + void recursiveThreshold(typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient); + void recursivePrune(typename OctreeType::NodeRefType node); }; + +using HashedChunkedWaveletOctreeBlock = HashedChunkedWaveletOctreeBlockT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h" diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h index 986792a8f..f8c667ac1 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h @@ -2,6 +2,7 @@ #define WAVEMAP_CORE_MAP_HASHED_WAVELET_OCTREE_H_ #include +#include #include #include "wavemap/core/common.h" @@ -56,23 +57,42 @@ struct HashedWaveletOctreeConfig : ConfigBase { bool isValid(bool verbose) const override; }; -class HashedWaveletOctree : public MapBase { +template +class HashedWaveletOctreeT : public MapBase { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; using Config = HashedWaveletOctreeConfig; + using CellDataType = CellDataT; static constexpr bool kRequiresExplicitThresholding = true; using BlockIndex = Index3D; using CellIndex = OctreeIndex; - using Block = HashedWaveletOctreeBlock; + using Block = HashedWaveletOctreeBlockT; using BlockHashMap = SpatialHash; - - explicit HashedWaveletOctree(const HashedWaveletOctreeConfig& config) - : MapBase(config), config_(config.checkValid()) {} + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedWaveletOctreeT( + const HashedWaveletOctreeConfig& config, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) + : MapBase(config), + config_(config.checkValid()), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} // Copy construction is not supported - HashedWaveletOctree(const HashedWaveletOctree&) = delete; + HashedWaveletOctreeT(const HashedWaveletOctreeT&) = delete; + + MapType getMapType() const override { + if constexpr (std::is_same_v) { + return MapType::kHashedWaveletOctree; + } else { + return MapType::kLayeredHashedWaveletOctree; + } + } bool empty() const override { return block_map_.empty(); } size_t size() const override; @@ -91,12 +111,19 @@ class HashedWaveletOctree : public MapBase { Index3D getBlockSize() const { return Index3D::Constant(cells_per_block_side_); } - const HashedWaveletOctreeConfig& getConfig() { return config_; } + const HashedWaveletOctreeConfig& getConfig() const { return config_; } + const ThresholdConfig& getThresholdConfig() const { + return threshold_config_; + } FloatingPoint getCellValue(const Index3D& index) const override; FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const Index3D& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const Index3D& index, FloatingPoint new_value) override; + void setVoxelValue(const Index3D& index, const CellDataT& new_value); void addToCellValue(const Index3D& index, FloatingPoint update) override; + void addToVoxelValue(const Index3D& index, const CellDataT& update); bool hasBlock(const Index3D& block_index) const; bool eraseBlock(const BlockIndex& block_index); @@ -118,6 +145,9 @@ class HashedWaveletOctree : public MapBase { void forEachLeaf( typename MapBase::IndexedLeafVisitorFunction visitor_fn) const override; + template + void forEachVoxelLeaf(IndexedVoxelLeafVisitorFunction visitor_fn) const; + BlockIndex indexToBlockIndex(const OctreeIndex& node_index) const; CellIndex indexToCellIndex(OctreeIndex index) const; @@ -125,9 +155,13 @@ class HashedWaveletOctree : public MapBase { const HashedWaveletOctreeConfig config_; const IndexElement cells_per_block_side_ = int_math::exp2(config_.tree_height); + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; BlockHashMap block_map_; }; + +using HashedWaveletOctree = HashedWaveletOctreeT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_wavelet_octree_inl.h" diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h index b622b9372..fc5a77c50 100644 --- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h +++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree_block.h @@ -3,26 +3,37 @@ #include "wavemap/core/common.h" #include "wavemap/core/data_structure/ndtree/ndtree.h" +#include "wavemap/core/map/cell_types/cell_data_traits.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" #include "wavemap/core/map/cell_types/haar_transform.h" +#include "wavemap/core/map/cell_types/voxel_data.h" #include "wavemap/core/map/map_base.h" #include "wavemap/core/utils/time/time.h" namespace wavemap { -class HashedWaveletOctreeBlock { +template +class HashedWaveletOctreeBlockT { public: static constexpr int kDim = 3; + using CellDataType = CellDataT; using BlockIndex = Index3D; - using Coefficients = HaarCoefficients; - using Transform = HaarTransform; - using OctreeType = Octree; - - explicit HashedWaveletOctreeBlock(IndexElement tree_height, - FloatingPoint min_log_odds, - FloatingPoint max_log_odds) + using Coefficients = HaarCoefficients; + using Transform = HaarTransform; + using OctreeType = Octree; + using Traits = CellDataTraits; + using ThresholdConfig = typename Traits::ThresholdConfig; + using PruningConfig = typename Traits::PruningConfig; + + explicit HashedWaveletOctreeBlockT( + IndexElement tree_height, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + ThresholdConfig threshold_config = ThresholdConfig{}, + PruningConfig pruning_config = PruningConfig{}) : tree_height_(tree_height), min_log_odds_(min_log_odds), - max_log_odds_(max_log_odds) {} + max_log_odds_(max_log_odds), + threshold_config_(threshold_config), + pruning_config_(pruning_config) {} bool empty() const; size_t size() const { return ndtree_.size(); } @@ -31,19 +42,31 @@ class HashedWaveletOctreeBlock { void clear(); FloatingPoint getCellValue(const OctreeIndex& index) const; + CellDataT getVoxelValue(const OctreeIndex& index) const; void setCellValue(const OctreeIndex& index, FloatingPoint new_value); + void setVoxelValue(const OctreeIndex& index, const CellDataT& new_value); void addToCellValue(const OctreeIndex& index, FloatingPoint update); + void addToVoxelValue(const OctreeIndex& index, const CellDataT& update); void forEachLeaf(const BlockIndex& block_index, typename MapBase::IndexedLeafVisitorFunction visitor_fn, IndexElement termination_height = 0) const; - Coefficients::Scale& getRootScale() { return root_scale_coefficient_; } - const Coefficients::Scale& getRootScale() const { + template + void forEachVoxelLeaf(const BlockIndex& block_index, + IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height = 0) const; + + typename Coefficients::Scale& getRootScale() { + return root_scale_coefficient_; + } + const typename Coefficients::Scale& getRootScale() const { return root_scale_coefficient_; } - OctreeType::NodeRefType getRootNode() { return ndtree_.getRootNode(); } - OctreeType::NodeConstRefType getRootNode() const { + typename OctreeType::NodeRefType getRootNode() { + return ndtree_.getRootNode(); + } + typename OctreeType::NodeConstRefType getRootNode() const { return ndtree_.getRootNode(); } @@ -59,11 +82,11 @@ class HashedWaveletOctreeBlock { template auto getNodeIterator() { - return ndtree_.getIterator(); + return ndtree_.template getIterator(); } template auto getNodeIterator() const { - return ndtree_.getIterator(); + return ndtree_.template getIterator(); } size_t getMemoryUsage() const { return ndtree_.getMemoryUsage(); } @@ -72,18 +95,22 @@ class HashedWaveletOctreeBlock { const IndexElement tree_height_; const FloatingPoint min_log_odds_; const FloatingPoint max_log_odds_; + const ThresholdConfig threshold_config_; + const PruningConfig pruning_config_; OctreeType ndtree_{tree_height_ - 1}; - Coefficients::Scale root_scale_coefficient_{}; + typename Coefficients::Scale root_scale_coefficient_{}; bool needs_thresholding_ = false; bool needs_pruning_ = false; Timestamp last_updated_stamp_ = Time::now(); - void recursiveThreshold(OctreeType::NodeRefType node, - Coefficients::Scale& node_scale_coefficient); - void recursivePrune(OctreeType::NodeRefType node); + void recursiveThreshold(typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient); + void recursivePrune(typename OctreeType::NodeRefType node); }; + +using HashedWaveletOctreeBlock = HashedWaveletOctreeBlockT; } // namespace wavemap #include "wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h" diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h index 0cd6ae505..a4e28fc65 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_block_inl.h @@ -1,28 +1,96 @@ #ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ +#include +#include +#include +#include +#include + +#include "wavemap/core/utils/profile/profiler_interface.h" #include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { -inline bool HashedChunkedWaveletOctreeBlock::empty() const { - // Check if all cells in the block are equal to zero - // NOTE: Aside from checking whether the block contains no detail - // coefficients, we also need to check whether its scale coefficient - // (average value over the whole block) is zero. - return ndtree_.empty() && - OccupancyClassifier::isUnobserved(root_scale_coefficient_); +namespace chunked_detail { +template +FloatingPoint getOccupancy(const CellDataT& value) { + return static_cast(value); +} + +template +void setOccupancy(CellDataT& value, FloatingPoint occupancy) { + value = occupancy; +} + +template +struct HasVoxelDataFields : std::false_type {}; + +template +struct HasVoxelDataFields< + T, std::void_t().occupancy), + decltype(std::declval().data)>> : std::true_type {}; + +template +struct HasOccupancyField : std::false_type {}; + +template +struct HasOccupancyField().occupancy)>> + : std::true_type {}; + +template +void thresholdCellData(CellDataT& value, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + const typename CellDataTraits::ThresholdConfig& + threshold_config) { + if constexpr (std::is_arithmetic_v) { + value = std::clamp(value, min_log_odds, max_log_odds); + } else { + if constexpr (HasVoxelDataFields::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } else if constexpr (HasOccupancyField::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } + CellDataTraits::threshold(value, threshold_config); + } +} + +template +bool detailsHaveNonzeroData( + const typename HaarCoefficients::Details& details, + const typename CellDataTraits::PruningConfig& pruning_config) { + return std::any_of(details.cbegin(), details.cend(), + [&pruning_config](const CellDataT& value) { + return CellDataTraits::isNonzero( + value, pruning_config); + }); } -inline FloatingPoint HashedChunkedWaveletOctreeBlock::getTimeSinceLastUpdated() +} // namespace chunked_detail + +template +bool HashedChunkedWaveletOctreeBlockT::empty() const { + return ndtree_.empty() && OccupancyClassifier::isUnobserved( + chunked_detail::getOccupancy(root_scale_coefficient_)); +} + +template +FloatingPoint HashedChunkedWaveletOctreeBlockT::getTimeSinceLastUpdated() const { return time::to_seconds(Time::now() - last_updated_stamp_); } -inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( +template +FloatingPoint HashedChunkedWaveletOctreeBlockT::getCellValue( + const OctreeIndex& index) const { + return chunked_detail::getOccupancy(getVoxelValue(index)); +} + +template +CellDataT HashedChunkedWaveletOctreeBlockT::getVoxelValue( const OctreeIndex& index) const { const MortonIndex morton_code = convert::nodeIndexToMorton(index); - OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); - FloatingPoint value = root_scale_coefficient_; + typename OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); + CellDataT value = root_scale_coefficient_; for (int parent_height = tree_height_; node && index.height < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = @@ -32,6 +100,230 @@ inline FloatingPoint HashedChunkedWaveletOctreeBlock::getCellValue( } return value; } + +template +void HashedChunkedWaveletOctreeBlockT::threshold() { + ProfilerZoneScoped; + if (getNeedsThresholding()) { + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); + setNeedsThresholding(false); + } +} + +template +void HashedChunkedWaveletOctreeBlockT::prune() { + ProfilerZoneScoped; + if (getNeedsPruning()) { + threshold(); + recursivePrune(ndtree_.getRootNode()); + setNeedsPruning(false); + } +} + +template +void HashedChunkedWaveletOctreeBlockT::clear() { + ProfilerZoneScoped; + root_scale_coefficient_ = typename Coefficients::Scale{}; + ndtree_.clear(); + setLastUpdatedStamp(); +} + +template +void HashedChunkedWaveletOctreeBlockT::setCellValue( + const OctreeIndex& index, FloatingPoint new_value) { + CellDataT voxel = getVoxelValue(index); + chunked_detail::setOccupancy(voxel, new_value); + setVoxelValue(index, voxel); +} + +template +void HashedChunkedWaveletOctreeBlockT::setVoxelValue( + const OctreeIndex& index, const CellDataT& new_value) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(ndtree_.getRootNode()); + CellDataT current_value = root_scale_coefficient_; + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodeRefType current_parent = ancestors.back(); + current_value = Transform::backwardSingleChild( + {current_value, current_parent.data()}, child_index); + typename OctreeType::NodeRefType child = + current_parent.getOrAllocateChild(child_index); + ancestors.emplace_back(child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{new_value - current_value, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodeRefType current_node = ancestors.back(); + ancestors.pop_back(); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node.data() += coefficients.details; + } + + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedChunkedWaveletOctreeBlockT::addToCellValue( + const OctreeIndex& index, FloatingPoint update) { + addToVoxelValue(index, CellDataT{update}); +} + +template +void HashedChunkedWaveletOctreeBlockT::addToVoxelValue( + const OctreeIndex& index, const CellDataT& update) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(ndtree_.getRootNode()); + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodeRefType current_parent = ancestors.back(); + typename OctreeType::NodeRefType child = + current_parent.getOrAllocateChild(child_index); + ancestors.emplace_back(child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{update, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + typename OctreeType::NodeRefType current_node = ancestors.back(); + ancestors.pop_back(); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node.data() += coefficients.details; + } + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedChunkedWaveletOctreeBlockT::forEachLeaf( + const BlockIndex& block_index, + MapBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + forEachVoxelLeaf( + block_index, + [&visitor_fn](const OctreeIndex& node_index, + const CellDataT& voxel_value) { + visitor_fn(node_index, chunked_detail::getOccupancy(voxel_value)); + }, + termination_height); +} + +template +template +void HashedChunkedWaveletOctreeBlockT::forEachVoxelLeaf( + const BlockIndex& block_index, IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + ProfilerZoneScoped; + if (empty()) { + return; + } + + struct StackElement { + const OctreeIndex node_index; + typename OctreeType::NodeConstRefType node; + const typename Coefficients::Scale scale_coefficient{}; + }; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height_, block_index}, + ndtree_.getRootNode(), root_scale_coefficient_}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + typename OctreeType::NodeConstRefType node = stack.top().node; + const typename Coefficients::Scale node_scale_coefficient = + stack.top().scale_coefficient; + stack.pop(); + + const typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, {node.data()}}); + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + const typename Coefficients::Scale child_scale_coefficient = + child_scale_coefficients[child_idx]; + typename OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node, + child_scale_coefficient}); + } else { + visitor_fn(child_node_index, child_scale_coefficient); + } + } + } +} + +template +void HashedChunkedWaveletOctreeBlockT::recursiveThreshold( + typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient) { + auto& node_detail_coefficients = node.data(); + typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, node_detail_coefficients}); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + typename Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; + if (auto child_node = node.getChild(child_idx); child_node) { + recursiveThreshold(*child_node, child_scale); + } else { + chunked_detail::thresholdCellData(child_scale, min_log_odds_, max_log_odds_, + threshold_config_); + } + } + + const auto [new_scale, new_details] = + Transform::forward(child_scale_coefficients); + node_detail_coefficients = new_details; + node_scale_coefficient = new_scale; +} + +template +void HashedChunkedWaveletOctreeBlockT::recursivePrune( + typename OctreeType::NodeRefType node) { + bool has_at_least_one_child = false; + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (typename OctreeType::NodePtrType child_node = node.getChild(child_idx); + child_node) { + recursivePrune(*child_node); + if (!child_node->hasAtLeastOneChild() && + !chunked_detail::detailsHaveNonzeroData(child_node->data(), + pruning_config_)) { + node.eraseChild(child_idx); + } else { + has_at_least_one_child = true; + } + } + } + if (!has_at_least_one_child) { + node.hasAtLeastOneChild() = has_at_least_one_child; + } +} } // namespace wavemap #endif // WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h index f6182d909..a5c65ea66 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_chunked_wavelet_octree_inl.h @@ -2,9 +2,11 @@ #define WAVEMAP_CORE_MAP_IMPL_HASHED_CHUNKED_WAVELET_OCTREE_INL_H_ #include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/profile/profiler_interface.h" namespace wavemap { -inline size_t HashedChunkedWaveletOctree::size() const { +template +size_t HashedChunkedWaveletOctreeT::size() const { size_t size = 0u; forEachBlock([&size](const BlockIndex& /*block_index*/, const Block& block) { size += block.size(); @@ -12,7 +14,63 @@ inline size_t HashedChunkedWaveletOctree::size() const { return size; } -inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( +template +void HashedChunkedWaveletOctreeT::threshold() { + ProfilerZoneScoped; + forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { + block.threshold(); + }); +} + +template +void HashedChunkedWaveletOctreeT::prune() { + ProfilerZoneScoped; + block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { + block.prune(); + return block.empty(); + }); +} + +template +void HashedChunkedWaveletOctreeT::pruneSmart() { + ProfilerZoneScoped; + block_map_.eraseBlockIf( + [&config = config_](const BlockIndex& /*block_index*/, Block& block) { + if (config.only_prune_blocks_if_unused_for < + block.getTimeSinceLastUpdated()) { + block.prune(); + } + return block.empty(); + }); +} + +template +size_t HashedChunkedWaveletOctreeT::getMemoryUsage() const { + ProfilerZoneScoped; + // TODO(victorr): Also include the memory usage of the unordered map itself + size_t memory_usage = 0u; + forEachBlock( + [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { + memory_usage += block.getMemoryUsage(); + }); + return memory_usage; +} + +template +Index3D HashedChunkedWaveletOctreeT::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +template +Index3D HashedChunkedWaveletOctreeT::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + +template +FloatingPoint HashedChunkedWaveletOctreeT::getCellValue( const Index3D& index) const { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); @@ -24,7 +82,8 @@ inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( +template +FloatingPoint HashedChunkedWaveletOctreeT::getCellValue( const OctreeIndex& index) const { const BlockIndex block_index = indexToBlockIndex(index); const Block* block = getBlock(block_index); @@ -35,8 +94,34 @@ inline FloatingPoint HashedChunkedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline void HashedChunkedWaveletOctree::setCellValue(const Index3D& index, - FloatingPoint new_value) { +template +CellDataT HashedChunkedWaveletOctreeT::getVoxelValue( + const Index3D& index) const { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex({0, index}); + return block->getVoxelValue(cell_index); +} + +template +CellDataT HashedChunkedWaveletOctreeT::getVoxelValue( + const OctreeIndex& index) const { + const BlockIndex block_index = indexToBlockIndex(index); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex(index); + return block->getVoxelValue(cell_index); +} + +template +void HashedChunkedWaveletOctreeT::setCellValue(const Index3D& index, + FloatingPoint new_value) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -44,8 +129,19 @@ inline void HashedChunkedWaveletOctree::setCellValue(const Index3D& index, block.setCellValue(cell_index, new_value); } -inline void HashedChunkedWaveletOctree::addToCellValue(const Index3D& index, - FloatingPoint update) { +template +void HashedChunkedWaveletOctreeT::setVoxelValue( + const Index3D& index, const CellDataT& new_value) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.setVoxelValue(cell_index, new_value); +} + +template +void HashedChunkedWaveletOctreeT::addToCellValue(const Index3D& index, + FloatingPoint update) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -53,59 +149,100 @@ inline void HashedChunkedWaveletOctree::addToCellValue(const Index3D& index, block.addToCellValue(cell_index, update); } -inline bool HashedChunkedWaveletOctree::hasBlock( +template +void HashedChunkedWaveletOctreeT::addToVoxelValue( + const Index3D& index, const CellDataT& update) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.addToVoxelValue(cell_index, update); +} + +template +bool HashedChunkedWaveletOctreeT::hasBlock( const Index3D& block_index) const { return block_map_.hasBlock(block_index); } -inline bool HashedChunkedWaveletOctree::eraseBlock( - const HashedChunkedWaveletOctree::BlockIndex& block_index) { +template +bool HashedChunkedWaveletOctreeT::eraseBlock( + const typename HashedChunkedWaveletOctreeT::BlockIndex& block_index) { return block_map_.eraseBlock(block_index); } +template template -void HashedChunkedWaveletOctree::eraseBlockIf( +void HashedChunkedWaveletOctreeT::eraseBlockIf( IndexedBlockVisitor indicator_fn) { block_map_.eraseBlockIf(indicator_fn); } -inline HashedChunkedWaveletOctree::Block* HashedChunkedWaveletOctree::getBlock( - const Index3D& block_index) { +template +typename HashedChunkedWaveletOctreeT::Block* +HashedChunkedWaveletOctreeT::getBlock(const Index3D& block_index) { return block_map_.getBlock(block_index); } -inline const HashedChunkedWaveletOctree::Block* -HashedChunkedWaveletOctree::getBlock(const Index3D& block_index) const { +template +const typename HashedChunkedWaveletOctreeT::Block* +HashedChunkedWaveletOctreeT::getBlock(const Index3D& block_index) const { return block_map_.getBlock(block_index); } -inline HashedChunkedWaveletOctree::Block& -HashedChunkedWaveletOctree::getOrAllocateBlock(const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index, config_.tree_height, - config_.min_log_odds, - config_.max_log_odds); +template +typename HashedChunkedWaveletOctreeT::Block& +HashedChunkedWaveletOctreeT::getOrAllocateBlock( + const Index3D& block_index) { + return block_map_.getOrAllocateBlock( + block_index, config_.tree_height, config_.min_log_odds, + config_.max_log_odds, threshold_config_, pruning_config_); } +template template -void HashedChunkedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) { +void HashedChunkedWaveletOctreeT::forEachBlock( + IndexedBlockVisitor visitor_fn) { block_map_.forEachBlock(visitor_fn); } +template template -void HashedChunkedWaveletOctree::forEachBlock( +void HashedChunkedWaveletOctreeT::forEachBlock( IndexedBlockVisitor visitor_fn) const { block_map_.forEachBlock(visitor_fn); } -inline HashedChunkedWaveletOctree::BlockIndex -HashedChunkedWaveletOctree::indexToBlockIndex( +template +void HashedChunkedWaveletOctreeT::forEachLeaf( + MapBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachLeaf(block_index, visitor_fn); + }); +} + +template +template +void HashedChunkedWaveletOctreeT::forEachVoxelLeaf( + IndexedVoxelLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachVoxelLeaf(block_index, visitor_fn); + }); +} + +template +typename HashedChunkedWaveletOctreeT::BlockIndex +HashedChunkedWaveletOctreeT::indexToBlockIndex( const OctreeIndex& node_index) const { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); return convert::indexToBlockIndex(index, config_.tree_height); } -inline HashedChunkedWaveletOctree::CellIndex -HashedChunkedWaveletOctree::indexToCellIndex(OctreeIndex index) const { +template +typename HashedChunkedWaveletOctreeT::CellIndex +HashedChunkedWaveletOctreeT::indexToCellIndex(OctreeIndex index) const { DCHECK_LE(index.height, config_.tree_height); const IndexElement height_difference = config_.tree_height - index.height; index.position = diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h index cfbfeda34..fa05876db 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_block_inl.h @@ -1,27 +1,96 @@ #ifndef WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ #define WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ +#include +#include +#include +#include +#include + +#include "wavemap/core/utils/profile/profiler_interface.h" #include "wavemap/core/utils/query/occupancy_classifier.h" namespace wavemap { -inline bool HashedWaveletOctreeBlock::empty() const { - // Check if all cells in the block are equal to zero - // NOTE: Aside from checking whether the block contains no detail - // coefficients, we also need to check whether its scale coefficient - // (average value over the whole block) is zero. - return ndtree_.empty() && - OccupancyClassifier::isUnobserved(root_scale_coefficient_); +namespace detail { +template +FloatingPoint getOccupancy(const CellDataT& value) { + return static_cast(value); +} + +template +void setOccupancy(CellDataT& value, FloatingPoint occupancy) { + value = occupancy; +} + +template +struct HasVoxelDataFields : std::false_type {}; + +template +struct HasVoxelDataFields< + T, std::void_t().occupancy), + decltype(std::declval().data)>> : std::true_type {}; + +template +struct HasOccupancyField : std::false_type {}; + +template +struct HasOccupancyField().occupancy)>> + : std::true_type {}; + +template +void thresholdCellData(CellDataT& value, FloatingPoint min_log_odds, + FloatingPoint max_log_odds, + const typename CellDataTraits::ThresholdConfig& + threshold_config) { + if constexpr (std::is_arithmetic_v) { + value = std::clamp(value, min_log_odds, max_log_odds); + } else { + if constexpr (HasVoxelDataFields::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } else if constexpr (HasOccupancyField::value) { + value.occupancy = std::clamp(value.occupancy, min_log_odds, max_log_odds); + } + CellDataTraits::threshold(value, threshold_config); + } +} + +template +bool detailsHaveNonzeroData( + const typename HaarCoefficients::Details& details, + const typename CellDataTraits::PruningConfig& pruning_config) { + return std::any_of(details.cbegin(), details.cend(), + [&pruning_config](const CellDataT& value) { + return CellDataTraits::isNonzero( + value, pruning_config); + }); } -inline FloatingPoint HashedWaveletOctreeBlock::getTimeSinceLastUpdated() const { +} // namespace detail + +template +bool HashedWaveletOctreeBlockT::empty() const { + return ndtree_.empty() && OccupancyClassifier::isUnobserved( + detail::getOccupancy(root_scale_coefficient_)); +} + +template +FloatingPoint HashedWaveletOctreeBlockT::getTimeSinceLastUpdated() + const { return time::to_seconds(Time::now() - last_updated_stamp_); } -inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( +template +FloatingPoint HashedWaveletOctreeBlockT::getCellValue( + const OctreeIndex& index) const { + return detail::getOccupancy(getVoxelValue(index)); +} + +template +CellDataT HashedWaveletOctreeBlockT::getVoxelValue( const OctreeIndex& index) const { const MortonIndex morton_code = convert::nodeIndexToMorton(index); - OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); - FloatingPoint value = root_scale_coefficient_; + typename OctreeType::NodeConstPtrType node = &ndtree_.getRootNode(); + CellDataT value = root_scale_coefficient_; for (int parent_height = tree_height_; node && index.height < parent_height; --parent_height) { const NdtreeIndexRelativeChild child_index = @@ -31,6 +100,230 @@ inline FloatingPoint HashedWaveletOctreeBlock::getCellValue( } return value; } + +template +void HashedWaveletOctreeBlockT::threshold() { + ProfilerZoneScoped; + if (getNeedsThresholding()) { + recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); + setNeedsThresholding(false); + } +} + +template +void HashedWaveletOctreeBlockT::prune() { + ProfilerZoneScoped; + if (getNeedsPruning()) { + threshold(); + recursivePrune(ndtree_.getRootNode()); + setNeedsPruning(false); + } +} + +template +void HashedWaveletOctreeBlockT::clear() { + ProfilerZoneScoped; + root_scale_coefficient_ = typename Coefficients::Scale{}; + ndtree_.clear(); + setLastUpdatedStamp(); +} + +template +void HashedWaveletOctreeBlockT::setCellValue( + const OctreeIndex& index, FloatingPoint new_value) { + CellDataT voxel = getVoxelValue(index); + detail::setOccupancy(voxel, new_value); + setVoxelValue(index, voxel); +} + +template +void HashedWaveletOctreeBlockT::setVoxelValue( + const OctreeIndex& index, const CellDataT& new_value) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(&ndtree_.getRootNode()); + CellDataT current_value = root_scale_coefficient_; + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodePtrType current_parent = ancestors.back(); + current_value = Transform::backwardSingleChild( + {current_value, current_parent->data()}, child_index); + typename OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); + ancestors.emplace_back(&child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{new_value - current_value, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodePtrType current_node = ancestors.back(); + ancestors.pop_back(); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node->data() += coefficients.details; + } + + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedWaveletOctreeBlockT::addToCellValue( + const OctreeIndex& index, FloatingPoint update) { + addToVoxelValue(index, CellDataT{update}); +} + +template +void HashedWaveletOctreeBlockT::addToVoxelValue( + const OctreeIndex& index, const CellDataT& update) { + setNeedsPruning(); + setNeedsThresholding(); + setLastUpdatedStamp(); + const MortonIndex morton_code = convert::nodeIndexToMorton(index); + + std::vector ancestors; + const int height_difference = tree_height_ - index.height; + ancestors.reserve(height_difference); + ancestors.emplace_back(&ndtree_.getRootNode()); + for (int parent_height = tree_height_; index.height + 1 < parent_height; + --parent_height) { + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + typename OctreeType::NodePtrType current_parent = ancestors.back(); + typename OctreeType::NodeRefType child = + current_parent->getOrAllocateChild(child_index); + ancestors.emplace_back(&child); + } + DCHECK_EQ(ancestors.size(), height_difference); + + typename Coefficients::Parent coefficients{update, {}}; + for (int parent_height = index.height + 1; parent_height <= tree_height_; + ++parent_height) { + typename OctreeType::NodePtrType current_node = ancestors.back(); + ancestors.pop_back(); + const NdtreeIndexRelativeChild child_index = + OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); + coefficients = + Transform::forwardSingleChild(coefficients.scale, child_index); + current_node->data() += coefficients.details; + } + root_scale_coefficient_ += coefficients.scale; +} + +template +void HashedWaveletOctreeBlockT::forEachLeaf( + const BlockIndex& block_index, + MapBase::IndexedLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + forEachVoxelLeaf( + block_index, + [&visitor_fn](const OctreeIndex& node_index, + const CellDataT& voxel_value) { + visitor_fn(node_index, detail::getOccupancy(voxel_value)); + }, + termination_height); +} + +template +template +void HashedWaveletOctreeBlockT::forEachVoxelLeaf( + const BlockIndex& block_index, IndexedVoxelLeafVisitorFunction visitor_fn, + IndexElement termination_height) const { + ProfilerZoneScoped; + if (empty()) { + return; + } + + struct StackElement { + const OctreeIndex node_index; + typename OctreeType::NodeConstRefType node; + const typename Coefficients::Scale scale_coefficient{}; + }; + std::stack stack; + stack.emplace(StackElement{OctreeIndex{tree_height_, block_index}, + ndtree_.getRootNode(), root_scale_coefficient_}); + while (!stack.empty()) { + const OctreeIndex node_index = stack.top().node_index; + typename OctreeType::NodeConstRefType node = stack.top().node; + const typename Coefficients::Scale node_scale_coefficient = + stack.top().scale_coefficient; + stack.pop(); + + const typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, {node.data()}}); + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + const OctreeIndex child_node_index = + node_index.computeChildIndex(child_idx); + const typename Coefficients::Scale child_scale_coefficient = + child_scale_coefficients[child_idx]; + typename OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); + if (child_node && termination_height < child_node_index.height) { + stack.emplace(StackElement{child_node_index, *child_node, + child_scale_coefficient}); + } else { + visitor_fn(child_node_index, child_scale_coefficient); + } + } + } +} + +template +void HashedWaveletOctreeBlockT::recursiveThreshold( + typename OctreeType::NodeRefType node, + typename Coefficients::Scale& node_scale_coefficient) { + auto& node_detail_coefficients = node.data(); + typename Coefficients::CoefficientsArray child_scale_coefficients = + Transform::backward({node_scale_coefficient, node_detail_coefficients}); + + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + typename Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; + if (auto child_node = node.getChild(child_idx); child_node) { + recursiveThreshold(*child_node, child_scale); + } else { + detail::thresholdCellData(child_scale, min_log_odds_, max_log_odds_, + threshold_config_); + } + } + + const auto [new_scale, new_details] = + Transform::forward(child_scale_coefficients); + node_detail_coefficients = new_details; + node_scale_coefficient = new_scale; +} + +template +void HashedWaveletOctreeBlockT::recursivePrune( + typename OctreeType::NodeRefType node) { + bool has_at_least_one_child = false; + for (NdtreeIndexRelativeChild child_idx = 0; + child_idx < OctreeIndex::kNumChildren; ++child_idx) { + if (typename OctreeType::NodePtrType child_node = node.getChild(child_idx); + child_node) { + recursivePrune(*child_node); + if (!child_node->hasChildrenArray() && + !detail::detailsHaveNonzeroData(child_node->data(), + pruning_config_)) { + node.eraseChild(child_idx); + } else { + has_at_least_one_child = true; + } + } + } + if (!has_at_least_one_child) { + node.deleteChildrenArray(); + } +} } // namespace wavemap #endif // WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_BLOCK_INL_H_ diff --git a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h index 3866b97dd..0526b0fa2 100644 --- a/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h +++ b/library/cpp/include/wavemap/core/map/impl/hashed_wavelet_octree_inl.h @@ -2,9 +2,11 @@ #define WAVEMAP_CORE_MAP_IMPL_HASHED_WAVELET_OCTREE_INL_H_ #include "wavemap/core/indexing/index_conversions.h" +#include "wavemap/core/utils/profile/profiler_interface.h" namespace wavemap { -inline size_t HashedWaveletOctree::size() const { +template +size_t HashedWaveletOctreeT::size() const { size_t size = 0u; forEachBlock([&size](const BlockIndex& /*block_index*/, const Block& block) { size += block.size(); @@ -12,7 +14,63 @@ inline size_t HashedWaveletOctree::size() const { return size; } -inline FloatingPoint HashedWaveletOctree::getCellValue( +template +void HashedWaveletOctreeT::threshold() { + ProfilerZoneScoped; + forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { + block.threshold(); + }); +} + +template +void HashedWaveletOctreeT::prune() { + ProfilerZoneScoped; + block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { + block.prune(); + return block.empty(); + }); +} + +template +void HashedWaveletOctreeT::pruneSmart() { + ProfilerZoneScoped; + block_map_.eraseBlockIf( + [&config = config_](const BlockIndex& /*block_index*/, Block& block) { + if (config.only_prune_blocks_if_unused_for < + block.getTimeSinceLastUpdated()) { + block.prune(); + } + return block.empty(); + }); +} + +template +size_t HashedWaveletOctreeT::getMemoryUsage() const { + ProfilerZoneScoped; + // TODO(victorr): Also include the memory usage of the unordered map itself + size_t memory_usage = 0u; + forEachBlock( + [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { + memory_usage += block.getMemoryUsage(); + }); + return memory_usage; +} + +template +Index3D HashedWaveletOctreeT::getMinIndex() const { + return cells_per_block_side_ * getMinBlockIndex(); +} + +template +Index3D HashedWaveletOctreeT::getMaxIndex() const { + if (empty()) { + return Index3D::Zero(); + } + return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; +} + +template +FloatingPoint HashedWaveletOctreeT::getCellValue( const Index3D& index) const { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); @@ -24,7 +82,8 @@ inline FloatingPoint HashedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline FloatingPoint HashedWaveletOctree::getCellValue( +template +FloatingPoint HashedWaveletOctreeT::getCellValue( const OctreeIndex& index) const { const BlockIndex block_index = indexToBlockIndex(index); const Block* block = getBlock(block_index); @@ -35,8 +94,34 @@ inline FloatingPoint HashedWaveletOctree::getCellValue( return block->getCellValue(cell_index); } -inline void HashedWaveletOctree::setCellValue(const Index3D& index, - FloatingPoint new_value) { +template +CellDataT HashedWaveletOctreeT::getVoxelValue( + const Index3D& index) const { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex({0, index}); + return block->getVoxelValue(cell_index); +} + +template +CellDataT HashedWaveletOctreeT::getVoxelValue( + const OctreeIndex& index) const { + const BlockIndex block_index = indexToBlockIndex(index); + const Block* block = getBlock(block_index); + if (!block) { + return {}; + } + const CellIndex cell_index = indexToCellIndex(index); + return block->getVoxelValue(cell_index); +} + +template +void HashedWaveletOctreeT::setCellValue(const Index3D& index, + FloatingPoint new_value) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -44,8 +129,19 @@ inline void HashedWaveletOctree::setCellValue(const Index3D& index, block.setCellValue(cell_index, new_value); } -inline void HashedWaveletOctree::addToCellValue(const Index3D& index, - FloatingPoint update) { +template +void HashedWaveletOctreeT::setVoxelValue( + const Index3D& index, const CellDataT& new_value) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.setVoxelValue(cell_index, new_value); +} + +template +void HashedWaveletOctreeT::addToCellValue(const Index3D& index, + FloatingPoint update) { const BlockIndex block_index = convert::indexToBlockIndex(index, config_.tree_height); auto& block = getOrAllocateBlock(block_index); @@ -53,55 +149,100 @@ inline void HashedWaveletOctree::addToCellValue(const Index3D& index, block.addToCellValue(cell_index, update); } -inline bool HashedWaveletOctree::hasBlock(const Index3D& block_index) const { +template +void HashedWaveletOctreeT::addToVoxelValue( + const Index3D& index, const CellDataT& update) { + const BlockIndex block_index = + convert::indexToBlockIndex(index, config_.tree_height); + auto& block = getOrAllocateBlock(block_index); + const CellIndex cell_index = indexToCellIndex({0, index}); + block.addToVoxelValue(cell_index, update); +} + +template +bool HashedWaveletOctreeT::hasBlock( + const Index3D& block_index) const { return block_map_.hasBlock(block_index); } -inline bool HashedWaveletOctree::eraseBlock( - const HashedWaveletOctree::BlockIndex& block_index) { +template +bool HashedWaveletOctreeT::eraseBlock( + const typename HashedWaveletOctreeT::BlockIndex& block_index) { return block_map_.eraseBlock(block_index); } +template template -void HashedWaveletOctree::eraseBlockIf(IndexedBlockVisitor indicator_fn) { +void HashedWaveletOctreeT::eraseBlockIf( + IndexedBlockVisitor indicator_fn) { block_map_.eraseBlockIf(indicator_fn); } -inline HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( - const Index3D& block_index) { +template +typename HashedWaveletOctreeT::Block* +HashedWaveletOctreeT::getBlock(const Index3D& block_index) { return block_map_.getBlock(block_index); } -inline const HashedWaveletOctree::Block* HashedWaveletOctree::getBlock( - const Index3D& block_index) const { +template +const typename HashedWaveletOctreeT::Block* +HashedWaveletOctreeT::getBlock(const Index3D& block_index) const { return block_map_.getBlock(block_index); } -inline HashedWaveletOctree::Block& HashedWaveletOctree::getOrAllocateBlock( +template +typename HashedWaveletOctreeT::Block& +HashedWaveletOctreeT::getOrAllocateBlock( const Index3D& block_index) { - return block_map_.getOrAllocateBlock(block_index, config_.tree_height, - config_.min_log_odds, - config_.max_log_odds); + return block_map_.getOrAllocateBlock( + block_index, config_.tree_height, config_.min_log_odds, + config_.max_log_odds, threshold_config_, pruning_config_); } +template template -void HashedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) { +void HashedWaveletOctreeT::forEachBlock( + IndexedBlockVisitor visitor_fn) { block_map_.forEachBlock(visitor_fn); } +template template -void HashedWaveletOctree::forEachBlock(IndexedBlockVisitor visitor_fn) const { +void HashedWaveletOctreeT::forEachBlock( + IndexedBlockVisitor visitor_fn) const { block_map_.forEachBlock(visitor_fn); } -inline HashedWaveletOctree::BlockIndex HashedWaveletOctree::indexToBlockIndex( +template +void HashedWaveletOctreeT::forEachLeaf( + MapBase::IndexedLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachLeaf(block_index, visitor_fn); + }); +} + +template +template +void HashedWaveletOctreeT::forEachVoxelLeaf( + IndexedVoxelLeafVisitorFunction visitor_fn) const { + forEachBlock( + [&visitor_fn](const BlockIndex& block_index, const Block& block) { + block.forEachVoxelLeaf(block_index, visitor_fn); + }); +} + +template +typename HashedWaveletOctreeT::BlockIndex +HashedWaveletOctreeT::indexToBlockIndex( const OctreeIndex& node_index) const { const Index3D index = convert::nodeIndexToMinCornerIndex(node_index); return convert::indexToBlockIndex(index, config_.tree_height); } -inline HashedWaveletOctree::CellIndex HashedWaveletOctree::indexToCellIndex( - OctreeIndex index) const { +template +typename HashedWaveletOctreeT::CellIndex +HashedWaveletOctreeT::indexToCellIndex(OctreeIndex index) const { DCHECK_LE(index.height, config_.tree_height); const IndexElement height_difference = config_.tree_height - index.height; index.position = diff --git a/library/cpp/include/wavemap/core/map/map_base.h b/library/cpp/include/wavemap/core/map/map_base.h index f618e86d6..15295d60d 100644 --- a/library/cpp/include/wavemap/core/map/map_base.h +++ b/library/cpp/include/wavemap/core/map/map_base.h @@ -19,12 +19,13 @@ struct MapType : TypeSelector { kOctree, kWaveletOctree, kHashedWaveletOctree, - kHashedChunkedWaveletOctree + kHashedChunkedWaveletOctree, + kLayeredHashedWaveletOctree }; static constexpr std::array names = { "hashed_blocks", "octree", "wavelet_octree", "hashed_wavelet_octree", - "hashed_chunked_wavelet_octree"}; + "hashed_chunked_wavelet_octree", "layered_hashed_wavelet_octree"}; }; /** @@ -66,6 +67,8 @@ class MapBase { : config_(config.checkValid()) {} virtual ~MapBase() = default; + virtual MapType getMapType() const = 0; + //! Whether the map is empty virtual bool empty() const = 0; //! The number of cells or nodes in the map diff --git a/library/cpp/include/wavemap/core/map/volumetric_octree.h b/library/cpp/include/wavemap/core/map/volumetric_octree.h index b763f5a11..ce173bf7c 100644 --- a/library/cpp/include/wavemap/core/map/volumetric_octree.h +++ b/library/cpp/include/wavemap/core/map/volumetric_octree.h @@ -55,6 +55,8 @@ class VolumetricOctree : public MapBase { explicit VolumetricOctree(const VolumetricOctreeConfig& config) : MapBase(config), config_(config.checkValid()) {} + MapType getMapType() const override { return MapType::kOctree; } + bool empty() const override { return ndtree_.empty(); } size_t size() const override { return ndtree_.size(); } void threshold() override; diff --git a/library/cpp/include/wavemap/core/map/wavelet_octree.h b/library/cpp/include/wavemap/core/map/wavelet_octree.h index 95e6beffc..03c0a52ca 100644 --- a/library/cpp/include/wavemap/core/map/wavelet_octree.h +++ b/library/cpp/include/wavemap/core/map/wavelet_octree.h @@ -61,6 +61,8 @@ class WaveletOctree : public MapBase { explicit WaveletOctree(const WaveletOctreeConfig& config) : MapBase(config), config_(config.checkValid()) {} + MapType getMapType() const override { return MapType::kWaveletOctree; } + bool empty() const override; size_t size() const override { return ndtree_.size(); } void threshold() override; diff --git a/library/cpp/include/wavemap/core/utils/data/comparisons.h b/library/cpp/include/wavemap/core/utils/data/comparisons.h index 791aa8ebc..9e5c87a72 100644 --- a/library/cpp/include/wavemap/core/utils/data/comparisons.h +++ b/library/cpp/include/wavemap/core/utils/data/comparisons.h @@ -6,27 +6,32 @@ #include #include "wavemap/core/common.h" +#include "wavemap/core/map/cell_types/cell_data_traits.h" namespace wavemap::data { -// Test strict equality to zero +// Test strict equality to zero. template bool is_nonzero(const T& data) { return data != T{}; } -// Test equality to zero with a tolerance (for POD types) +// Test equality to zero with a tolerance for scalar arithmetic types. template , bool> = true> bool is_nonzero(const DataT& data, ThresholdT threshold) { return threshold < std::abs(data); } -// Test element-wise equality to zero with a tolerance (for array types) +// Test element-wise equality to zero with the element type's default pruning +// config. Code paths that need custom thresholds should call CellDataTraits +// directly with an explicit PruningConfig. template -bool is_nonzero(const DataT& data, ThresholdT threshold) { - return std::any_of(data.cbegin(), data.cend(), [threshold](auto value) { - return threshold < std::abs(value); +bool is_nonzero(const DataT& data, ThresholdT /*threshold*/) { + using ElementT = std::decay_t; + const typename CellDataTraits::PruningConfig pruning_config{}; + return std::any_of(data.cbegin(), data.cend(), [&pruning_config](const auto& value) { + return CellDataTraits::isNonzero(value, pruning_config); }); } diff --git a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h index aff568c0c..e19c8abaf 100644 --- a/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h +++ b/library/cpp/include/wavemap/core/utils/undistortion/stamped_pointcloud.h @@ -13,10 +13,13 @@ namespace wavemap::undistortion { struct StampedPoint { Point3D position; TimeOffset time_offset; + size_t source_index; StampedPoint(FloatingPoint x, FloatingPoint y, FloatingPoint z, - TimeOffset time_offset) - : position(x, y, z), time_offset(time_offset) {} + TimeOffset time_offset, size_t source_index = 0u) + : position(x, y, z), + time_offset(time_offset), + source_index(source_index) {} std::string toStr() const; }; diff --git a/library/cpp/include/wavemap/io/file_conversions.h b/library/cpp/include/wavemap/io/file_conversions.h index 6e0cb70f2..9bfb8521c 100644 --- a/library/cpp/include/wavemap/io/file_conversions.h +++ b/library/cpp/include/wavemap/io/file_conversions.h @@ -9,6 +9,13 @@ namespace wavemap::io { bool mapToFile(const MapBase& map, const std::filesystem::path& file_path); bool fileToMap(const std::filesystem::path& file_path, MapBase::Ptr& map); +template +bool mapToFile(const HashedWaveletOctreeT& map, const std::filesystem::path& file_path); + +template +bool fileToMap(const std::filesystem::path& file_path, typename HashedWaveletOctreeT::Ptr& map); } // namespace wavemap::io +#include "wavemap/io/impl/file_conversions_inl.h" + #endif // WAVEMAP_IO_FILE_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/impl/file_conversions_inl.h b/library/cpp/include/wavemap/io/impl/file_conversions_inl.h new file mode 100644 index 000000000..b9ec5c72a --- /dev/null +++ b/library/cpp/include/wavemap/io/impl/file_conversions_inl.h @@ -0,0 +1,55 @@ +#ifndef WAVEMAP_IO_IMPL_FILE_CONVERSIONS_INL_H_ +#define WAVEMAP_IO_IMPL_FILE_CONVERSIONS_INL_H_ + +#include +#include +#include + +namespace wavemap::io { +template +bool mapToFile(const HashedWaveletOctreeT& map, const std::filesystem::path& file_path) { + if (file_path.empty()) { + LOG(WARNING) << "Could open file for writing. Specified file path is empty."; + return false; + } + + std::ofstream file_ostream(file_path, + std::ofstream::out | std::ofstream::binary); + if (!file_ostream.is_open()) { + LOG(WARNING) << "Could not open file " << file_path + << " for writing. Error: " << strerror(errno); + return false; + } + + if (!mapToStream(map, file_ostream)) { + return false; + } + + file_ostream.close(); + return static_cast(file_ostream); +} + +template +bool fileToMap(const std::filesystem::path& file_path, typename HashedWaveletOctreeT::Ptr& map) { + if (file_path.empty()) { + LOG(WARNING) << "Could not open file for reading. Specified file path is empty."; + return false; + } + + std::ifstream file_istream(file_path, + std::ifstream::in | std::ifstream::binary); + if (!file_istream.is_open()) { + LOG(WARNING) << "Could not open file " << file_path << " for reading. Error: " << strerror(errno); + return false; + } + + if (!streamToMap(file_istream, map)) { + LOG(WARNING) << "Failed to parse map from file " << file_path << "."; + return false; + } + + return true; +} +} // namespace wavemap::io + +#endif // WAVEMAP_IO_IMPL_FILE_CONVERSIONS_INL_H_ diff --git a/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h b/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h new file mode 100644 index 000000000..b72ba9b63 --- /dev/null +++ b/library/cpp/include/wavemap/io/impl/stream_conversions_inl.h @@ -0,0 +1,253 @@ +#ifndef WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ +#define WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ + +#include + +#include "wavemap/core/utils/bits/bit_operations.h" + +namespace wavemap::io { +template +bool mapToStream(const HashedWaveletOctreeT& map, + std::ostream& ostream) { + if (!ostream.good()) { + return false; + } + + using Block = typename HashedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + streamable::StorageFormat storage_format = + streamable::StorageFormat::kLayeredHashedWaveletOctree; + storage_format.write(ostream); + + streamable::HashedWaveletOctreeHeader header; + header.min_cell_width = map.getMinCellWidth(); + header.min_log_odds = map.getMinLogOdds(); + header.max_log_odds = map.getMaxLogOdds(); + header.tree_height = map.getTreeHeight(); + header.num_blocks = map.getHashMap().size(); + header.write(ostream); + + map.forEachBlock([&ostream, min_log_odds, max_log_odds]( + const Index3D& block_index, const auto& block) { + if (!ostream.good()) { + return; + } + + streamable::Index3D streamable_block_index{block_index.x(), + block_index.y(), + block_index.z()}; + streamable_block_index.write(ostream); + CellDataSerializerT::write(ostream, block.getRootScale()); + + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto& node = stack.top().node; + stack.pop(); + + streamable::UInt8 allocated_children_bitset{}; + for (const CellDataT& coefficient : node.data()) { + CellDataSerializerT::write(ostream, coefficient); + } + + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + + const auto* child = node.getChild(relative_child_idx); + if (child) { + stack.emplace(StackElement{child_scale, *child}); + allocated_children_bitset += (1 << relative_child_idx); + } + } + + ostream.write(reinterpret_cast(&allocated_children_bitset), + sizeof(allocated_children_bitset)); + } + }); + + return ostream.good(); +} + +template +bool streamToMap(std::istream& istream, + typename HashedWaveletOctreeT::Ptr& map) { + if (!istream.good()) { + return false; + } + + if (streamable::StorageFormat::read(istream) != + streamable::StorageFormat::kLayeredHashedWaveletOctree) { + return false; + } + + const auto header = streamable::HashedWaveletOctreeHeader::read(istream); + HashedWaveletOctreeConfig config; + config.min_cell_width = header.min_cell_width; + config.min_log_odds = header.min_log_odds; + config.max_log_odds = header.max_log_odds; + config.tree_height = header.tree_height; + map = std::make_shared>(config); + + using Block = typename HashedWaveletOctreeT::Block; + using NodeType = typename Block::OctreeType::NodeType; + + for (size_t block_idx = 0; block_idx < header.num_blocks; ++block_idx) { + if (!istream.good()) { + return false; + } + + const auto streamable_block_index = streamable::Index3D::read(istream); + const Index3D block_index{streamable_block_index.x, + streamable_block_index.y, + streamable_block_index.z}; + auto& block = map->getOrAllocateBlock(block_index); + block.getRootScale() = CellDataSerializerT::read(istream); + + std::stack stack; + stack.emplace(&block.getRootNode()); + while (!stack.empty()) { + NodeType* node = stack.top(); + stack.pop(); + + for (CellDataT& coefficient : node->data()) { + coefficient = CellDataSerializerT::read(istream); + } + + streamable::UInt8 allocated_children_bitset{}; + istream.read(reinterpret_cast(&allocated_children_bitset), + sizeof(allocated_children_bitset)); + + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const bool child_exists = bit_ops::is_bit_set( + allocated_children_bitset, relative_child_idx); + if (child_exists) { + stack.emplace(&node->getOrAllocateChild(relative_child_idx)); + } + } + } + } + + return istream.good(); +} + +template +bool mapToStream(const HashedChunkedWaveletOctreeT& map, + std::ostream& ostream) { + if (!ostream.good()) { + return false; + } + + using Block = typename HashedChunkedWaveletOctreeT::Block; + using NodeConstRefType = typename Block::OctreeType::NodeConstRefType; + struct StackElement { + const CellDataT scale; + NodeConstRefType node; + }; + + constexpr FloatingPoint kNumericalNoise = 1e-3f; + const auto min_log_odds = map.getMinLogOdds() + kNumericalNoise; + const auto max_log_odds = map.getMaxLogOdds() - kNumericalNoise; + + streamable::StorageFormat storage_format = + streamable::StorageFormat::kLayeredHashedWaveletOctree; + storage_format.write(ostream); + + streamable::HashedWaveletOctreeHeader header; + header.min_cell_width = map.getMinCellWidth(); + header.min_log_odds = map.getMinLogOdds(); + header.max_log_odds = map.getMaxLogOdds(); + header.tree_height = map.getTreeHeight(); + header.num_blocks = map.getHashMap().size(); + header.write(ostream); + + map.forEachBlock([&ostream, min_log_odds, max_log_odds]( + const Index3D& block_index, const auto& block) { + if (!ostream.good()) { + return; + } + + streamable::Index3D streamable_block_index{ + block_index.x(), block_index.y(), block_index.z()}; + streamable_block_index.write(ostream); + CellDataSerializerT::write(ostream, block.getRootScale()); + + std::stack stack; + stack.emplace(StackElement{block.getRootScale(), block.getRootNode()}); + while (!stack.empty()) { + const CellDataT scale = stack.top().scale; + const auto node = stack.top().node; + stack.pop(); + + streamable::UInt8 allocated_children_bitset{}; + for (const CellDataT& coefficient : node.data()) { + CellDataSerializerT::write(ostream, coefficient); + } + + const auto child_scales = Block::Transform::backward({scale, node.data()}); + for (int relative_child_idx = OctreeIndex::kNumChildren - 1; + 0 <= relative_child_idx; --relative_child_idx) { + const auto child_scale = child_scales[relative_child_idx]; + const FloatingPoint child_occupancy = + static_cast(child_scale); + if (child_occupancy < min_log_odds || max_log_odds < child_occupancy) { + continue; + } + if (auto child = node.getChild(relative_child_idx); child) { + stack.emplace(StackElement{child_scale, *child}); + allocated_children_bitset += (1 << relative_child_idx); + } + } + + ostream.write(reinterpret_cast(&allocated_children_bitset), + sizeof(allocated_children_bitset)); + } + }); + return ostream.good(); +} + +template +bool streamToMap( + std::istream& istream, + typename HashedChunkedWaveletOctreeT::Ptr& map) { + typename HashedWaveletOctreeT::Ptr logical_map; + if (!streamToMap(istream, logical_map) || + !logical_map) { + return false; + } + + HashedChunkedWaveletOctreeConfig config; + config.min_cell_width = logical_map->getMinCellWidth(); + config.min_log_odds = logical_map->getMinLogOdds(); + config.max_log_odds = logical_map->getMaxLogOdds(); + config.tree_height = logical_map->getTreeHeight(); + map = std::make_shared>(config); + logical_map->forEachVoxelLeaf( + [&map](const OctreeIndex& node_index, const CellDataT& value) { + const auto block_index = map->indexToBlockIndex(node_index); + auto& block = map->getOrAllocateBlock(block_index); + block.setVoxelValue(map->indexToCellIndex(node_index), value); + }); + return true; +} +} // namespace wavemap::io + +#endif // WAVEMAP_IO_IMPL_STREAM_CONVERSIONS_INL_H_ diff --git a/library/cpp/include/wavemap/io/stream_conversions.h b/library/cpp/include/wavemap/io/stream_conversions.h index f3341999b..cbafd4b29 100644 --- a/library/cpp/include/wavemap/io/stream_conversions.h +++ b/library/cpp/include/wavemap/io/stream_conversions.h @@ -3,6 +3,7 @@ #include #include +#include #include "wavemap/core/common.h" #include "wavemap/core/map/cell_types/haar_coefficients.h" @@ -16,6 +17,44 @@ namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream); bool streamToMap(std::istream& istream, MapBase::Ptr& map); +struct StreamableFloatingPoint { + static void write(std::ostream& ostream, FloatingPoint value) { + streamable::Float serialized_value = value; + ostream.write(reinterpret_cast(&serialized_value), + sizeof(serialized_value)); + } + + static FloatingPoint read(std::istream& istream) { + streamable::Float serialized_value{}; + istream.read(reinterpret_cast(&serialized_value), + sizeof(serialized_value)); + return serialized_value; + } +}; + +template +struct StreamableCellData; + +template <> +struct StreamableCellData : StreamableFloatingPoint {}; + +template +bool mapToStream(const HashedWaveletOctreeT& map, + std::ostream& ostream); + +template +bool streamToMap(std::istream& istream, + typename HashedWaveletOctreeT::Ptr& map); + +template +bool mapToStream(const HashedChunkedWaveletOctreeT& map, + std::ostream& ostream); + +template +bool streamToMap( + std::istream& istream, + typename HashedChunkedWaveletOctreeT::Ptr& map); + bool mapToStream(const HashedBlocks& map, std::ostream& ostream); bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map); @@ -28,4 +67,6 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map); bool mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream); } // namespace wavemap::io +#include "wavemap/io/impl/stream_conversions_inl.h" + #endif // WAVEMAP_IO_STREAM_CONVERSIONS_H_ diff --git a/library/cpp/include/wavemap/io/streamable_types.h b/library/cpp/include/wavemap/io/streamable_types.h index 3a82d09c5..7112f159d 100644 --- a/library/cpp/include/wavemap/io/streamable_types.h +++ b/library/cpp/include/wavemap/io/streamable_types.h @@ -88,10 +88,16 @@ struct HashedWaveletOctreeHeader { struct StorageFormat : TypeSelector { using TypeSelector::TypeSelector; - enum Id : TypeId { kWaveletOctree, kHashedWaveletOctree, kHashedBlocks }; + enum Id : TypeId { + kWaveletOctree, + kHashedWaveletOctree, + kHashedBlocks, + kLayeredHashedWaveletOctree + }; static constexpr std::array names = { - "wavelet_octree", "hashed_wavelet_octree", "hashed_blocks"}; + "wavelet_octree", "hashed_wavelet_octree", "hashed_blocks", + "layered_hashed_wavelet_octree"}; inline void write(std::ostream& ostream) const; inline static StorageFormat read(std::istream& istream); diff --git a/library/cpp/include/wavemap/layered/classification/local_elevation_classifier.h b/library/cpp/include/wavemap/layered/classification/local_elevation_classifier.h new file mode 100644 index 000000000..1b65f7d43 --- /dev/null +++ b/library/cpp/include/wavemap/layered/classification/local_elevation_classifier.h @@ -0,0 +1,141 @@ +#ifndef WAVEMAP_LAYERED_LOCAL_ELEVATION_CLASSIFIER_H_ +#define WAVEMAP_LAYERED_LOCAL_ELEVATION_CLASSIFIER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace wavemap::layered { + +enum class GeometricClass : int { + kUnknown = 0, + kGround = 1, + kObstacle = 2, +}; + +struct LocalElevationClassifierConfig { + FloatingPoint grid_cell_width = 0.5f; + FloatingPoint ground_tolerance = 0.2f; + FloatingPoint obstacle_height = 0.35f; + size_t min_points_per_cell = 3u; + + bool isValid() const { + return 0.f < grid_cell_width && 0.f <= ground_tolerance && + ground_tolerance < obstacle_height && 0u < min_points_per_cell; + } +}; + +// Lightweight frame-local classifier. It estimates the ground elevation as +// the lowest endpoint in each horizontal grid cell. Cells without enough +// support and points in the transition band remain unknown. +class LocalElevationClassifier { + public: + explicit LocalElevationClassifier( + LocalElevationClassifierConfig config = {}) + : config_(config) { + CHECK(config_.isValid()); + } + + std::vector classify( + const PosedPointcloud<>& pointcloud) const { + struct CellStats { + FloatingPoint min_height = std::numeric_limits::max(); + size_t point_count = 0u; + }; + + const FloatingPoint cell_width_inv = 1.f / config_.grid_cell_width; + std::map cells; + std::vector world_points(pointcloud.size()); + std::vector valid(pointcloud.size(), false); + + for (size_t index = 0u; index < pointcloud.size(); ++index) { + const Point3D sensor_point = pointcloud[index]; + if (!sensor_point.array().isFinite().all()) { + continue; + } + const Point3D world_point = pointcloud.getPose() * sensor_point; + if (!world_point.array().isFinite().all()) { + continue; + } + world_points[index] = world_point; + valid[index] = true; + CellStats& stats = cells[toCellKey(world_point, cell_width_inv)]; + stats.min_height = std::min(stats.min_height, world_point.z()); + ++stats.point_count; + } + + std::vector classes(pointcloud.size(), + GeometricClass::kUnknown); + for (size_t index = 0u; index < world_points.size(); ++index) { + if (!valid[index]) { + continue; + } + const Point3D& point = world_points[index]; + const CellStats& stats = cells.at(toCellKey(point, cell_width_inv)); + if (stats.point_count < config_.min_points_per_cell) { + continue; + } + const FloatingPoint height_above_ground = point.z() - stats.min_height; + if (height_above_ground <= config_.ground_tolerance) { + classes[index] = GeometricClass::kGround; + } else if (config_.obstacle_height <= height_above_ground) { + classes[index] = GeometricClass::kObstacle; + } + } + return classes; + } + + const LocalElevationClassifierConfig& config() const { return config_; } + + private: + struct GridCellKey { + int x; + int y; + + bool operator<(const GridCellKey& other) const { + return std::tie(x, y) < std::tie(other.x, other.y); + } + }; + + static GridCellKey toCellKey(const Point3D& point, + FloatingPoint cell_width_inv) { + return GridCellKey{ + static_cast(std::floor(point.x() * cell_width_inv)), + static_cast(std::floor(point.y() * cell_width_inv))}; + } + + LocalElevationClassifierConfig config_; +}; + +template +std::vector> makeGeometricClassObservations( + const PosedPointcloud<>& pointcloud, + const LocalElevationClassifier& classifier, + const EndpointRange& endpoint_range = {}) { + static_assert(std::is_integral_v); + const auto classes = classifier.classify(pointcloud); + std::vector> observations; + observations.reserve(classes.size()); + for (size_t index = 0u; index < classes.size(); ++index) { + if (classes[index] == GeometricClass::kUnknown || + !endpoint_range.contains(pointcloud[index])) { + continue; + } + observations.emplace_back(pointcloud.getPose() * pointcloud[index], + static_cast(classes[index])); + } + return observations; +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LOCAL_ELEVATION_CLASSIFIER_H_ diff --git a/library/cpp/include/wavemap/layered/integration/continuous_layer_updater.h b/library/cpp/include/wavemap/layered/integration/continuous_layer_updater.h new file mode 100644 index 000000000..fc50a0525 --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/continuous_layer_updater.h @@ -0,0 +1,38 @@ +#ifndef WAVEMAP_LAYERED_CONTINUOUS_LAYER_UPDATER_H_ +#define WAVEMAP_LAYERED_CONTINUOUS_LAYER_UPDATER_H_ + +#include + +#include + +namespace wavemap::layered { + +// Mutates only the additional continuous data and applies the resulting delta +// with zero occupancy. This preserves occupancy and every field the mutator +// leaves unchanged while still updating the wavelet representation correctly. +template +void updateContinuousLayersAtVoxel( + ContinuousMapT& map, const wavemap::Index3D& index, MutatorT&& mutator) { + using Voxel = typename ContinuousMapT::CellDataType; + using ContinuousPolicy = typename Voxel::AdditionalDataPolicy; + + const Voxel current_voxel = map.getVoxelValue(index); + auto updated_data = current_voxel.data; + std::forward(mutator)(updated_data); + + const auto data_update = + ContinuousPolicy::subtract(updated_data, current_voxel.data); + map.addToVoxelValue(index, Voxel(0.f, data_update)); +} + +template +void setContinuousLayersAtVoxel( + ContinuousMapT& map, const wavemap::Index3D& index, + const typename ContinuousMapT::CellDataType::AdditionalData& data) { + updateContinuousLayersAtVoxel( + map, index, [&data](auto& current_data) { current_data = data; }); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_CONTINUOUS_LAYER_UPDATER_H_ diff --git a/library/cpp/include/wavemap/layered/integration/endpoint_attribute_integrator.h b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_integrator.h new file mode 100644 index 000000000..5e1dbcf6b --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_integrator.h @@ -0,0 +1,92 @@ +#ifndef WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_INTEGRATOR_H_ +#define WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_INTEGRATOR_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace wavemap::layered { + +struct EndpointAttributeIntegrationResult { + size_t received = 0u; + size_t integrated = 0u; + size_t rejected = 0u; +}; + +// Adds endpoint-only continuous fields around an existing occupancy integrator. +// Occupancy remains entirely owned by the wrapped Wavemap integrator. +template +class EndpointAttributeIntegrator { + public: + using Measurement = + PosedEndpointAttributePointcloud; + + EndpointAttributeIntegrator(LayeredMapT& map, + IntegratorBase& occupancy_integrator) + : map_(map), occupancy_integrator_(occupancy_integrator) { + static_assert((kIsWaveletCompatibleLayer && ...), + "Endpoint sensor fields must be continuous layers."); + } + + EndpointAttributeIntegrationResult integrate(const Measurement& measurement) { + EndpointAttributeIntegrationResult result; + result.received = measurement.size(); + if (!measurement.hasConsistentSizes()) { + result.rejected = result.received; + return result; + } + + // Preserve the original occupancy path and its measurement model exactly. + const PosedPointcloud<> occupancy_measurement(measurement.getPose(), + measurement.points().data()); + occupancy_integrator_.integrate(occupancy_measurement); + + const FloatingPoint cell_width_inv = + 1.f / map_.continuousMap().getMinCellWidth(); + for (size_t point_idx = 0u; point_idx < measurement.size(); ++point_idx) { + const Point3D C_endpoint = measurement.point(point_idx); + if (!C_endpoint.array().isFinite().all()) { + ++result.rejected; + continue; + } + const Point3D W_endpoint = measurement.getPose() * C_endpoint; + const Index3D endpoint_index = + convert::pointToNearestIndex(W_endpoint, cell_width_inv); + updateContinuousLayersAtVoxel( + map_.continuousMap(), endpoint_index, [&](auto& layers) { + (updateLayer(layers, measurement, point_idx, + W_endpoint), + ...); + }); + ++result.integrated; + } + return result; + } + + private: + template + static void updateLayer(ContinuousLayersT& layers, + const Measurement& measurement, size_t point_idx, + const Point3D& W_endpoint) { + constexpr auto descriptor = + detail::descriptorForLayer(); + auto& current_value = descriptor.value(layers); + const LayerObservation> observation( + W_endpoint, measurement.template attribute(point_idx)); + current_value = LayerUpdatePolicyT{}(current_value, observation); + } + + LayeredMapT& map_; + IntegratorBase& occupancy_integrator_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_INTEGRATOR_H_ diff --git a/library/cpp/include/wavemap/layered/integration/endpoint_attribute_pointcloud.h b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_pointcloud.h new file mode 100644 index 000000000..b65a7e00f --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/endpoint_attribute_pointcloud.h @@ -0,0 +1,94 @@ +#ifndef WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_POINTCLOUD_H_ +#define WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_POINTCLOUD_H_ + +#include +#include +#include + +#include +#include + +namespace wavemap::layered { +namespace detail { +template +struct EndpointAttributeSlot { + std::vector> values; +}; +} // namespace detail + +// Structure-of-arrays point cloud whose endpoint attributes remain aligned by +// point index. Positions use the existing Pointcloud format and every +// configured attribute uses one contiguous typed array. +template +class EndpointAttributePointcloud + : private detail::EndpointAttributeSlot... { + public: + EndpointAttributePointcloud() = default; + explicit EndpointAttributePointcloud(size_t size) { resize(size); } + + bool empty() const { return points_.empty(); } + size_t size() const { return points_.size(); } + + void resize(size_t size) { + points_.resize(size); + (attributeValues().resize(size), ...); + } + + void clear() { + points_.clear(); + (attributeValues().clear(), ...); + } + + void truncate(size_t size) { + CHECK_LE(size, this->size()); + points_.conservativeResize(size); + (attributeValues().resize(size), ...); + } + + Pointcloud<>& points() { return points_; } + const Pointcloud<>& points() const { return points_; } + + auto point(size_t index) { return points_[index]; } + auto point(size_t index) const { return points_[index]; } + + template + std::vector>& attributeValues() { + static_assert((std::is_same_v || ...), + "Endpoint attribute is not present in this point cloud."); + return static_cast&>(*this) + .values; + } + + template + const std::vector>& attributeValues() const { + static_assert((std::is_same_v || ...), + "Endpoint attribute is not present in this point cloud."); + return static_cast&>(*this) + .values; + } + + template + LayerValueT& attribute(size_t index) { + return attributeValues()[index]; + } + + template + const LayerValueT& attribute(size_t index) const { + return attributeValues()[index]; + } + + bool hasConsistentSizes() const { + return ((attributeValues().size() == size()) && ...); + } + + private: + Pointcloud<> points_; +}; + +template +using PosedEndpointAttributePointcloud = + PosedObject>; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_ENDPOINT_ATTRIBUTE_POINTCLOUD_H_ diff --git a/library/cpp/include/wavemap/layered/integration/endpoint_range.h b/library/cpp/include/wavemap/layered/integration/endpoint_range.h new file mode 100644 index 000000000..0ac98832c --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/endpoint_range.h @@ -0,0 +1,30 @@ +#ifndef WAVEMAP_LAYERED_ENDPOINT_RANGE_H_ +#define WAVEMAP_LAYERED_ENDPOINT_RANGE_H_ + +#include +#include + +#include + +namespace wavemap::layered { + +struct EndpointRange { + FloatingPoint min = 0.f; + FloatingPoint max = std::numeric_limits::infinity(); + + bool isValid() const { + return 0.f <= min && min <= max && !std::isnan(max); + } + + bool contains(const Point3D& sensor_point) const { + if (!sensor_point.array().isFinite().all()) { + return false; + } + const FloatingPoint squared_range = sensor_point.squaredNorm(); + return min * min <= squared_range && squared_range <= max * max; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_ENDPOINT_RANGE_H_ diff --git a/library/cpp/include/wavemap/layered/integration/layer_integrator.h b/library/cpp/include/wavemap/layered/integration/layer_integrator.h new file mode 100644 index 000000000..6bf5e5f26 --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/layer_integrator.h @@ -0,0 +1,146 @@ +#ifndef WAVEMAP_LAYERED_LAYER_INTEGRATOR_H_ +#define WAVEMAP_LAYERED_LAYER_INTEGRATOR_H_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace wavemap::layered { + +struct LayerIntegrationResult { + size_t received = 0u; + size_t integrated = 0u; + size_t rejected = 0u; + size_t updated_voxels = 0u; +}; + +namespace detail { + +inline bool isFinite(const Point3D& position) { + return position.array().isFinite().all(); +} + +template +inline constexpr bool kDescriptorMatchesLayer = + std::is_same_v; + +template +constexpr size_t descriptorMatchCount(std::index_sequence) { + return (size_t{0u} + ... + + (kDescriptorMatchesLayer< + LayerTagT, std::tuple_element_t> + ? size_t{1u} + : size_t{0u})); +} + +template +constexpr auto descriptorForLayer(const TupleT& descriptors) { + static_assert(Index < std::tuple_size_v, + "The requested layer is not present in this bundle."); + using Descriptor = std::tuple_element_t; + if constexpr (kDescriptorMatchesLayer) { + return std::get(descriptors); + } else { + return descriptorForLayer(descriptors); + } +} + +template +constexpr auto descriptorForLayer() { + constexpr auto descriptors = layerDescriptors(); + using DescriptorTuple = decltype(descriptors); + constexpr size_t kDescriptorCount = std::tuple_size_v; + static_assert( + descriptorMatchCount( + std::make_index_sequence{}) == 1u, + "Each layer tag must occur exactly once in its storage bundle."); + return descriptorForLayer(descriptors); +} + +template +ValueT applyDiscretePolicy(const PolicyT& policy, + const std::optional& current_value, + const LayerObservation& observation) { + if constexpr (std::is_invocable_r_v< + ValueT, PolicyT, const std::optional&, + const LayerObservation&>) { + return policy(current_value, observation); + } else { + return policy(current_value.value_or(ValueT{}), observation); + } +} + +} // namespace detail + +template +LayerIntegrationResult integrateLayerObservations( + LayeredMapT& map, + const std::vector>>& observations, + const LayerUpdatePolicyT& policy = + LayerUpdatePolicyT{}) { + using Value = LayerValueT; + LayerIntegrationResult result; + result.received = observations.size(); + + const FloatingPoint cell_width_inv = + 1.f / map.continuousMap().getMinCellWidth(); + std::map*>> grouped; + for (const auto& observation : observations) { + if (!detail::isFinite(observation.position)) { + ++result.rejected; + continue; + } + const Index3D index = + convert::pointToNearestIndex(observation.position, cell_width_inv); + grouped[IndexKey(index)].push_back(&observation); + ++result.integrated; + } + result.updated_voxels = grouped.size(); + + if constexpr (kIsWaveletCompatibleLayer) { + constexpr auto descriptor = + detail::descriptorForLayer< + LayerTagT, typename LayeredMapT::ContinuousLayers>(); + for (const auto& [index_key, voxel_observations] : grouped) { + updateContinuousLayersAtVoxel( + map.continuousMap(), indexFromKey(index_key), [&](auto& layers) { + auto& state = descriptor.value(layers); + for (const auto* observation : voxel_observations) { + state = policy(state, *observation); + } + }); + } + } else { + constexpr auto descriptor = + detail::descriptorForLayer< + LayerTagT, typename LayeredMapT::DiscreteLayers>(); + auto& layer = descriptor.value(map.discreteLayers()); + std::vector> updates; + updates.reserve(grouped.size()); + for (const auto& [index_key, voxel_observations] : grouped) { + const Index3D index = indexFromKey(index_key); + std::optional value = layer.getValue(index); + for (const auto* observation : voxel_observations) { + value = detail::applyDiscretePolicy(policy, value, *observation); + } + updates.emplace_back(index, *value); + } + layer.setValues(updates); + } + + return result; +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_INTEGRATOR_H_ diff --git a/library/cpp/include/wavemap/layered/integration/layer_observation.h b/library/cpp/include/wavemap/layered/integration/layer_observation.h new file mode 100644 index 000000000..0b57bd922 --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/layer_observation.h @@ -0,0 +1,34 @@ +#ifndef WAVEMAP_LAYERED_LAYER_OBSERVATION_H_ +#define WAVEMAP_LAYERED_LAYER_OBSERVATION_H_ + +#include +#include +#include + +#include +#include + +namespace wavemap::layered { + +// A sensor-independent observation of one layer at a spatial position. +// Observation generation stays outside the map. The update policy decides how +// this value is fused. +template +struct LayerObservation { + using Value = ValueT; + + Point3D position = Point3D::Zero(); + ValueT value{}; + std::optional confidence; + std::optional timestamp; + std::string source; + + LayerObservation() = default; + LayerObservation(Point3D observed_position, ValueT observed_value) + : position(std::move(observed_position)), + value(std::move(observed_value)) {} +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_OBSERVATION_H_ diff --git a/library/cpp/include/wavemap/layered/integration/layer_observation_batch.h b/library/cpp/include/wavemap/layered/integration/layer_observation_batch.h new file mode 100644 index 000000000..df598a0d4 --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/layer_observation_batch.h @@ -0,0 +1,97 @@ +#ifndef WAVEMAP_LAYERED_LAYER_OBSERVATION_BATCH_H_ +#define WAVEMAP_LAYERED_LAYER_OBSERVATION_BATCH_H_ + +#include +#include +#include +#include + +#include +#include + +namespace wavemap::layered { +namespace detail { + +template +struct LayerObservationSlot { + std::vector>> observations; +}; + +template +struct LayerTagIdentity { + using Type = LayerTagT; +}; + +} // namespace detail + +// A heterogeneous, sensor-independent batch generated directly from a layer +// schema. One sensor callback can populate any subset of the configured layers +// without defining a new adapter or storage type. +template +class LayerObservationBatch; + +template +class LayerObservationBatch> + : private detail::LayerObservationSlot... { + public: + using Schema = schema::LayerSchema; + + template + void add(LayerObservation> observation) { + static_assert(Schema::template contains, + "The observation layer is not present in this map schema."); + observations().emplace_back(std::move(observation)); + } + + template + void add(Point3D position, LayerValueT value) { + add({std::move(position), std::move(value)}); + } + + template + auto& observations() { + static_assert(Schema::template contains, + "The requested layer is not present in this map schema."); + return static_cast&>(*this) + .observations; + } + + template + const auto& observations() const { + static_assert(Schema::template contains, + "The requested layer is not present in this map schema."); + return static_cast&>(*this) + .observations; + } + + bool empty() const { return (observations().empty() && ...); } + size_t size() const { + return (size_t{0u} + ... + observations().size()); + } + + template + void forEachLayer(FunctionT&& function) { + (function(detail::LayerTagIdentity{}, + observations()), + ...); + } + + template + void forEachLayer(FunctionT&& function) const { + (function(detail::LayerTagIdentity{}, + observations()), + ...); + } + + void transformPositions(const Transformation3D& transform) { + forEachLayer([&transform](auto /*layer_tag*/, auto& layer_observations) { + for (auto& observation : layer_observations) { + observation.position = transform * observation.position; + } + }); + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_OBSERVATION_BATCH_H_ diff --git a/library/cpp/include/wavemap/layered/integration/layer_update_policy.h b/library/cpp/include/wavemap/layered/integration/layer_update_policy.h new file mode 100644 index 000000000..68e334cf8 --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/layer_update_policy.h @@ -0,0 +1,170 @@ +#ifndef WAVEMAP_LAYERED_LAYER_UPDATE_POLICY_H_ +#define WAVEMAP_LAYERED_LAYER_UPDATE_POLICY_H_ + +#include +#include +#include +#include + +#include + +#include +#include + +namespace wavemap::layered { + +template +inline constexpr bool kIsContinuousLayerUpdatePolicy = + std::is_invocable_r_v&>; + +template +inline constexpr bool kIsStatefulContinuousLayerUpdatePolicy = + std::is_invocable_r_v&>; + +template +inline constexpr bool kIsDiscreteLayerUpdatePolicy = + kIsContinuousLayerUpdatePolicy || + std::is_invocable_r_v&, + const LayerObservation&>; + +template +struct ReplaceLayerUpdatePolicy { + ValueT operator()(const ValueT& /*current_value*/, + const LayerObservation& observation) const { + return observation.value; + } +}; + +template +struct MinimumLayerUpdatePolicy { + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return std::min(current_value, observation.value); + } +}; + +template +struct MaximumLayerUpdatePolicy { + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return std::max(current_value, observation.value); + } +}; + +template > +struct AccumulateLayerUpdatePolicy { + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return AddT{}(current_value, observation.value); + } +}; + +struct LogicalOrLayerUpdatePolicy { + bool operator()(bool current_value, + const LayerObservation& observation) const { + return current_value || observation.value; + } +}; + +struct LogicalAndLayerUpdatePolicy { + bool operator()(bool current_value, + const LayerObservation& observation) const { + return current_value && observation.value; + } +}; + +// Schema-friendly exponential average for scalar floating-point layers. +// The compile-time ratio keeps the policy default-constructible, as required +// by the layer integrator, while allowing each layer to select its own gain. +// This is a convex update, so bounded current values and observations remain +// bounded. The zero-initialized voxel acts as the initial prior. +template +struct ExponentialScalarLayerUpdatePolicy { + static_assert(0 < ObservationWeightDenominator, + "The exponential update denominator must be positive."); + static_assert(0 <= ObservationWeightNumerator, + "The exponential observation weight cannot be negative."); + static_assert(ObservationWeightNumerator <= ObservationWeightDenominator, + "The exponential observation weight cannot exceed one."); + + static constexpr FloatingPoint observationWeight() { + return static_cast(ObservationWeightNumerator) / + static_cast(ObservationWeightDenominator); + } + + FloatingPoint operator()( + FloatingPoint current_value, + const LayerObservation& observation) const { + const FloatingPoint weight = observationWeight(); + return current_value + weight * (observation.value - current_value); + } +}; + +// Runtime form for direct/custom use when a policy instance is explicitly +// owned by the caller rather than default-constructed from a layer schema. +template +class ExponentialLayerUpdatePolicy { + public: + explicit ExponentialLayerUpdatePolicy(FloatingPoint observation_weight) + : observation_weight_( + std::clamp(observation_weight, FloatingPoint{0.f}, + FloatingPoint{1.f})) {} + + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + return ArithmeticPolicyT::add( + ArithmeticPolicyT::scale(current_value, 1.f - observation_weight_), + ArithmeticPolicyT::scale(observation.value, observation_weight_)); + } + + private: + FloatingPoint observation_weight_; +}; + +// Uses observation confidence as the blend factor when present. This is an +// exponential update, not a running weighted mean: no historical weight is +// hidden outside the voxel value. +template +class ConfidenceBlendLayerUpdatePolicy { + public: + explicit ConfidenceBlendLayerUpdatePolicy( + FloatingPoint default_observation_weight = 1.f) + : default_observation_weight_(std::clamp( + default_observation_weight, FloatingPoint{0.f}, + FloatingPoint{1.f})) {} + + ValueT operator()(const ValueT& current_value, + const LayerObservation& observation) const { + const FloatingPoint weight = std::clamp( + observation.confidence.value_or(default_observation_weight_), + FloatingPoint{0.f}, FloatingPoint{1.f}); + return ArithmeticPolicyT::add( + ArithmeticPolicyT::scale(current_value, 1.f - weight), + ArithmeticPolicyT::scale(observation.value, weight)); + } + + private: + FloatingPoint default_observation_weight_; +}; + +struct WeightedMeanLayerUpdatePolicy { + WeightedMeanState operator()( + const WeightedMeanState& current, + const LayerObservation& observation) const { + const FloatingPoint weight = + std::max(0.f, observation.confidence.value_or(1.f)); + if (weight == 0.f) { + return current; + } + return {current.weighted_sum + weight * observation.value, + current.total_weight + weight}; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_UPDATE_POLICY_H_ diff --git a/library/cpp/include/wavemap/layered/integration/layered_pipeline.h b/library/cpp/include/wavemap/layered/integration/layered_pipeline.h new file mode 100644 index 000000000..a66ac487a --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/layered_pipeline.h @@ -0,0 +1,56 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_PIPELINE_H_ +#define WAVEMAP_LAYERED_LAYERED_PIPELINE_H_ + +#include + +#include +#include + +namespace wavemap::layered { + +// A typed, sensor-independent input path for additional map layers. It is +// intentionally independent of wavemap::Pipeline so the original occupancy +// pipeline remains unchanged. +template +class LayeredPipeline { + public: + explicit LayeredPipeline(LayeredMapT& map) : map_(map) {} + + template + LayerIntegrationResult integrate( + const std::vector>>& + observations) { + return integrateLayerObservations(map_, observations); + } + + template + LayerIntegrationResult integrate( + const std::vector>>& observations, + const LayerUpdatePolicyT& policy) { + return integrateLayerObservations(map_, observations, policy); + } + + template + LayerIntegrationResult integrateBatch(const ObservationBatchT& batch) { + LayerIntegrationResult total; + batch.forEachLayer([&](auto layer_tag, const auto& observations) { + using LayerTag = typename decltype(layer_tag)::Type; + if (observations.empty()) { + return; + } + const auto result = integrate(observations); + total.received += result.received; + total.integrated += result.integrated; + total.rejected += result.rejected; + total.updated_voxels += result.updated_voxels; + }); + return total; + } + + private: + LayeredMapT& map_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_PIPELINE_H_ diff --git a/library/cpp/include/wavemap/layered/integration/pointcloud_layer_integration_mode.h b/library/cpp/include/wavemap/layered/integration/pointcloud_layer_integration_mode.h new file mode 100644 index 000000000..f438caed0 --- /dev/null +++ b/library/cpp/include/wavemap/layered/integration/pointcloud_layer_integration_mode.h @@ -0,0 +1,37 @@ +#ifndef WAVEMAP_LAYERED_POINTCLOUD_LAYER_INTEGRATION_MODE_H_ +#define WAVEMAP_LAYERED_POINTCLOUD_LAYER_INTEGRATION_MODE_H_ + +#include + +#include +#include + +namespace wavemap::layered { + +// Where a value attached to a PointCloud2 sample is written. EndpointOnly is +// the appropriate default for surface measurements such as reflectivity, +// color, roughness, and class. AlongRay is intentionally opt-in because it is +// both more expensive and only meaningful for values that describe traversed +// free space. +enum class PointcloudLayerIntegrationMode { kEndpointOnly, kAlongRay }; + +template +void forEachPointcloudIntegrationIndex(const Point3D& sensor_origin, + const Point3D& endpoint, + FloatingPoint cell_width, + VisitorT&& visitor) { + static_assert(Mode == PointcloudLayerIntegrationMode::kEndpointOnly || + Mode == PointcloudLayerIntegrationMode::kAlongRay, + "Unsupported pointcloud layer integration mode."); + if constexpr (Mode == PointcloudLayerIntegrationMode::kEndpointOnly) { + visitor(convert::pointToNearestIndex(endpoint, 1.f / cell_width)); + } else { + for (const Index3D& index : Ray<3>(sensor_origin, endpoint, cell_width)) { + visitor(index); + } + } +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_POINTCLOUD_LAYER_INTEGRATION_MODE_H_ diff --git a/library/cpp/include/wavemap/layered/io/descriptor_layered_map_io.h b/library/cpp/include/wavemap/layered/io/descriptor_layered_map_io.h new file mode 100644 index 000000000..b2d469b03 --- /dev/null +++ b/library/cpp/include/wavemap/layered/io/descriptor_layered_map_io.h @@ -0,0 +1,113 @@ +#ifndef WAVEMAP_LAYERED_DESCRIPTOR_LAYERED_MAP_IO_H_ +#define WAVEMAP_LAYERED_DESCRIPTOR_LAYERED_MAP_IO_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap::layered { + +template +std::vector descriptorContinuousLayerSchema() { + std::vector schema; + const auto descriptors = layerDescriptors(); + schema.reserve(std::tuple_size_v); + std::apply( + [&](const auto&... descriptor) { + (schema.emplace_back( + LayerSchemaEntry{ + std::string(descriptor.name()), + LayerStreamCodec< + std::remove_cv_t()))>>>:: + typeName()}), + ...); + }, + descriptors); + return schema; +} + +template +struct DescriptorVoxelSerializer { + static void write(std::ostream& ostream, const VoxelT& voxel) { + wavemap::io::StreamableCellData::write(ostream, voxel.occupancy); + forEachLayerDescriptor( + voxel.data, [&](const auto& /*descriptor*/, const auto& value) { + using Value = + std::remove_cv_t>; + LayerStreamCodec::write(ostream, value); + }); + } + + static VoxelT read(std::istream& istream) { + VoxelT voxel; + voxel.occupancy = + wavemap::io::StreamableCellData::read(istream); + forEachLayerDescriptor( + voxel.data, [&](const auto& /*descriptor*/, auto& value) { + using Value = + std::remove_cv_t>; + value = LayerStreamCodec::read(istream); + }); + return voxel; + } +}; + +template +auto descriptorNamedDiscreteLayers(DiscreteLayersT& layers) { + return transformLayerDescriptors( + layerDescriptors(), [&](const auto& descriptor) { + return io::namedDiscreteLayer(std::string(descriptor.name()), + descriptor.value(layers)); + }); +} + +template +auto descriptorNamedDiscreteLayers(const DiscreteLayersT& layers) { + return transformLayerDescriptors( + layerDescriptors(), [&](const auto& descriptor) { + return io::namedDiscreteLayer(std::string(descriptor.name()), + descriptor.value(layers)); + }); +} + +template +struct ContinuousLayerSchemaTraits< + schema::ContinuousLayerBundle>> { + using Bundle = + schema::ContinuousLayerBundle>; + + static std::vector layers() { + return descriptorContinuousLayerSchema(); + } +}; + +} // namespace wavemap::layered + +namespace wavemap::layered::io { + +template +struct DiscreteLayerBundleTraits< + schema::DiscreteLayerBundle>> { + using Bundle = + schema::DiscreteLayerBundle>; + + static auto layers(Bundle& bundle) { + return descriptorNamedDiscreteLayers(bundle); + } + + static auto layers(const Bundle& bundle) { + return descriptorNamedDiscreteLayers(bundle); + } +}; + +} // namespace wavemap::layered::io + +#endif // WAVEMAP_LAYERED_DESCRIPTOR_LAYERED_MAP_IO_H_ diff --git a/library/cpp/include/wavemap/layered/io/layer_stream_codec.h b/library/cpp/include/wavemap/layered/io/layer_stream_codec.h new file mode 100644 index 000000000..7a462afca --- /dev/null +++ b/library/cpp/include/wavemap/layered/io/layer_stream_codec.h @@ -0,0 +1,86 @@ +#ifndef WAVEMAP_LAYERED_LAYER_STREAM_CODEC_H_ +#define WAVEMAP_LAYERED_LAYER_STREAM_CODEC_H_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace wavemap::layered { + +template +struct LayerStreamCodec; + +namespace detail { +template +struct HasLayerStreamCodec : std::false_type {}; + +template +struct HasLayerStreamCodec< + ValueT, + std::void_t::typeName()), + decltype(LayerStreamCodec::write( + std::declval(), + std::declval())), + decltype(LayerStreamCodec::read( + std::declval()))>> : std::true_type {}; +} // namespace detail + +template +inline constexpr bool kHasLayerStreamCodec = + detail::HasLayerStreamCodec::value; + +template <> +struct LayerStreamCodec { + static constexpr const char* typeName() { return "float32"; } + + static void write(std::ostream& ostream, FloatingPoint value) { + wavemap::io::StreamableCellData::write(ostream, value); + } + + static FloatingPoint read(std::istream& istream) { + return wavemap::io::StreamableCellData::read(istream); + } +}; + +template <> +struct LayerStreamCodec { + static constexpr const char* typeName() { return "float32_rgb"; } + + static void write(std::ostream& ostream, const Rgb& value) { + LayerStreamCodec::write(ostream, value.r); + LayerStreamCodec::write(ostream, value.g); + LayerStreamCodec::write(ostream, value.b); + } + + static Rgb read(std::istream& istream) { + return {LayerStreamCodec::read(istream), + LayerStreamCodec::read(istream), + LayerStreamCodec::read(istream)}; + } +}; + +template <> +struct LayerStreamCodec { + static constexpr const char* typeName() { + return "float32_weighted_mean_state"; + } + + static void write(std::ostream& ostream, const WeightedMeanState& state) { + LayerStreamCodec::write(ostream, state.weighted_sum); + LayerStreamCodec::write(ostream, state.total_weight); + } + + static WeightedMeanState read(std::istream& istream) { + return {LayerStreamCodec::read(istream), + LayerStreamCodec::read(istream)}; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_STREAM_CODEC_H_ diff --git a/library/cpp/include/wavemap/layered/io/layered_map_io.h b/library/cpp/include/wavemap/layered/io/layered_map_io.h new file mode 100644 index 000000000..59bc22e77 --- /dev/null +++ b/library/cpp/include/wavemap/layered/io/layered_map_io.h @@ -0,0 +1,586 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_IO_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_IO_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace wavemap::layered::io { +namespace detail { +template +inline constexpr bool kAlwaysFalse = false; + +constexpr char kMagic[] = "LWVMP"; +constexpr uint32_t kVersion = 1u; +constexpr char kSchemaSection[] = "schema"; +constexpr char kContinuousMapSection[] = "continuous_map"; +constexpr char kDiscreteLayersSection[] = "discrete_layers"; + +inline bool writeBytes(std::ostream& ostream, const char* data, size_t size) { + ostream.write(data, static_cast(size)); + return static_cast(ostream); +} + +inline bool readBytes(std::istream& istream, char* data, size_t size) { + istream.read(data, static_cast(size)); + return static_cast(istream); +} + +template +bool writePod(std::ostream& ostream, const T& value) { + static_assert(std::is_trivially_copyable_v); + return writeBytes(ostream, reinterpret_cast(&value), sizeof(T)); +} + +template +bool readPod(std::istream& istream, T& value) { + static_assert(std::is_trivially_copyable_v); + return readBytes(istream, reinterpret_cast(&value), sizeof(T)); +} + +inline bool writeString(std::ostream& ostream, const std::string& value) { + const uint64_t size = value.size(); + return writePod(ostream, size) && writeBytes(ostream, value.data(), value.size()); +} + +inline bool readString(std::istream& istream, std::string& value) { + uint64_t size = 0u; + if (!readPod(istream, size)) { + return false; + } + value.resize(size); + return readBytes(istream, value.data(), value.size()); +} + +inline bool writeSectionMarker(std::ostream& ostream, + const std::string& section_name) { + return writeString(ostream, section_name); +} + +inline bool readSectionMarker(std::istream& istream, + const std::string& expected_section_name) { + std::string section_name; + return readString(istream, section_name) && + section_name == expected_section_name; +} + +inline bool writeFileHeader(std::ostream& ostream) { + return writeString(ostream, kMagic) && writePod(ostream, kVersion); +} + +inline bool readFileHeader(std::istream& istream, uint32_t& version) { + std::string magic; + version = 0u; + return readString(istream, magic) && magic == kMagic && + readPod(istream, version) && version == kVersion; +} +} // namespace detail + +inline bool writeLayerSchemaEntries( + std::ostream& ostream, const std::vector& entries) { + const uint64_t entry_count = entries.size(); + if (!detail::writePod(ostream, entry_count)) { + return false; + } + for (const auto& entry : entries) { + if (!detail::writeString(ostream, entry.name) || + !detail::writeString(ostream, entry.type)) { + return false; + } + } + return static_cast(ostream); +} + +inline bool readLayerSchemaEntries( + std::istream& istream, std::vector& entries) { + uint64_t entry_count = 0u; + if (!detail::readPod(istream, entry_count)) { + return false; + } + entries.clear(); + entries.reserve(entry_count); + for (uint64_t entry_i = 0u; entry_i < entry_count; ++entry_i) { + auto& entry = entries.emplace_back(); + if (!detail::readString(istream, entry.name) || + !detail::readString(istream, entry.type)) { + return false; + } + } + return true; +} + +inline bool writeLayeredMapSchema(std::ostream& ostream, + const LayeredMapSchema& schema) { + return writeLayerSchemaEntries(ostream, schema.continuous_layers) && + writeLayerSchemaEntries(ostream, schema.discrete_layers); +} + +inline bool readLayeredMapSchema(std::istream& istream, + LayeredMapSchema& schema) { + return readLayerSchemaEntries(istream, schema.continuous_layers) && + readLayerSchemaEntries(istream, schema.discrete_layers); +} + +inline bool readLayeredMapSchema(const std::filesystem::path& file_path, + LayeredMapSchema& schema) { + std::ifstream istream(file_path, std::ios::binary); + if (!istream.is_open()) { + return false; + } + + uint32_t version = 0u; + if (!detail::readFileHeader(istream, version)) { + return false; + } + + return detail::readSectionMarker(istream, detail::kSchemaSection) && + readLayeredMapSchema(istream, schema); +} + +template +struct DiscreteValueSerializer { + static constexpr const char* typeName() { + static_assert(detail::kAlwaysFalse, + "DiscreteValueSerializer must be specialized before " + "DiscreteLayer can be saved or loaded. Add a serializer " + "for this value type, for example by following the int/bool " + "specializations in layered_map_io.h."); + return "unsupported"; + } + + static bool write(std::ostream&, const ValueT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteValueSerializer::write is not specialized for " + "this discrete value type."); + return false; + } + + static bool read(std::istream&, ValueT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteValueSerializer::read is not specialized for " + "this discrete value type."); + return false; + } +}; + +template <> +struct DiscreteValueSerializer { + static constexpr const char* typeName() { return "int"; } + + static bool write(std::ostream& ostream, int value) { + const int32_t serialized = value; + return detail::writePod(ostream, serialized); + } + + static bool read(std::istream& istream, int& value) { + int32_t serialized = 0; + if (!detail::readPod(istream, serialized)) { + return false; + } + value = serialized; + return true; + } +}; + +template <> +struct DiscreteValueSerializer { + static constexpr const char* typeName() { return "bool"; } + + static bool write(std::ostream& ostream, bool value) { + const uint8_t serialized = value ? 1u : 0u; + return detail::writePod(ostream, serialized); + } + + static bool read(std::istream& istream, bool& value) { + uint8_t serialized = 0u; + if (!detail::readPod(istream, serialized)) { + return false; + } + value = serialized != 0u; + return true; + } +}; + +template +struct NamedDiscreteLayer { + std::string name; + DiscreteLayer& layer; +}; + +template +struct ConstNamedDiscreteLayer { + std::string name; + const DiscreteLayer& layer; +}; + +template +NamedDiscreteLayer namedDiscreteLayer(const std::string& name, + DiscreteLayer& layer) { + return {name, layer}; +} + +template +ConstNamedDiscreteLayer namedDiscreteLayer( + const std::string& name, const DiscreteLayer& layer) { + return {name, layer}; +} + +template +struct DiscreteLayerBundleTraits { + static auto layers(DiscreteLayersT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteLayerBundleTraits must be specialized for your " + "discrete layer bundle. Return a tuple of " + "namedDiscreteLayer(name, layers.layer_field) entries."); + return std::tuple<>(); + } + + static auto layers(const DiscreteLayersT&) { + static_assert(detail::kAlwaysFalse, + "DiscreteLayerBundleTraits must be specialized for your " + "discrete layer bundle. Return a tuple of " + "namedDiscreteLayer(name, layers.layer_field) entries."); + return std::tuple<>(); + } +}; + +template +struct DiscreteLayerSerializer { + static bool write(std::ostream& ostream, const DiscreteLayer& layer) { + const int32_t block_height = layer.config().block_height; + if (!detail::writePod(ostream, block_height)) { + return false; + } + + const uint64_t parent_count = layer.cells().size(); + if (!detail::writePod(ostream, parent_count)) { + return false; + } + + for (const auto& [parent_key, cell] : layer.cells()) { + const int32_t x = parent_key.x; + const int32_t y = parent_key.y; + const int32_t z = parent_key.z; + if (!detail::writePod(ostream, x) || !detail::writePod(ostream, y) || + !detail::writePod(ostream, z) || + !DiscreteValueSerializer::write(ostream, cell.dominant_value)) { + return false; + } + + const uint64_t observed_count = cell.observed_offsets.size(); + if (!detail::writePod(ostream, observed_count)) { + return false; + } + for (const int offset : cell.observed_offsets) { + const int32_t serialized_offset = offset; + if (!detail::writePod(ostream, serialized_offset)) { + return false; + } + } + + const uint64_t exception_count = cell.exceptions.size(); + if (!detail::writePod(ostream, exception_count)) { + return false; + } + for (const auto& [offset, value] : cell.exceptions) { + const int32_t serialized_offset = offset; + if (!detail::writePod(ostream, serialized_offset) || + !DiscreteValueSerializer::write(ostream, value)) { + return false; + } + } + } + return static_cast(ostream); + } + + static bool read(std::istream& istream, DiscreteLayer& layer) { + int32_t block_height = 0; + uint64_t parent_count = 0u; + if (!detail::readPod(istream, block_height) || + !detail::readPod(istream, parent_count)) { + return false; + } + + std::map::Cell> cells; + for (uint64_t parent_i = 0u; parent_i < parent_count; ++parent_i) { + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + typename DiscreteLayer::Cell cell; + if (!detail::readPod(istream, x) || !detail::readPod(istream, y) || + !detail::readPod(istream, z) || + !DiscreteValueSerializer::read(istream, cell.dominant_value)) { + return false; + } + + uint64_t observed_count = 0u; + if (!detail::readPod(istream, observed_count)) { + return false; + } + for (uint64_t observed_i = 0u; observed_i < observed_count; ++observed_i) { + int32_t offset = 0; + if (!detail::readPod(istream, offset)) { + return false; + } + cell.observed_offsets.insert(offset); + } + + uint64_t exception_count = 0u; + if (!detail::readPod(istream, exception_count)) { + return false; + } + for (uint64_t exception_i = 0u; exception_i < exception_count; + ++exception_i) { + int32_t offset = 0; + ValueT value{}; + if (!detail::readPod(istream, offset) || + !DiscreteValueSerializer::read(istream, value)) { + return false; + } + cell.exceptions[offset] = value; + } + + cells[IndexKey(wavemap::Index3D(x, y, z))] = cell; + } + + layer.replaceCellsForLoad(DiscreteCompressionConfig{block_height}, + std::move(cells)); + return true; + } +}; + +template +bool forEachTupleElementImpl(TupleT&& tuple, FuncT&& func, + std::index_sequence) { + bool ok = true; + ((ok = ok && func(std::get(tuple))), ...); + return ok; +} + +template +bool forEachTupleElement(TupleT&& tuple, FuncT&& func) { + constexpr size_t tuple_size = + std::tuple_size_v>; + return forEachTupleElementImpl(std::forward(tuple), + std::forward(func), + std::make_index_sequence{}); +} + +template +LayerSchemaEntry discreteLayerSchemaEntry( + const ConstNamedDiscreteLayer& named_layer) { + return {named_layer.name, DiscreteValueSerializer::typeName()}; +} + +template +LayerSchemaEntry discreteLayerSchemaEntry( + const NamedDiscreteLayer& named_layer) { + return {named_layer.name, DiscreteValueSerializer::typeName()}; +} + +template +std::vector discreteLayerBundleSchema( + const DiscreteLayersT& layers) { + std::vector schema; + const auto named_layers = + DiscreteLayerBundleTraits::layers(layers); + schema.reserve( + std::tuple_size_v>); + forEachTupleElement(named_layers, [&](const auto& named_layer) { + schema.emplace_back(discreteLayerSchemaEntry(named_layer)); + return true; + }); + return schema; +} + +template +LayeredMapSchema layeredMapSchema(const LayeredMapT& map) { + return {continuousLayerSchema(), + discreteLayerBundleSchema(map.discreteLayers())}; +} + +template +bool writeDiscreteLayerBundle(std::ostream& ostream, + const DiscreteLayersT& layers) { + const auto named_layers = DiscreteLayerBundleTraits::layers(layers); + constexpr uint64_t layer_count = + std::tuple_size_v>; + if (!detail::writePod(ostream, layer_count)) { + return false; + } + return forEachTupleElement(named_layers, [&](const auto& named_layer) { + using ValueT = typename std::remove_reference_t::Value; + return detail::writeString(ostream, named_layer.name) && + detail::writeString(ostream, DiscreteValueSerializer::typeName()) && + DiscreteLayerSerializer::write(ostream, named_layer.layer); + }); +} + +template +bool readDiscreteLayerBundle(std::istream& istream, DiscreteLayersT& layers) { + const auto named_layers = DiscreteLayerBundleTraits::layers(layers); + constexpr uint64_t expected_layer_count = + std::tuple_size_v>; + uint64_t layer_count = 0u; + if (!detail::readPod(istream, layer_count) || + layer_count != expected_layer_count) { + return false; + } + + return forEachTupleElement(named_layers, [&](auto named_layer) { + using ValueT = typename std::remove_reference_t::Value; + std::string name; + std::string type_name; + if (!detail::readString(istream, name) || + !detail::readString(istream, type_name)) { + return false; + } + if (name != named_layer.name || + type_name != DiscreteValueSerializer::typeName()) { + return false; + } + return DiscreteLayerSerializer::read(istream, named_layer.layer); + }); +} + +template +bool saveLayeredMap(const std::filesystem::path& file_path, + const LayeredMapT& map) { + std::ofstream ostream(file_path, std::ios::binary); + if (!ostream.is_open()) { + return false; + } + + const LayeredMapSchema schema = layeredMapSchema(map); + if (!detail::writeFileHeader(ostream) || + !detail::writeSectionMarker(ostream, detail::kSchemaSection) || + !writeLayeredMapSchema(ostream, schema) || + !detail::writeSectionMarker(ostream, detail::kContinuousMapSection)) { + return false; + } + if (!wavemap::io::mapToStream( + map.continuousMap(), ostream)) { + return false; + } + if (!detail::writeSectionMarker(ostream, detail::kDiscreteLayersSection)) { + return false; + } + return writeDiscreteLayerBundle(ostream, map.discreteLayers()); +} + +template +bool loadLayeredMap(const std::filesystem::path& file_path, LayeredMapT& map, + std::string* error_message = nullptr) { + std::ifstream istream(file_path, std::ios::binary); + if (!istream.is_open()) { + if (error_message) { + *error_message = "Could not open LayeredMap file: " + file_path.string(); + } + return false; + } + + uint32_t version = 0u; + if (!detail::readFileHeader(istream, version)) { + if (error_message) { + *error_message = "Invalid LayeredMap file header or unsupported version."; + } + return false; + } + + LayeredMapSchema schema; + if (!detail::readSectionMarker(istream, detail::kSchemaSection) || + !readLayeredMapSchema(istream, schema)) { + if (error_message) { + *error_message = "Failed to read LayeredMap schema section."; + } + return false; + } + + const LayeredMapSchema expected_schema = layeredMapSchema(map); + if (!checkLayeredMapSchemaCompatibility(schema, expected_schema, + error_message)) { + return false; + } + + if (!detail::readSectionMarker(istream, detail::kContinuousMapSection)) { + if (error_message) { + *error_message = "Failed to find LayeredMap continuous map section."; + } + return false; + } + + typename LayeredMapT::ContinuousMap::Ptr continuous_map; + if (!wavemap::io::streamToMap(istream, + continuous_map) || + !continuous_map) { + if (error_message) { + *error_message = "Failed to deserialize LayeredMap continuous map data."; + } + return false; + } + + typename LayeredMapT::DiscreteLayers discrete_layers; + if (!detail::readSectionMarker(istream, detail::kDiscreteLayersSection) || + !readDiscreteLayerBundle(istream, discrete_layers)) { + if (error_message) { + *error_message = "Failed to deserialize LayeredMap discrete layer data."; + } + return false; + } + + // Keep the destination map object and its continuous-map allocation alive. + // The ROS server's occupancy pipeline and layered integrators may retain a + // shared pointer to this exact octree. Replacing LayeredMap here would leave + // those components writing into the old allocation and would also discard + // the destination's runtime threshold and pruning policies, which are not + // part of the serialized map payload. + auto& destination_continuous_map = map.continuousMap(); + if (continuous_map->getConfig() != destination_continuous_map.getConfig()) { + if (error_message) { + *error_message = + "Loaded LayeredMap geometry is incompatible with the configured " + "destination map."; + } + return false; + } + + destination_continuous_map.getHashMap() = + std::move(continuous_map->getHashMap()); + map.discreteLayers() = std::move(discrete_layers); + return true; +} + + +template +struct LayeredMapIo { + static bool save(const std::filesystem::path& file_path, + const LayeredMapT& map) { + return saveLayeredMap(file_path, + map); + } + + static bool load(const std::filesystem::path& file_path, LayeredMapT& map, + std::string* error_message = nullptr) { + return loadLayeredMap( + file_path, map, error_message); + } +}; + +} // namespace wavemap::layered::io + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_IO_H_ diff --git a/library/cpp/include/wavemap/layered/io/layered_map_schema.h b/library/cpp/include/wavemap/layered/io/layered_map_schema.h new file mode 100644 index 000000000..4b28d50e5 --- /dev/null +++ b/library/cpp/include/wavemap/layered/io/layered_map_schema.h @@ -0,0 +1,111 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_SCHEMA_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_SCHEMA_H_ + +#include +#include +#include + +namespace wavemap::layered { +namespace detail { +template +inline constexpr bool kLayeredMapSchemaAlwaysFalse = false; +} + +struct LayerSchemaEntry { + std::string name; + std::string type; + + bool operator==(const LayerSchemaEntry& other) const { + return name == other.name && type == other.type; + } + + bool operator!=(const LayerSchemaEntry& other) const { + return !(*this == other); + } +}; + +struct LayeredMapSchema { + std::vector continuous_layers; + std::vector discrete_layers; + + bool operator==(const LayeredMapSchema& other) const { + return continuous_layers == other.continuous_layers && + discrete_layers == other.discrete_layers; + } + + bool operator!=(const LayeredMapSchema& other) const { + return !(*this == other); + } +}; + +inline std::string layerSchemaEntriesToString( + const std::vector& entries) { + std::ostringstream stream; + if (entries.empty()) { + stream << "none"; + return stream.str(); + } + + for (size_t entry_i = 0u; entry_i < entries.size(); ++entry_i) { + if (entry_i != 0u) { + stream << ", "; + } + stream << entries[entry_i].name << " (" << entries[entry_i].type << ")"; + } + return stream.str(); +} + +inline std::string layeredMapSchemaToString(const LayeredMapSchema& schema) { + std::ostringstream stream; + stream << "continuous: " + << layerSchemaEntriesToString(schema.continuous_layers) + << "; discrete: " + << layerSchemaEntriesToString(schema.discrete_layers); + return stream.str(); +} + +inline bool checkLayeredMapSchemaCompatibility( + const LayeredMapSchema& file_schema, const LayeredMapSchema& expected_schema, + std::string* error_message = nullptr) { + if (file_schema == expected_schema) { + return true; + } + + if (error_message) { + std::ostringstream stream; + stream << "Layered map schema mismatch. File has [" + << layeredMapSchemaToString(file_schema) << "]; expected [" + << layeredMapSchemaToString(expected_schema) << "]."; + *error_message = stream.str(); + } + return false; +} + +template +struct ContinuousLayerSchemaTraits { + static std::vector layers() { + static_assert(detail::kLayeredMapSchemaAlwaysFalse, + "ContinuousLayerSchemaTraits must be specialized for " + "your continuous layer bundle. Return the custom " + "continuous layers, excluding occupancy, because " + "occupancy is part of every LayeredMap."); + return {}; + } +}; + +inline std::vector occupancySchemaEntry() { + return {{"occupancy", "float32"}}; +} + +template +std::vector continuousLayerSchema() { + auto layers = occupancySchemaEntry(); + const auto custom_layers = + ContinuousLayerSchemaTraits::layers(); + layers.insert(layers.end(), custom_layers.begin(), custom_layers.end()); + return layers; +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_SCHEMA_H_ diff --git a/library/cpp/include/wavemap/layered/map/continuous_map_backend.h b/library/cpp/include/wavemap/layered/map/continuous_map_backend.h new file mode 100644 index 000000000..0c8f40db0 --- /dev/null +++ b/library/cpp/include/wavemap/layered/map/continuous_map_backend.h @@ -0,0 +1,21 @@ +#ifndef WAVEMAP_LAYERED_MAP_CONTINUOUS_MAP_BACKEND_H_ +#define WAVEMAP_LAYERED_MAP_CONTINUOUS_MAP_BACKEND_H_ + +#include +#include + +namespace wavemap::layered { + +struct HashedWaveletOctreeBackend { + template + using Map = wavemap::HashedWaveletOctreeT; +}; + +struct HashedChunkedWaveletOctreeBackend { + template + using Map = wavemap::HashedChunkedWaveletOctreeT; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_MAP_CONTINUOUS_MAP_BACKEND_H_ diff --git a/library/cpp/include/wavemap/layered/map/discrete_layer.h b/library/cpp/include/wavemap/layered/map/discrete_layer.h new file mode 100644 index 000000000..f9e26aaaf --- /dev/null +++ b/library/cpp/include/wavemap/layered/map/discrete_layer.h @@ -0,0 +1,327 @@ +#ifndef WAVEMAP_LAYERED_DISCRETE_LAYER_H_ +#define WAVEMAP_LAYERED_DISCRETE_LAYER_H_ + +#include +#include +#include +#include +#include +#include + +#include + +namespace wavemap::layered { + +struct IndexKey { + int x = 0; + int y = 0; + int z = 0; + + explicit IndexKey(const wavemap::Index3D& index) + : x(index.x()), y(index.y()), z(index.z()) {} + + bool operator<(const IndexKey& other) const { + return std::tie(x, y, z) < std::tie(other.x, other.y, other.z); + } +}; + +inline wavemap::Index3D indexFromKey(const IndexKey& key) { + return wavemap::Index3D(key.x, key.y, key.z); +} + +struct DiscreteCompressionConfig { + // Matches one octree level by default: one parent represents 2x2x2 children. + int block_height = 1; + + int blockSideLength() const { return 1 << block_height; } + + int blockVoxelCount() const { + const int side_length = blockSideLength(); + return side_length * side_length * side_length; + } +}; + +inline int floorDiv(int value, int divisor) { + int quotient = value / divisor; + const int remainder = value % divisor; + if (remainder != 0 && ((remainder < 0) != (divisor < 0))) { + --quotient; + } + return quotient; +} + +inline int positiveModulo(int value, int divisor) { + const int remainder = value % divisor; + return remainder < 0 ? remainder + divisor : remainder; +} + +inline IndexKey parentKey(const wavemap::Index3D& index, + const DiscreteCompressionConfig& config) { + const int side_length = config.blockSideLength(); + return IndexKey(wavemap::Index3D(floorDiv(index.x(), side_length), + floorDiv(index.y(), side_length), + floorDiv(index.z(), side_length))); +} + +inline int childOffset(const wavemap::Index3D& index, + const DiscreteCompressionConfig& config) { + const int side_length = config.blockSideLength(); + const int x = positiveModulo(index.x(), side_length); + const int y = positiveModulo(index.y(), side_length); + const int z = positiveModulo(index.z(), side_length); + return x + side_length * (y + side_length * z); +} + +template +class RawDiscreteLayer { + public: + using Value = ValueT; + + void setValue(const wavemap::Index3D& index, const ValueT& value) { + values_[IndexKey(index)] = value; + } + + std::optional getValue(const wavemap::Index3D& index) const { + const auto it = values_.find(IndexKey(index)); + if (it == values_.end()) { + return std::nullopt; + } + return it->second; + } + + bool hasValue(const wavemap::Index3D& index) const { + return values_.find(IndexKey(index)) != values_.end(); + } + + size_t size() const { return values_.size(); } + + void clear() { values_.clear(); } + + const std::map& values() const { return values_; } + + private: + std::map values_; +}; + +template +struct CompressedDiscreteCell { + ValueT dominant_value{}; + std::set observed_offsets; + std::map exceptions; +}; + +template +class DiscreteLayer { + public: + using Value = ValueT; + using Cell = CompressedDiscreteCell; + + explicit DiscreteLayer( + DiscreteCompressionConfig config = DiscreteCompressionConfig()) + : config_(config) {} + + static DiscreteLayer fromRaw( + const RawDiscreteLayer& layer, + DiscreteCompressionConfig config = DiscreteCompressionConfig()) { + std::map> grouped_values; + for (const auto& [key, value] : layer.values()) { + const wavemap::Index3D index = indexFromKey(key); + grouped_values[parentKey(index, config)][childOffset(index, config)] = + value; + } + + DiscreteLayer compressed(config); + for (const auto& [parent_key, child_values] : grouped_values) { + Cell cell; + compressed.rebuildCell(child_values, cell); + compressed.cells_[parent_key] = cell; + } + return compressed; + } + + static DiscreteLayer fromRaw2x2x2( + const RawDiscreteLayer& layer) { + return fromRaw(layer, DiscreteCompressionConfig{1}); + } + + void setValue(const wavemap::Index3D& index, const ValueT& value) { + const IndexKey parent_key = parentKey(index, config_); + const int offset = childOffset(index, config_); + const auto cell_it = cells_.find(parent_key); + std::map values = + cell_it == cells_.end() ? std::map{} + : expandCell(cell_it->second); + const auto value_it = values.find(offset); + if (value_it != values.end() && value_it->second == value) { + return; + } + values[offset] = value; + rebuildCell(values, cells_[parent_key]); + dirty_parent_keys_.insert(parent_key); + } + + void setValues( + const std::vector>& updates) { + std::map> grouped_updates; + for (const auto& [index, value] : updates) { + grouped_updates[parentKey(index, config_)][childOffset(index, config_)] = + value; + } + + for (const auto& [parent_key, parent_updates] : grouped_updates) { + const auto cell_it = cells_.find(parent_key); + std::map values = + cell_it == cells_.end() ? std::map{} + : expandCell(cell_it->second); + bool changed = false; + for (const auto& [offset, value] : parent_updates) { + const auto value_it = values.find(offset); + if (value_it != values.end() && value_it->second == value) { + continue; + } + values[offset] = value; + changed = true; + } + if (!changed) { + continue; + } + rebuildCell(values, cells_[parent_key]); + dirty_parent_keys_.insert(parent_key); + } + } + + bool eraseValue(const wavemap::Index3D& index) { + const IndexKey parent_key = parentKey(index, config_); + const auto cell_it = cells_.find(parent_key); + if (cell_it == cells_.end()) { + return false; + } + + std::map values = expandCell(cell_it->second); + if (values.erase(childOffset(index, config_)) == 0u) { + return false; + } + + if (values.empty()) { + cells_.erase(cell_it); + } else { + rebuildCell(values, cell_it->second); + } + dirty_parent_keys_.insert(parent_key); + return true; + } + + std::optional getValue(const wavemap::Index3D& index) const { + const auto cell_it = cells_.find(parentKey(index, config_)); + if (cell_it == cells_.end()) { + return std::nullopt; + } + const Cell& cell = cell_it->second; + const int offset = childOffset(index, config_); + if (cell.observed_offsets.find(offset) == cell.observed_offsets.end()) { + return std::nullopt; + } + const auto exception_it = cell.exceptions.find(offset); + if (exception_it != cell.exceptions.end()) { + return exception_it->second; + } + return cell.dominant_value; + } + + size_t parentCount() const { return cells_.size(); } + + size_t exceptionCount() const { + size_t count = 0u; + for (const auto& [parent_key, cell] : cells_) { + (void)parent_key; + count += cell.exceptions.size(); + } + return count; + } + + size_t observedValueCount() const { + size_t count = 0u; + for (const auto& [parent_key, cell] : cells_) { + (void)parent_key; + count += cell.observed_offsets.size(); + } + return count; + } + + void clear() { + for (const auto& [parent_key, cell] : cells_) { + (void)cell; + dirty_parent_keys_.insert(parent_key); + } + cells_.clear(); + } + + std::optional dominantValue(const wavemap::Index3D& index) const { + const auto cell_it = cells_.find(parentKey(index, config_)); + if (cell_it == cells_.end()) { + return std::nullopt; + } + return cell_it->second.dominant_value; + } + + const DiscreteCompressionConfig& config() const { return config_; } + + const std::map& cells() const { return cells_; } + + const std::set& dirtyParentKeys() const { + return dirty_parent_keys_; + } + + void clearDirtyParentKeys() { dirty_parent_keys_.clear(); } + + void replaceCellsForLoad(DiscreteCompressionConfig config, + std::map cells) { + config_ = config; + cells_ = std::move(cells); + dirty_parent_keys_.clear(); + } + + private: + std::map expandCell(const Cell& cell) const { + std::map values; + for (const int offset : cell.observed_offsets) { + const auto exception_it = cell.exceptions.find(offset); + values[offset] = exception_it != cell.exceptions.end() + ? exception_it->second + : cell.dominant_value; + } + return values; + } + + void rebuildCell(const std::map& values, Cell& cell) const { + std::map counts; + ValueT dominant_value{}; + size_t dominant_count = 0u; + for (const auto& [offset, value] : values) { + (void)offset; + const size_t count = ++counts[value]; + if (dominant_count < count) { + dominant_value = value; + dominant_count = count; + } + } + + cell.dominant_value = dominant_value; + cell.observed_offsets.clear(); + cell.exceptions.clear(); + for (const auto& [offset, value] : values) { + cell.observed_offsets.insert(offset); + if (value != dominant_value) { + cell.exceptions[offset] = value; + } + } + } + + DiscreteCompressionConfig config_; + std::map cells_; + std::set dirty_parent_keys_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_DISCRETE_LAYER_H_ diff --git a/library/cpp/include/wavemap/layered/map/layered_map.h b/library/cpp/include/wavemap/layered/map/layered_map.h new file mode 100644 index 000000000..8c46ff7dd --- /dev/null +++ b/library/cpp/include/wavemap/layered/map/layered_map.h @@ -0,0 +1,78 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_H_ + +#include +#include + +#include +#include +#include + +namespace wavemap::layered { + +template +using LayeredMapVoxel = + wavemap::VoxelData; + +template +struct LayeredMapConfig { + typename ContinuousMapT::Config continuous_map; + typename ContinuousMapT::ThresholdConfig continuous_threshold; + typename ContinuousMapT::PruningConfig continuous_pruning; + DiscreteCompressionConfig discrete_compression; +}; + +template +class LayeredMap { + public: + using ContinuousLayers = ContinuousLayersT; + using ContinuousPolicy = ContinuousPolicyT; + using ContinuousVoxel = LayeredMapVoxel; + using ContinuousMapBackend = ContinuousMapBackendT; + using ContinuousMap = typename ContinuousMapBackend::template Map< + ContinuousVoxel>; + using Config = LayeredMapConfig; + using DiscreteLayers = DiscreteLayersT; + + explicit LayeredMap(const Config& config) + : continuous_map_(std::make_shared( + config.continuous_map, config.continuous_threshold, + config.continuous_pruning)), + discrete_layers_(config.discrete_compression) {} + + explicit LayeredMap(const typename ContinuousMap::Config& config) + : continuous_map_(std::make_shared(config)), + discrete_layers_() {} + + LayeredMap(const typename ContinuousMap::Config& config, + const typename ContinuousMap::ThresholdConfig& threshold_config, + const typename ContinuousMap::PruningConfig& pruning_config) + : continuous_map_(std::make_shared( + config, threshold_config, pruning_config)), + discrete_layers_() {} + + LayeredMap(typename ContinuousMap::Ptr continuous_map, + DiscreteLayersT discrete_layers) + : continuous_map_(std::move(continuous_map)), + discrete_layers_(std::move(discrete_layers)) {} + + ContinuousMap& continuousMap() { return *continuous_map_; } + const ContinuousMap& continuousMap() const { return *continuous_map_; } + typename ContinuousMap::Ptr continuousMapPtr() { return continuous_map_; } + typename ContinuousMap::ConstPtr continuousMapPtr() const { + return continuous_map_; + } + + DiscreteLayersT& discreteLayers() { return discrete_layers_; } + const DiscreteLayersT& discreteLayers() const { return discrete_layers_; } + + private: + typename ContinuousMap::Ptr continuous_map_; + DiscreteLayersT discrete_layers_; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_H_ diff --git a/library/cpp/include/wavemap/layered/map/layered_map_config_builder.h b/library/cpp/include/wavemap/layered/map/layered_map_config_builder.h new file mode 100644 index 000000000..de19779d6 --- /dev/null +++ b/library/cpp/include/wavemap/layered/map/layered_map_config_builder.h @@ -0,0 +1,133 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_CONFIG_BUILDER_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_CONFIG_BUILDER_H_ + +#include + +#include +#include + +namespace wavemap::layered { + +// User-facing facade over LayeredMap's internal threshold and pruning bundles. +// It intentionally owns no behavior: build() returns the existing Config type +// consumed by LayeredMap. +template +class LayeredMapConfigBuilder { + public: + using Definition = MapDefinitionT; + using Schema = typename Definition::Schema; + using Config = typename Definition::Config; + + class MapSettings { + public: + explicit MapSettings(Config& config) : config_(config) {} + + MapSettings& minCellWidth(FloatingPoint meters) { + config_.continuous_map.min_cell_width = meters; + return *this; + } + MapSettings& occupancyLogOdds(FloatingPoint min_log_odds, + FloatingPoint max_log_odds) { + config_.continuous_map.min_log_odds = min_log_odds; + config_.continuous_map.max_log_odds = max_log_odds; + return *this; + } + MapSettings& treeHeight(IndexElement tree_height) { + config_.continuous_map.tree_height = tree_height; + return *this; + } + MapSettings& onlyPruneBlocksIfUnusedFor(FloatingPoint seconds) { + config_.continuous_map.only_prune_blocks_if_unused_for = seconds; + return *this; + } + + private: + Config& config_; + }; + + class OccupancySettings { + public: + explicit OccupancySettings(Config& config) : config_(config) {} + + OccupancySettings& pruningScale(FloatingPoint scale) { + config_.continuous_pruning.occupancy.scale = scale; + return *this; + } + OccupancySettings& pruningWeight(FloatingPoint weight) { + config_.continuous_pruning.occupancy.weight = weight; + return *this; + } + + private: + Config& config_; + }; + + template + class ContinuousLayerSettings { + public: + using Value = LayerValueT; + + explicit ContinuousLayerSettings(Config& config) : config_(config) { + static_assert( + Schema::template containsContinuous, + "layer() requires a continuous layer in this schema."); + } + + ContinuousLayerSettings& storageBounds(const Value& min_value, + const Value& max_value) { + auto& bounds = + config_.continuous_threshold.data.template get(); + bounds.min = min_value; + bounds.max = max_value; + return *this; + } + ContinuousLayerSettings& pruningScale(FloatingPoint scale) { + config_.continuous_pruning.data.template get().scale = scale; + return *this; + } + ContinuousLayerSettings& pruningWeight(FloatingPoint weight) { + config_.continuous_pruning.data.template get().weight = + weight; + return *this; + } + + private: + Config& config_; + }; + + LayeredMapConfigBuilder() = default; + explicit LayeredMapConfigBuilder(Config config) + : config_(std::move(config)) {} + + MapSettings map() { return MapSettings(config_); } + OccupancySettings occupancy() { return OccupancySettings(config_); } + + template + ContinuousLayerSettings layer() { + return ContinuousLayerSettings(config_); + } + + LayeredMapConfigBuilder& combinedPruningThreshold(FloatingPoint threshold) { + config_.continuous_pruning.combined_threshold = threshold; + // The additional-data bundle uses the same cutoff while scoring its own + // fields. Keep this internal value synchronized for the user. + config_.continuous_pruning.data.combined_threshold = threshold; + return *this; + } + + LayeredMapConfigBuilder& discreteBlockHeight(int block_height) { + config_.discrete_compression.block_height = block_height; + return *this; + } + + const Config& config() const { return config_; } + Config build() const { return config_; } + Config&& moveConfig() { return std::move(config_); } + + private: + Config config_{}; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_CONFIG_BUILDER_H_ diff --git a/library/cpp/include/wavemap/layered/map/layered_map_definition.h b/library/cpp/include/wavemap/layered/map/layered_map_definition.h new file mode 100644 index 000000000..b390da1fa --- /dev/null +++ b/library/cpp/include/wavemap/layered/map/layered_map_definition.h @@ -0,0 +1,77 @@ +#ifndef WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ +#define WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ + +#include +#include +#include +#include + +namespace wavemap::layered { +namespace detail { + +template +inline constexpr bool kContinuousLayerHasValueTraits = + !kIsWaveletCompatibleLayer || + schema::kHasContinuousValueTraits>; + +template +inline constexpr bool kContinuousLayerHasStreamCodec = + !kIsWaveletCompatibleLayer || + kHasLayerStreamCodec>; + +template +constexpr bool continuousLayersHaveValueTraits( + std::index_sequence) { + return (kContinuousLayerHasValueTraits< + std::tuple_element_t> && + ...); +} + +template +constexpr bool continuousLayersHaveStreamCodecs( + std::index_sequence) { + return (kContinuousLayerHasStreamCodec< + std::tuple_element_t> && + ...); +} + +} // namespace detail + +// Derives the complete core map representation from a user layer schema. +// Occupancy remains built into LayeredMapVoxel and is therefore intentionally +// not listed in SchemaT. +template +struct LayeredMapDefinition { + using Schema = SchemaT; + using ContinuousMapBackend = ContinuousMapBackendT; + using LayerTags = typename Schema::Layers; + + static_assert( + detail::continuousLayersHaveValueTraits( + std::make_index_sequence>{}), + "Every continuous layer value needs ContinuousValueTraits providing " + "add, subtract, scale, threshold, and magnitude operations."); + static_assert( + detail::continuousLayersHaveStreamCodecs( + std::make_index_sequence>{}), + "Every continuous layer value needs a LayerStreamCodec for map " + "persistence."); + + using ContinuousLayers = schema::ContinuousLayerBundle; + using ContinuousArithmetic = schema::ContinuousBundleArithmetic; + using DiscreteLayers = schema::DiscreteLayerBundle; + + using Map = LayeredMap; + using Config = typename Map::Config; + using Voxel = typename Map::ContinuousVoxel; + using ContinuousMap = typename Map::ContinuousMap; + + using VoxelSerializer = DescriptorVoxelSerializer; + using MapIo = io::LayeredMapIo; +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYERED_MAP_DEFINITION_H_ diff --git a/library/cpp/include/wavemap/layered/schema/layer_bundle_traits.h b/library/cpp/include/wavemap/layered/schema/layer_bundle_traits.h new file mode 100644 index 000000000..ed89a4c95 --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema/layer_bundle_traits.h @@ -0,0 +1,40 @@ +#ifndef WAVEMAP_LAYERED_LAYER_BUNDLE_TRAITS_H_ +#define WAVEMAP_LAYERED_LAYER_BUNDLE_TRAITS_H_ + +#include +#include +#include + +namespace wavemap::layered { +namespace detail { +template +inline constexpr bool kLayerBundleTraitsAlwaysFalse = false; + +template +constexpr void forEachLayerTagImpl(FunctionT&& function, + std::index_sequence) { + (function(std::tuple_element_t{}), ...); +} +} // namespace detail + +// Users specialize this trait and expose LayerTags as a tuple of layer tags. +// The metadata is storage-independent so it can later drive integration, +// schema validation, ROS conversion, and visualization. +template +struct LayerBundleTraits { + static_assert( + detail::kLayerBundleTraitsAlwaysFalse, + "LayerBundleTraits must be specialized for each bundle."); +}; + +template +constexpr void forEachLayerTag(FunctionT&& function) { + using LayerTags = typename LayerBundleTraits::LayerTags; + detail::forEachLayerTagImpl( + std::forward(function), + std::make_index_sequence>{}); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_BUNDLE_TRAITS_H_ diff --git a/library/cpp/include/wavemap/layered/schema/layer_descriptor.h b/library/cpp/include/wavemap/layered/schema/layer_descriptor.h new file mode 100644 index 000000000..77ebef5ae --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema/layer_descriptor.h @@ -0,0 +1,101 @@ +#ifndef WAVEMAP_LAYERED_LAYER_DESCRIPTOR_H_ +#define WAVEMAP_LAYERED_LAYER_DESCRIPTOR_H_ + +#include +#include +#include + +#include + +namespace wavemap::layered { +namespace detail { +template +inline constexpr bool kLayerDescriptorsAlwaysFalse = false; + +template +auto transformTupleImpl(TupleT&& tuple, FunctionT&& function, + std::index_sequence) { + return std::make_tuple( + function(std::get(std::forward(tuple)))...); +} +} // namespace detail + +template +struct LayerDescriptor { + using LayerTag = LayerTagT; + using Bundle = BundleT; + using Member = MemberT; + + MemberT BundleT::*member; + + static constexpr std::string_view name() { + return LayerTraits::name; + } + + MemberT& value(BundleT& bundle) const { return bundle.*member; } + const MemberT& value(const BundleT& bundle) const { return bundle.*member; } +}; + +template +constexpr auto layerDescriptor(MemberT BundleT::*member) { + return LayerDescriptor{member}; +} + +template +struct AccessorLayerDescriptor { + using LayerTag = LayerTagT; + using Bundle = BundleT; + using Member = LayerStateT; + + static constexpr std::string_view name() { + return LayerTraits::name; + } + + decltype(auto) value(BundleT& bundle) const { + return bundle.template get(); + } + decltype(auto) value(const BundleT& bundle) const { + return bundle.template get(); + } +}; + +template +constexpr auto accessorLayerDescriptor() { + return AccessorLayerDescriptor{}; +} + +// One specialization per bundle is the authoritative ordered layer list. +template +struct LayerDescriptors { + static_assert(detail::kLayerDescriptorsAlwaysFalse, + "LayerDescriptors must be specialized."); +}; + +template +constexpr auto layerDescriptors() { + return LayerDescriptors::descriptors(); +} + +template +auto transformLayerDescriptors(TupleT&& tuple, FunctionT&& function) { + constexpr size_t kTupleSize = + std::tuple_size_v>; + return detail::transformTupleImpl( + std::forward(tuple), std::forward(function), + std::make_index_sequence{}); +} + +template +void forEachLayerDescriptor(BundleT&& bundle, FunctionT&& function) { + const auto descriptors = + layerDescriptors>>(); + std::apply( + [&](const auto&... descriptor) { + (function(descriptor, descriptor.value(bundle)), ...); + }, + descriptors); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_DESCRIPTOR_H_ diff --git a/library/cpp/include/wavemap/layered/schema/layer_schema.h b/library/cpp/include/wavemap/layered/schema/layer_schema.h new file mode 100644 index 000000000..eb6956edc --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema/layer_schema.h @@ -0,0 +1,114 @@ +#ifndef WAVEMAP_LAYERED_LAYER_SCHEMA_H_ +#define WAVEMAP_LAYERED_LAYER_SCHEMA_H_ + +#include +#include +#include +#include +#include + +#include +#include + +namespace wavemap::layered::schema { + +template +struct ContinuousLayer { + static_assert(kIsContinuousLayerUpdatePolicy, + "A continuous layer policy must return Value and accept " + "(const Value&, const LayerObservation&)."); + using Value = ValueT; + using State = ValueT; + using UpdatePolicy = UpdatePolicyT; + static constexpr LayerStorageKind storage_kind = + LayerStorageKind::kWaveletCompatible; +}; + +template +struct StatefulContinuousLayer { + static_assert( + kIsStatefulContinuousLayerUpdatePolicy, + "A stateful continuous policy must return State and accept " + "(const State&, const LayerObservation&)."); + using Value = ValueT; + using State = StateT; + using UpdatePolicy = UpdatePolicyT; + static constexpr LayerStorageKind storage_kind = + LayerStorageKind::kWaveletCompatible; +}; + +template +struct DiscreteLayer { + static_assert(kIsDiscreteLayerUpdatePolicy, + "A discrete layer policy must return Value and accept either " + "Value or optional plus LayerObservation."); + using Value = ValueT; + using State = ValueT; + using UpdatePolicy = UpdatePolicyT; + static constexpr LayerStorageKind storage_kind = + LayerStorageKind::kNonWavelet; +}; + +namespace detail { +template +struct IsLayerTag : std::false_type {}; + +template +struct IsLayerTag< + LayerTagT, + std::void_t::Value, + typename LayerTraits::UpdatePolicy, + decltype(LayerTraits::name), + decltype(LayerTraits::storage_kind)>> + : std::true_type {}; + +template +constexpr bool namesAreUnique() { + constexpr std::array names = { + LayerTraits::name...}; + for (size_t lhs = 0u; lhs < sizeof...(LayerTags); ++lhs) { + if (names[lhs].empty()) { + return false; + } + for (size_t rhs = lhs + 1u; rhs < sizeof...(LayerTags); ++rhs) { + if (names[lhs] == names[rhs]) { + return false; + } + } + } + return true; +} +} // namespace detail + +template +struct LayerSchema { + static_assert((detail::IsLayerTag::value && ...), + "Every schema entry must be a valid layer tag."); + static_assert(detail::namesAreUnique(), + "Layer names must be non-empty and unique within a schema."); + + using Layers = std::tuple; + static constexpr size_t size = sizeof...(LayerTags); + + template + static constexpr bool contains = + (std::is_same_v || ...); + + template + static constexpr bool containsContinuous = + contains && kIsWaveletCompatibleLayer; + + template + static constexpr bool containsDiscrete = + contains && !kIsWaveletCompatibleLayer; + + static constexpr size_t continuousLayerCount = + (size_t{0u} + ... + + (kIsWaveletCompatibleLayer ? size_t{1u} : size_t{0u})); + static constexpr size_t discreteLayerCount = + size - continuousLayerCount; +}; + +} // namespace wavemap::layered::schema + +#endif // WAVEMAP_LAYERED_LAYER_SCHEMA_H_ diff --git a/library/cpp/include/wavemap/layered/schema/layer_traits.h b/library/cpp/include/wavemap/layered/schema/layer_traits.h new file mode 100644 index 000000000..f7a27c499 --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema/layer_traits.h @@ -0,0 +1,173 @@ +#ifndef WAVEMAP_LAYERED_LAYER_TRAITS_H_ +#define WAVEMAP_LAYERED_LAYER_TRAITS_H_ + +#include +#include +#include +#include +#include +#include + +#include + +namespace wavemap::layered { + +// Storage follows mathematical semantics, not the value C++ numeric type. +enum class LayerStorageKind { + kWaveletCompatible, + kNonWavelet, +}; + +struct VisualizationColor { + float r; + float g; + float b; +}; + +struct ScalarLayerVisualization { + VisualizationColor low; + VisualizationColor high; +}; + +struct IntegerCategoryLabel { + int value; + std::string_view label; + std::optional color = std::nullopt; + + constexpr IntegerCategoryLabel(int value_in, std::string_view label_in) + : value(value_in), label(label_in) {} + constexpr IntegerCategoryLabel(int value_in, std::string_view label_in, + VisualizationColor color_in) + : value(value_in), label(label_in), color(color_in) {} +}; + +struct IntegerCategoryMetadata { + int value; + std::string label; + std::optional color; +}; + +namespace detail { +template +inline constexpr bool kLayerTraitsAlwaysFalse = false; + +template +struct HasIntegerCategoryLabels : std::false_type {}; + +template +struct HasIntegerCategoryLabels< + LayerTagT, std::void_t> + : std::true_type {}; + +template +struct HasVisualization : std::false_type {}; + +template +struct HasVisualization> + : std::true_type {}; +} // namespace detail + +template +std::vector integerCategoryMetadata() { + std::vector result; + if constexpr (detail::HasIntegerCategoryLabels::value) { + result.reserve(LayerTagT::categories.size()); + for (const IntegerCategoryLabel& category : LayerTagT::categories) { + result.push_back( + {category.value, std::string(category.label), category.color}); + } + } + return result; +} + +template +std::optional scalarLayerVisualization() { + if constexpr (detail::HasVisualization::value) { + return LayerTagT::visualization; + } + return std::nullopt; +} + +// Layer tags can inherit from schema::ContinuousLayer or +// schema::DiscreteLayer, in which case this trait is populated automatically. +// Explicit specializations remain supported for backwards compatibility. +template +struct LayerTraits { + static_assert(detail::kLayerTraitsAlwaysFalse, + "LayerTraits must be specialized for each layer."); +}; + +template +struct LayerTraits< + LayerTagT, + std::void_t> { + using Value = typename LayerTagT::Value; + using UpdatePolicy = typename LayerTagT::UpdatePolicy; + static constexpr std::string_view name = LayerTagT::name; + static constexpr LayerStorageKind storage_kind = LayerTagT::storage_kind; +}; + +template +using LayerValueT = typename LayerTraits::Value; + +namespace detail { +template +struct LayerState { + using Type = LayerValueT; +}; + +template +struct LayerState> { + using Type = typename LayerTagT::State; +}; +} // namespace detail + +// Value is the public observation/query type. State is the representation +// actually stored in the map. They are identical for ordinary layers. +template +using LayerStateT = typename detail::LayerState::Type; + +template +using LayerUpdatePolicyT = typename LayerTraits::UpdatePolicy; + +template +struct LayerStateConversion { + static LayerValueT value(const LayerStateT& state) { + if constexpr (std::is_same_v, + WeightedMeanState>) { + static_assert(std::is_same_v, FloatingPoint>); + return state.valueOr(); + } else { + static_assert(std::is_same_v, + LayerStateT>, + "A stateful layer must specialize LayerStateConversion."); + return state; + } + } + + static LayerStateT initialState( + const LayerValueT& value) { + if constexpr (std::is_same_v, + WeightedMeanState>) { + static_assert(std::is_same_v, FloatingPoint>); + return {value, 1.f}; + } else { + static_assert(std::is_same_v, + LayerStateT>, + "A stateful layer must specialize LayerStateConversion."); + return value; + } + } +}; + +template +inline constexpr bool kIsWaveletCompatibleLayer = + LayerTraits::storage_kind == + LayerStorageKind::kWaveletCompatible; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_LAYER_TRAITS_H_ diff --git a/library/cpp/include/wavemap/layered/schema/schema_continuous_traits.h b/library/cpp/include/wavemap/layered/schema/schema_continuous_traits.h new file mode 100644 index 000000000..eb12f1ca4 --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema/schema_continuous_traits.h @@ -0,0 +1,310 @@ +#ifndef WAVEMAP_LAYERED_SCHEMA_CONTINUOUS_TRAITS_H_ +#define WAVEMAP_LAYERED_SCHEMA_CONTINUOUS_TRAITS_H_ + +#include +#include +#include + +#include +#include +#include + +namespace wavemap::layered::schema { + +template +struct ContinuousValueTraits; + +namespace detail { +template +struct HasContinuousValueTraits : std::false_type {}; + +template +struct HasContinuousValueTraits< + ValueT, + std::void_t< + typename ContinuousValueTraits::ThresholdConfig, + decltype(ContinuousValueTraits::add( + std::declval(), std::declval())), + decltype(ContinuousValueTraits::subtract( + std::declval(), std::declval())), + decltype(ContinuousValueTraits::scale( + std::declval(), std::declval())), + decltype(ContinuousValueTraits::threshold( + std::declval(), + std::declval::ThresholdConfig&>())), + decltype(ContinuousValueTraits::magnitude( + std::declval()))>> : std::true_type {}; +} // namespace detail + +template +inline constexpr bool kHasContinuousValueTraits = + detail::HasContinuousValueTraits::value; + +template <> +struct ContinuousValueTraits { + struct ThresholdConfig { + FloatingPoint min = std::numeric_limits::lowest(); + FloatingPoint max = std::numeric_limits::max(); + }; + + static FloatingPoint add(FloatingPoint lhs, FloatingPoint rhs) { + return lhs + rhs; + } + static FloatingPoint subtract(FloatingPoint lhs, FloatingPoint rhs) { + return lhs - rhs; + } + static FloatingPoint scale(FloatingPoint value, FloatingPoint factor) { + return factor * value; + } + static void threshold(FloatingPoint& value, const ThresholdConfig& config) { + value = std::clamp(value, config.min, config.max); + } + static FloatingPoint magnitude(FloatingPoint value) { + return std::abs(value); + } +}; + +template <> +struct ContinuousValueTraits { + struct ThresholdConfig { + Rgb min{std::numeric_limits::lowest(), + std::numeric_limits::lowest(), + std::numeric_limits::lowest()}; + Rgb max{std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + }; + + static Rgb add(const Rgb& lhs, const Rgb& rhs) { + return RgbArithmetic::add(lhs, rhs); + } + static Rgb subtract(const Rgb& lhs, const Rgb& rhs) { + return RgbArithmetic::subtract(lhs, rhs); + } + static Rgb scale(const Rgb& value, FloatingPoint factor) { + return RgbArithmetic::scale(value, factor); + } + static void threshold(Rgb& value, const ThresholdConfig& config) { + value = clampRgb(value, config.min, config.max); + } + static FloatingPoint magnitude(const Rgb& value) { return maxAbs(value); } +}; + +template <> +struct ContinuousValueTraits { + struct ThresholdConfig { + FloatingPoint min = std::numeric_limits::lowest(); + FloatingPoint max = std::numeric_limits::max(); + FloatingPoint max_total_weight = + std::numeric_limits::max(); + }; + + static WeightedMeanState add(const WeightedMeanState& lhs, + const WeightedMeanState& rhs) { + return {lhs.weighted_sum + rhs.weighted_sum, + lhs.total_weight + rhs.total_weight}; + } + static WeightedMeanState subtract(const WeightedMeanState& lhs, + const WeightedMeanState& rhs) { + return {lhs.weighted_sum - rhs.weighted_sum, + lhs.total_weight - rhs.total_weight}; + } + static WeightedMeanState scale(const WeightedMeanState& value, + FloatingPoint factor) { + return {factor * value.weighted_sum, factor * value.total_weight}; + } + static void threshold(WeightedMeanState& state, + const ThresholdConfig& config) { + if (state.total_weight <= 0.f) { + state = {}; + return; + } + if (config.max_total_weight < state.total_weight) { + const FloatingPoint factor = + config.max_total_weight / state.total_weight; + state = scale(state, factor); + } + const FloatingPoint bounded_value = + std::clamp(state.valueOr(), config.min, config.max); + state.weighted_sum = bounded_value * state.total_weight; + } + static FloatingPoint magnitude(const WeightedMeanState& state) { + return std::max(std::abs(state.weighted_sum), + std::abs(state.total_weight)); + } +}; + +namespace detail { +template +using ThresholdConfigFor = + typename ContinuousValueTraits>::ThresholdConfig; + +template +using PruningConfigFor = wavemap::FieldPruningConfig; + +template class ConfigT, bool Enabled> +struct ContinuousConfigSlot {}; + +template class ConfigT> +struct ContinuousConfigSlot { + ConfigT value{}; +}; +} // namespace detail + +template class ConfigT> +class ContinuousConfigBundle; + +template class ConfigT> +class ContinuousConfigBundle, ConfigT> + : private detail::ContinuousConfigSlot< + LayerTags, ConfigT, kIsWaveletCompatibleLayer>... { + public: + using Schema = LayerSchema; + + template + ConfigT& get() { + static_assert(Schema::template containsContinuous); + return static_cast< + detail::ContinuousConfigSlot&>(*this) + .value; + } + + template + const ConfigT& get() const { + static_assert(Schema::template containsContinuous); + return static_cast&>(*this) + .value; + } +}; + +template +using ContinuousThresholdConfig = + ContinuousConfigBundle; + +template +struct ContinuousPruningConfig + : ContinuousConfigBundle { + FloatingPoint combined_threshold = 1.f; +}; + +template +struct ContinuousBundleArithmetic; + +template +struct ContinuousBundleArithmetic> { + using Schema = LayerSchema; + using Bundle = ContinuousLayerBundle; + + static Bundle add(const Bundle& lhs, const Bundle& rhs) { + Bundle result; + (applyAdd(result, lhs, rhs), ...); + return result; + } + + static Bundle subtract(const Bundle& lhs, const Bundle& rhs) { + Bundle result; + (applySubtract(result, lhs, rhs), ...); + return result; + } + + static Bundle scale(const Bundle& value, FloatingPoint factor) { + Bundle result; + if constexpr (sizeof...(LayerTags) == 0u) { + (void)value; + (void)factor; + } + (applyScale(result, value, factor), ...); + return result; + } + + private: + template + static void applyAdd(Bundle& result, const Bundle& lhs, const Bundle& rhs) { + if constexpr (kIsWaveletCompatibleLayer) { + result.template get() = + ContinuousValueTraits>::add( + lhs.template get(), rhs.template get()); + } + } + + template + static void applySubtract(Bundle& result, const Bundle& lhs, + const Bundle& rhs) { + if constexpr (kIsWaveletCompatibleLayer) { + result.template get() = + ContinuousValueTraits>::subtract( + lhs.template get(), rhs.template get()); + } + } + + template + static void applyScale(Bundle& result, const Bundle& value, + FloatingPoint factor) { + if constexpr (kIsWaveletCompatibleLayer) { + result.template get() = + ContinuousValueTraits>::scale( + value.template get(), factor); + } else { + (void)result; + (void)value; + (void)factor; + } + } +}; + +} // namespace wavemap::layered::schema + +namespace wavemap { + +template +struct CellDataTraits>> { + using Schema = layered::schema::LayerSchema; + using Bundle = layered::schema::ContinuousLayerBundle; + using ThresholdConfig = layered::schema::ContinuousThresholdConfig; + using PruningConfig = layered::schema::ContinuousPruningConfig; + + static void threshold(Bundle& bundle, const ThresholdConfig& config) { + (thresholdOne(bundle, config), ...); + } + + static FloatingPoint pruningScore(const Bundle& bundle, + const PruningConfig& config) { + return (FloatingPoint{0.f} + ... + scoreOne(bundle, config)); + } + + static bool isNonzero(const Bundle& bundle, const PruningConfig& config) { + return config.combined_threshold < pruningScore(bundle, config); + } + + private: + template + static void thresholdOne(Bundle& bundle, const ThresholdConfig& config) { + if constexpr (layered::kIsWaveletCompatibleLayer) { + layered::schema::ContinuousValueTraits< + layered::LayerStateT>::threshold( + bundle.template get(), + config.template get()); + } + } + + template + static FloatingPoint scoreOne(const Bundle& bundle, + const PruningConfig& config) { + if constexpr (layered::kIsWaveletCompatibleLayer) { + return weightedPruningScore( + layered::schema::ContinuousValueTraits< + layered::LayerStateT>::magnitude( + bundle.template get()), + config.template get()); + } else { + return 0.f; + } + } +}; + +} // namespace wavemap + +#endif // WAVEMAP_LAYERED_SCHEMA_CONTINUOUS_TRAITS_H_ diff --git a/library/cpp/include/wavemap/layered/schema/schema_layer_storage.h b/library/cpp/include/wavemap/layered/schema/schema_layer_storage.h new file mode 100644 index 000000000..9bea2f32f --- /dev/null +++ b/library/cpp/include/wavemap/layered/schema/schema_layer_storage.h @@ -0,0 +1,166 @@ +#ifndef WAVEMAP_LAYERED_SCHEMA_LAYER_STORAGE_H_ +#define WAVEMAP_LAYERED_SCHEMA_LAYER_STORAGE_H_ + +#include +#include +#include + +#include +#include +#include + +namespace wavemap::layered::schema { +namespace detail { + +template +struct ContinuousSlot { + bool operator==(const ContinuousSlot&) const { return true; } +}; + +template +struct ContinuousSlot { + LayerStateT value{}; + + bool operator==(const ContinuousSlot& other) const { + return value == other.value; + } +}; + +template +struct DiscreteSlot { + explicit DiscreteSlot(const DiscreteCompressionConfig&) {} +}; + +template +struct DiscreteSlot { + explicit DiscreteSlot(const DiscreteCompressionConfig& config) + : value(config) {} + + layered::DiscreteLayer> value; +}; + +template +constexpr auto continuousDescriptorTuple() { + if constexpr (kIsWaveletCompatibleLayer) { + return std::make_tuple( + accessorLayerDescriptor()); + } else { + return std::tuple<>(); + } +} + +template +constexpr auto discreteDescriptorTuple() { + if constexpr (kIsWaveletCompatibleLayer) { + return std::tuple<>(); + } else { + return std::make_tuple( + accessorLayerDescriptor()); + } +} + +} // namespace detail + +template +class ContinuousLayerBundle; + +template +class ContinuousLayerBundle> + : private detail::ContinuousSlot< + LayerTags, kIsWaveletCompatibleLayer>... { + public: + using Schema = LayerSchema; + + template + LayerStateT& get() { + static_assert(Schema::template containsContinuous, + "Requested layer is not a continuous layer in this schema."); + return static_cast&>(*this).value; + } + + template + const LayerStateT& get() const { + static_assert(Schema::template containsContinuous, + "Requested layer is not a continuous layer in this schema."); + return static_cast&>(*this) + .value; + } + + bool operator==(const ContinuousLayerBundle& other) const { + return ((! + kIsWaveletCompatibleLayer || + getIfContinuous() == + other.template getIfContinuous()) && + ...); + } + + private: + template + const auto& getIfContinuous() const { + return static_cast>&>(*this); + } +}; + +template +class DiscreteLayerBundle; + +template +class DiscreteLayerBundle> + : private detail::DiscreteSlot< + LayerTags, !kIsWaveletCompatibleLayer>... { + public: + using Schema = LayerSchema; + + explicit DiscreteLayerBundle( + const DiscreteCompressionConfig& config = DiscreteCompressionConfig()) + : detail::DiscreteSlot< + LayerTags, !kIsWaveletCompatibleLayer>(config)... {} + + template + layered::DiscreteLayer>& get() { + static_assert(Schema::template containsDiscrete, + "Requested layer is not a discrete layer in this schema."); + return static_cast&>(*this).value; + } + + template + const layered::DiscreteLayer>& get() const { + static_assert(Schema::template containsDiscrete, + "Requested layer is not a discrete layer in this schema."); + return static_cast&>(*this) + .value; + } +}; + +} // namespace wavemap::layered::schema + +namespace wavemap::layered { + +template +struct LayerDescriptors< + schema::ContinuousLayerBundle>> { + using Bundle = + schema::ContinuousLayerBundle>; + + static constexpr auto descriptors() { + return std::tuple_cat( + schema::detail::continuousDescriptorTuple()...); + } +}; + +template +struct LayerDescriptors< + schema::DiscreteLayerBundle>> { + using Bundle = + schema::DiscreteLayerBundle>; + + static constexpr auto descriptors() { + return std::tuple_cat( + schema::detail::discreteDescriptorTuple()...); + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_SCHEMA_LAYER_STORAGE_H_ diff --git a/library/cpp/include/wavemap/layered/types/rgb.h b/library/cpp/include/wavemap/layered/types/rgb.h new file mode 100644 index 000000000..12813f7ff --- /dev/null +++ b/library/cpp/include/wavemap/layered/types/rgb.h @@ -0,0 +1,47 @@ +#ifndef WAVEMAP_LAYERED_TYPES_RGB_H_ +#define WAVEMAP_LAYERED_TYPES_RGB_H_ + +#include +#include + +#include + +namespace wavemap::layered { + +struct Rgb { + float r = 0.f; + float g = 0.f; + float b = 0.f; + + bool operator==(const Rgb& other) const { + return r == other.r && g == other.g && b == other.b; + } +}; + +struct RgbArithmetic { + static Rgb add(const Rgb& lhs, const Rgb& rhs) { + return {lhs.r + rhs.r, lhs.g + rhs.g, lhs.b + rhs.b}; + } + + static Rgb subtract(const Rgb& lhs, const Rgb& rhs) { + return {lhs.r - rhs.r, lhs.g - rhs.g, lhs.b - rhs.b}; + } + + static Rgb scale(const Rgb& color, FloatingPoint factor) { + return {factor * color.r, factor * color.g, factor * color.b}; + } +}; + +inline Rgb clampRgb(const Rgb& color, const Rgb& min, const Rgb& max) { + return {std::clamp(color.r, min.r, max.r), + std::clamp(color.g, min.g, max.g), + std::clamp(color.b, min.b, max.b)}; +} + +inline FloatingPoint maxAbs(const Rgb& color) { + return std::max({std::abs(color.r), std::abs(color.g), std::abs(color.b)}); +} + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_TYPES_RGB_H_ diff --git a/library/cpp/include/wavemap/layered/types/weighted_mean_state.h b/library/cpp/include/wavemap/layered/types/weighted_mean_state.h new file mode 100644 index 000000000..95d5db8ea --- /dev/null +++ b/library/cpp/include/wavemap/layered/types/weighted_mean_state.h @@ -0,0 +1,26 @@ +#ifndef WAVEMAP_LAYERED_TYPES_WEIGHTED_MEAN_STATE_H_ +#define WAVEMAP_LAYERED_TYPES_WEIGHTED_MEAN_STATE_H_ + +#include + +namespace wavemap::layered { + +// Sufficient statistics for a true scalar running weighted mean. Zero weight +// explicitly represents an unobserved voxel and is not a measurement of zero. +struct WeightedMeanState { + FloatingPoint weighted_sum = 0.f; + FloatingPoint total_weight = 0.f; + + FloatingPoint valueOr(FloatingPoint fallback = 0.f) const { + return 0.f < total_weight ? weighted_sum / total_weight : fallback; + } + + bool operator==(const WeightedMeanState& other) const { + return weighted_sum == other.weighted_sum && + total_weight == other.total_weight; + } +}; + +} // namespace wavemap::layered + +#endif // WAVEMAP_LAYERED_TYPES_WEIGHTED_MEAN_STATE_H_ diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc index cc9709acc..293b185f0 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc @@ -1,163 +1 @@ #include "wavemap/core/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h" - -#include -#include -#include -#include - -#include - -namespace wavemap { -void HashedChunkedWaveletIntegrator::updateMap() { - ProfilerZoneScoped; - // Update the range image intersector - { - ProfilerZoneScopedN("updateRangeImageIntersector"); - range_image_intersector_ = std::make_shared( - posed_range_image_, projection_model_, *measurement_model_, - config_.min_range, config_.max_range); - } - - // Find all the indices of blocks that need updating - BlockList blocks_to_update; - { - ProfilerZoneScopedN("selectBlocksToUpdate"); - const auto [fov_min_idx, fov_max_idx] = - getFovMinMaxIndices(posed_range_image_->getOrigin()); - for (const auto& block_index : - Grid(fov_min_idx.position, fov_max_idx.position)) { - recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, - blocks_to_update); - } - } - - // Make sure the to-be-updated blocks are allocated - for (const auto& block_index : blocks_to_update) { - occupancy_map_->getOrAllocateBlock(block_index); - } - - // Update it with the threadpool - for (const auto& block_index : blocks_to_update) { - thread_pool_->add_task([this, block_index]() { - if (auto* block = occupancy_map_->getBlock(block_index); block) { - updateBlock(*block, block_index); - } - }); - } - thread_pool_->wait_all(); -} - -std::pair -HashedChunkedWaveletIntegrator::getFovMinMaxIndices( - const Point3D& sensor_origin) const { - const int height = 1 + std::max(static_cast(std::ceil(std::log2( - config_.max_range / min_cell_width_))), - tree_height_); - const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToFloorIndex<3>( - sensor_origin - Vector3D::Constant(config_.max_range), - min_cell_width_inv_) - - occupancy_map_->getBlockSize(), - height); - const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToCeilIndex<3>( - sensor_origin + Vector3D::Constant(config_.max_range), - min_cell_width_inv_) + - occupancy_map_->getBlockSize(), - height); - return {fov_min_idx, fov_max_idx}; -} - -void HashedChunkedWaveletIntegrator::updateBlock( - HashedChunkedWaveletOctree::Block& block, - const HashedChunkedWaveletOctree::BlockIndex& block_index) { - ProfilerZoneScoped; - block.setNeedsPruning(); - block.setLastUpdatedStamp(); - - bool block_needs_thresholding = block.getNeedsThresholding(); - const OctreeIndex root_node_index{tree_height_, block_index}; - updateNodeRecursive(block.getRootNode(), root_node_index, - block.getRootScale(), block_needs_thresholding); - block.setNeedsThresholding(block_needs_thresholding); -} - -void HashedChunkedWaveletIntegrator::updateNodeRecursive( // NOLINT - HashedChunkedWaveletIntegrator::OctreeType::NodeRefType node, - const OctreeIndex& node_index, FloatingPoint& node_value, - bool& block_needs_thresholding) { - // Decompress child values - auto& node_details = node.data(); - auto child_values = HashedChunkedWaveletOctreeBlock::Transform::backward( - {node_value, node_details}); - - // Handle each child - for (NdtreeIndexRelativeChild relative_child_idx = 0; - relative_child_idx < OctreeIndex::kNumChildren; ++relative_child_idx) { - const OctreeIndex child_index = - node_index.computeChildIndex(relative_child_idx); - FloatingPoint& child_value = child_values[relative_child_idx]; - - // Test whether it is fully occupied; free or unknown; or fully unknown - const AABB W_child_aabb = - convert::nodeIndexToAABB(child_index, min_cell_width_); - const UpdateType update_type = - range_image_intersector_->determineUpdateType( - W_child_aabb, posed_range_image_->getRotationMatrixInverse(), - posed_range_image_->getOrigin()); - - // If we're fully in unknown space, - // there's no need to evaluate this node or its children - if (update_type == UpdateType::kFullyUnobserved) { - continue; - } - - // We can also stop here if the cell will result in a free space update - // (or zero) and the map is already saturated free - if (update_type != UpdateType::kPossiblyOccupied && - child_value < min_log_odds_shrunk_) { - continue; - } - - // Test if the worst-case error for the intersection type at the current - // resolution falls within the acceptable approximation error - const FloatingPoint child_width = W_child_aabb.width<0>(); - const Point3D W_child_center = - W_child_aabb.min + Vector3D::Constant(child_width / 2.f); - const Point3D C_child_center = - posed_range_image_->getPoseInverse() * W_child_center; - const FloatingPoint d_C_child = - projection_model_->cartesianToSensorZ(C_child_center); - const FloatingPoint bounding_sphere_radius = - kUnitCubeHalfDiagonal * child_width; - if (measurement_model_->computeWorstCaseApproximationError( - update_type, d_C_child, bounding_sphere_radius) < - config_.termination_update_error) { - const FloatingPoint sample = computeUpdate(C_child_center); - child_value += sample; - block_needs_thresholding = true; - continue; - } - - // Since the approximation error would still be too big, refine - auto child_node = node.getOrAllocateChild(relative_child_idx); - auto& child_details = child_node.data(); - - // If we're at the leaf level, directly compute the update - if (child_index.height <= termination_height_ + 1) { - updateLeavesBatch(child_index, child_value, child_details); - } else { - // Otherwise, recurse - DCHECK_GE(child_index.height, 0); - updateNodeRecursive(child_node, child_index, child_value, - block_needs_thresholding); - } - } - - // Compress - const auto [new_value, new_details] = - HashedChunkedWaveletOctreeBlock::Transform::forward(child_values); - node_details = new_details; - node_value = new_value; -} -} // namespace wavemap diff --git a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc index 7751b8ede..ff2c6bcb9 100644 --- a/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc +++ b/library/cpp/src/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc @@ -1,203 +1 @@ #include "wavemap/core/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h" - -#include -#include -#include -#include - -#include - -namespace wavemap { -void HashedWaveletIntegrator::updateMap() { - ProfilerZoneScoped; - // Update the range image intersector - { - ProfilerZoneScopedN("updateRangeImageIntersector"); - range_image_intersector_ = std::make_shared( - posed_range_image_, projection_model_, *measurement_model_, - config_.min_range, config_.max_range); - } - - // Find all the indices of blocks that need updating - BlockList blocks_to_update; - { - ProfilerZoneScopedN("selectBlocksToUpdate"); - const auto [fov_min_idx, fov_max_idx] = - getFovMinMaxIndices(posed_range_image_->getOrigin()); - for (const auto& block_index : - Grid(fov_min_idx.position, fov_max_idx.position)) { - recursiveTester(OctreeIndex{fov_min_idx.height, block_index}, - blocks_to_update); - } - } - - // Make sure the to-be-updated blocks are allocated - for (const auto& block_index : blocks_to_update) { - occupancy_map_->getOrAllocateBlock(block_index); - } - - // Update it with the threadpool - for (const auto& block_index : blocks_to_update) { - thread_pool_->add_task([this, block_index]() { - if (auto* block = occupancy_map_->getBlock(block_index); block) { - updateBlock(*block, block_index); - } - }); - } - thread_pool_->wait_all(); -} - -std::pair -HashedWaveletIntegrator::getFovMinMaxIndices( - const Point3D& sensor_origin) const { - const int height = 1 + std::max(static_cast(std::ceil(std::log2( - config_.max_range / min_cell_width_))), - tree_height_); - const OctreeIndex fov_min_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToFloorIndex<3>( - sensor_origin - Vector3D::Constant(config_.max_range), - min_cell_width_inv_) - - occupancy_map_->getBlockSize(), - height); - const OctreeIndex fov_max_idx = convert::indexAndHeightToNodeIndex<3>( - convert::pointToCeilIndex<3>( - sensor_origin + Vector3D::Constant(config_.max_range), - min_cell_width_inv_) + - occupancy_map_->getBlockSize(), - height); - return {fov_min_idx, fov_max_idx}; -} - -void HashedWaveletIntegrator::updateBlock( - HashedWaveletOctree::Block& block, - const HashedWaveletOctree::BlockIndex& block_index) { - ProfilerZoneScoped; - block.setNeedsPruning(); - block.setLastUpdatedStamp(); - - struct StackElement { - OctreeType::NodeRefType parent_node; - const OctreeIndex parent_node_index; - NdtreeIndexRelativeChild next_child_idx; - HashedWaveletOctreeBlock::Coefficients::CoefficientsArray - child_scale_coefficients; - }; - std::stack stack; - - OctreeType::NodeRefType root_node = block.getRootNode(); - HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale = - block.getRootScale(); - stack.emplace(StackElement{root_node, - {tree_height_, block_index}, - 0, - HashedWaveletOctreeBlock::Transform::backward( - {root_node_scale, root_node.data()})}); - - while (!stack.empty()) { - // If the current stack element has fully been processed, propagate upward - if (OctreeIndex::kNumChildren <= stack.top().next_child_idx) { - const auto [scale, details] = - HashedWaveletOctreeBlock::Transform::forward( - stack.top().child_scale_coefficients); - stack.top().parent_node.data() = details; - stack.pop(); - if (stack.empty()) { - root_node_scale = scale; - return; - } else { - const NdtreeIndexRelativeChild current_child_idx = - stack.top().next_child_idx - 1; - stack.top().child_scale_coefficients[current_child_idx] = scale; - continue; - } - } - - // Evaluate stack element's active child - const NdtreeIndexRelativeChild current_child_idx = - stack.top().next_child_idx; - ++stack.top().next_child_idx; - DCHECK_GE(current_child_idx, 0); - DCHECK_LT(current_child_idx, OctreeIndex::kNumChildren); - - OctreeType::NodeRefType parent_node = stack.top().parent_node; - FloatingPoint& node_value = - stack.top().child_scale_coefficients[current_child_idx]; - const OctreeIndex node_index = - stack.top().parent_node_index.computeChildIndex(current_child_idx); - DCHECK_GE(node_index.height, 0); - - // If we're at the leaf level, directly update the node - if (node_index.height <= termination_height_) { - const Point3D W_node_center = - convert::nodeIndexToCenterPoint(node_index, min_cell_width_); - const Point3D C_node_center = - posed_range_image_->getPoseInverse() * W_node_center; - const FloatingPoint sample = computeUpdate(C_node_center); - node_value = - std::clamp(sample + node_value, min_log_odds_ - kNoiseThreshold, - max_log_odds_ + kNoiseThreshold); - continue; - } - - // Otherwise, test whether the current node is fully occupied; - // free or unknown; or fully unknown - const AABB W_cell_aabb = - convert::nodeIndexToAABB(node_index, min_cell_width_); - const UpdateType update_type = - range_image_intersector_->determineUpdateType( - W_cell_aabb, posed_range_image_->getRotationMatrixInverse(), - posed_range_image_->getOrigin()); - - // If we're fully in unknown space, - // there's no need to evaluate this node or its children - if (update_type == UpdateType::kFullyUnobserved) { - continue; - } - - // We can also stop here if the cell will result in a free space update - // (or zero) and the map is already saturated free - if (update_type != UpdateType::kPossiblyOccupied && - node_value < min_log_odds_ + kNoiseThreshold / 10.f) { - continue; - } - - // Test if the worst-case error for the intersection type at the current - // resolution falls within the acceptable approximation error - const FloatingPoint node_width = W_cell_aabb.width<0>(); - const Point3D W_node_center = - W_cell_aabb.min + Vector3D::Constant(node_width / 2.f); - const Point3D C_node_center = - posed_range_image_->getPoseInverse() * W_node_center; - const FloatingPoint d_C_cell = - projection_model_->cartesianToSensorZ(C_node_center); - const FloatingPoint bounding_sphere_radius = - kUnitCubeHalfDiagonal * node_width; - OctreeType::NodePtrType node = - parent_node.getChild(node_index.computeRelativeChildIndex()); - if (measurement_model_->computeWorstCaseApproximationError( - update_type, d_C_cell, bounding_sphere_radius) < - config_.termination_update_error) { - const FloatingPoint sample = computeUpdate(C_node_center); - if (!node || !node->hasAtLeastOneChild()) { - node_value = - std::clamp(sample + node_value, min_log_odds_ - kNoiseThreshold, - max_log_odds_ + kNoiseThreshold); - } else { - node_value += sample; - block.setNeedsThresholding(); - } - continue; - } - - // Since the approximation error would still be too big, refine - if (!node) { - // Allocate the current node if it has not yet been allocated - node = &parent_node.getOrAllocateChild( - node_index.computeRelativeChildIndex()); - } - stack.emplace(StackElement{*node, node_index, 0, - HashedWaveletOctreeBlock::Transform::backward( - {node_value, node->data()})}); - } -} -} // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc index 702a0eba6..4992dabbf 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree.cc @@ -23,61 +23,4 @@ bool HashedChunkedWaveletOctreeConfig::isValid(bool verbose) const { return is_valid; } -void HashedChunkedWaveletOctree::threshold() { - ProfilerZoneScoped; - forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { - block.threshold(); - }); -} - -void HashedChunkedWaveletOctree::prune() { - ProfilerZoneScoped; - block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { - block.prune(); - return block.empty(); - }); -} - -void HashedChunkedWaveletOctree::pruneSmart() { - ProfilerZoneScoped; - block_map_.eraseBlockIf( - [&config = config_](const BlockIndex& /*block_index*/, Block& block) { - if (config.only_prune_blocks_if_unused_for < - block.getTimeSinceLastUpdated()) { - block.prune(); - } - return block.empty(); - }); -} - -size_t HashedChunkedWaveletOctree::getMemoryUsage() const { - ProfilerZoneScoped; - // TODO(victorr): Also include the memory usage of the unordered map itself - size_t memory_usage = 0u; - forEachBlock( - [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { - memory_usage += block.getMemoryUsage(); - }); - return memory_usage; -} - -Index3D HashedChunkedWaveletOctree::getMinIndex() const { - return cells_per_block_side_ * getMinBlockIndex(); -} - -Index3D HashedChunkedWaveletOctree::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; -} - -void HashedChunkedWaveletOctree::forEachLeaf( - MapBase::IndexedLeafVisitorFunction visitor_fn) const { - forEachBlock( - [&visitor_fn](const BlockIndex& block_index, const Block& block) { - block.forEachLeaf(block_index, visitor_fn); - }); -} - } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc index b5600653a..d1d468160 100644 --- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc @@ -1,194 +1 @@ #include "wavemap/core/map/hashed_chunked_wavelet_octree_block.h" - -#include -#include -#include - -#include - -namespace wavemap { -void HashedChunkedWaveletOctreeBlock::threshold() { - ProfilerZoneScoped; - if (getNeedsThresholding()) { - recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); - setNeedsThresholding(false); - } -} - -void HashedChunkedWaveletOctreeBlock::prune() { - ProfilerZoneScoped; - if (getNeedsPruning()) { - threshold(); - recursivePrune(ndtree_.getRootNode()); - setNeedsPruning(false); - } -} - -void HashedChunkedWaveletOctreeBlock::clear() { - ProfilerZoneScoped; - root_scale_coefficient_ = Coefficients::Scale{}; - ndtree_.clear(); - setLastUpdatedStamp(); -} - -void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, - FloatingPoint new_value) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(ndtree_.getRootNode()); - FloatingPoint current_value = root_scale_coefficient_; - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodeRefType current_parent = ancestors.back(); - current_value = Transform::backwardSingleChild( - {current_value, current_parent.data()}, child_index); - OctreeType::NodeRefType child = - current_parent.getOrAllocateChild(child_index); - ancestors.emplace_back(child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{new_value - current_value, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodeRefType current_node = ancestors.back(); - ancestors.pop_back(); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node.data() += coefficients.details; - } - - root_scale_coefficient_ += coefficients.scale; -} - -void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, - FloatingPoint update) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(ndtree_.getRootNode()); - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodeRefType current_parent = ancestors.back(); - OctreeType::NodeRefType child = - current_parent.getOrAllocateChild(child_index); - ancestors.emplace_back(child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{update, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - OctreeType::NodeRefType current_node = ancestors.back(); - ancestors.pop_back(); - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node.data() += coefficients.details; - } - root_scale_coefficient_ += coefficients.scale; -} - -void HashedChunkedWaveletOctreeBlock::forEachLeaf( - const BlockIndex& block_index, - MapBase::IndexedLeafVisitorFunction visitor_fn, - IndexElement termination_height) const { - ProfilerZoneScoped; - if (empty()) { - return; - } - - struct StackElement { - const OctreeIndex node_index; - OctreeType::NodeConstRefType node; - const Coefficients::Scale scale_coefficient{}; - }; - std::stack stack; - stack.emplace(StackElement{{tree_height_, block_index}, - ndtree_.getRootNode(), - root_scale_coefficient_}); - while (!stack.empty()) { - const OctreeIndex index = stack.top().node_index; - const auto node = stack.top().node; - const FloatingPoint scale_coefficient = stack.top().scale_coefficient; - stack.pop(); - - const Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({scale_coefficient, {node.data()}}); - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const OctreeIndex child_node_index = index.computeChildIndex(child_idx); - const FloatingPoint child_scale_coefficient = - child_scale_coefficients[child_idx]; - if (auto child_node = node.getChild(child_idx); - child_node && termination_height < child_node_index.height) { - stack.emplace(StackElement{child_node_index, *child_node, - child_scale_coefficient}); - } else { - visitor_fn(child_node_index, child_scale_coefficient); - } - } - } -} - -void HashedChunkedWaveletOctreeBlock::recursiveThreshold( // NOLINT - OctreeType::NodeRefType node, Coefficients::Scale& node_scale_coefficient) { - // Decompress child values - auto& node_detail_coefficients = node.data(); - Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({node_scale_coefficient, node_detail_coefficients}); - - // Handle each child - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; - if (auto child_node = node.getChild(child_idx); child_node) { - recursiveThreshold(*child_node, child_scale); - } else { - child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); - } - } - - // Compress - const auto [new_scale, new_details] = - Transform::forward(child_scale_coefficients); - node_detail_coefficients = new_details; - node_scale_coefficient = new_scale; -} - -void HashedChunkedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedChunkedWaveletOctreeBlock::OctreeType::NodeRefType node) { - bool has_at_least_one_child = false; - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (OctreeType::NodePtrType child_node = node.getChild(child_idx); - child_node) { - recursivePrune(*child_node); - if (!child_node->hasAtLeastOneChild() && - !child_node->hasNonzeroData(1e-3f)) { - node.eraseChild(child_idx); - } else { - has_at_least_one_child = true; - } - } - } - node.hasAtLeastOneChild() = has_at_least_one_child; -} -} // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_wavelet_octree.cc b/library/cpp/src/core/map/hashed_wavelet_octree.cc index 91c468527..92139d8d0 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree.cc @@ -1,9 +1,5 @@ #include "wavemap/core/map/hashed_wavelet_octree.h" -#include - -#include - namespace wavemap { DECLARE_CONFIG_MEMBERS(HashedWaveletOctreeConfig, (min_cell_width) @@ -21,62 +17,4 @@ bool HashedWaveletOctreeConfig::isValid(bool verbose) const { return is_valid; } - -void HashedWaveletOctree::threshold() { - ProfilerZoneScoped; - forEachBlock([](const BlockIndex& /*block_index*/, Block& block) { - block.threshold(); - }); -} - -void HashedWaveletOctree::prune() { - ProfilerZoneScoped; - block_map_.eraseBlockIf([](const BlockIndex& /*block_index*/, Block& block) { - block.prune(); - return block.empty(); - }); -} - -void HashedWaveletOctree::pruneSmart() { - ProfilerZoneScoped; - block_map_.eraseBlockIf( - [&config = config_](const BlockIndex& /*block_index*/, Block& block) { - if (config.only_prune_blocks_if_unused_for < - block.getTimeSinceLastUpdated()) { - block.prune(); - } - return block.empty(); - }); -} - -size_t HashedWaveletOctree::getMemoryUsage() const { - ProfilerZoneScoped; - // TODO(victorr): Also include the memory usage of the unordered map itself - size_t memory_usage = 0u; - forEachBlock( - [&memory_usage](const BlockIndex& /*block_index*/, const Block& block) { - memory_usage += block.getMemoryUsage(); - }); - return memory_usage; -} - -Index3D HashedWaveletOctree::getMinIndex() const { - return cells_per_block_side_ * getMinBlockIndex(); -} - -Index3D HashedWaveletOctree::getMaxIndex() const { - if (empty()) { - return Index3D::Zero(); - } - return cells_per_block_side_ * (getMaxBlockIndex().array() + 1) - 1; -} - -void HashedWaveletOctree::forEachLeaf( - MapBase::IndexedLeafVisitorFunction visitor_fn) const { - forEachBlock( - [&visitor_fn](const BlockIndex& block_index, const Block& block) { - block.forEachLeaf(block_index, visitor_fn); - }); -} - } // namespace wavemap diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc index a0633ab36..0d827cad4 100644 --- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc +++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc @@ -1,196 +1 @@ #include "wavemap/core/map/hashed_wavelet_octree_block.h" - -#include -#include - -#include - -namespace wavemap { -void HashedWaveletOctreeBlock::threshold() { - ProfilerZoneScoped; - if (getNeedsThresholding()) { - recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_); - setNeedsThresholding(false); - } -} - -void HashedWaveletOctreeBlock::prune() { - ProfilerZoneScoped; - if (getNeedsPruning()) { - threshold(); - recursivePrune(ndtree_.getRootNode()); - setNeedsPruning(false); - } -} - -void HashedWaveletOctreeBlock::clear() { - ProfilerZoneScoped; - root_scale_coefficient_ = Coefficients::Scale{}; - ndtree_.clear(); - setLastUpdatedStamp(); -} - -void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index, - FloatingPoint new_value) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(&ndtree_.getRootNode()); - FloatingPoint current_value = root_scale_coefficient_; - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodePtrType current_parent = ancestors.back(); - current_value = Transform::backwardSingleChild( - {current_value, current_parent->data()}, child_index); - OctreeType::NodeRefType child = - current_parent->getOrAllocateChild(child_index); - ancestors.emplace_back(&child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{new_value - current_value, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodePtrType current_node = ancestors.back(); - ancestors.pop_back(); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node->data() += coefficients.details; - } - - root_scale_coefficient_ += coefficients.scale; -} - -void HashedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index, - FloatingPoint update) { - setNeedsPruning(); - setNeedsThresholding(); - setLastUpdatedStamp(); - const MortonIndex morton_code = convert::nodeIndexToMorton(index); - - std::vector ancestors; - const int height_difference = tree_height_ - index.height; - ancestors.reserve(height_difference); - ancestors.emplace_back(&ndtree_.getRootNode()); - for (int parent_height = tree_height_; index.height + 1 < parent_height; - --parent_height) { - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - OctreeType::NodePtrType current_parent = ancestors.back(); - OctreeType::NodeRefType child = - current_parent->getOrAllocateChild(child_index); - ancestors.emplace_back(&child); - } - DCHECK_EQ(ancestors.size(), height_difference); - - Coefficients::Parent coefficients{update, {}}; - for (int parent_height = index.height + 1; parent_height <= tree_height_; - ++parent_height) { - OctreeType::NodePtrType current_node = ancestors.back(); - ancestors.pop_back(); - const NdtreeIndexRelativeChild child_index = - OctreeIndex::computeRelativeChildIndex(morton_code, parent_height); - coefficients = - Transform::forwardSingleChild(coefficients.scale, child_index); - current_node->data() += coefficients.details; - } - root_scale_coefficient_ += coefficients.scale; -} - -void HashedWaveletOctreeBlock::forEachLeaf( - const BlockIndex& block_index, - MapBase::IndexedLeafVisitorFunction visitor_fn, - IndexElement termination_height) const { - ProfilerZoneScoped; - if (empty()) { - return; - } - - struct StackElement { - const OctreeIndex node_index; - OctreeType::NodeConstRefType node; - const Coefficients::Scale scale_coefficient{}; - }; - std::stack stack; - stack.emplace(StackElement{OctreeIndex{tree_height_, block_index}, - ndtree_.getRootNode(), root_scale_coefficient_}); - while (!stack.empty()) { - const OctreeIndex node_index = stack.top().node_index; - OctreeType::NodeConstRefType node = stack.top().node; - const FloatingPoint node_scale_coefficient = stack.top().scale_coefficient; - stack.pop(); - - const Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({node_scale_coefficient, {node.data()}}); - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - const OctreeIndex child_node_index = - node_index.computeChildIndex(child_idx); - const FloatingPoint child_scale_coefficient = - child_scale_coefficients[child_idx]; - OctreeType::NodeConstPtrType child_node = node.getChild(child_idx); - if (child_node && termination_height < child_node_index.height) { - stack.emplace(StackElement{child_node_index, *child_node, - child_scale_coefficient}); - } else { - visitor_fn(child_node_index, child_scale_coefficient); - } - } - } -} - -void HashedWaveletOctreeBlock::recursiveThreshold( // NOLINT - HashedWaveletOctreeBlock::OctreeType::NodeRefType node, - FloatingPoint& node_scale_coefficient) { - // Decompress child values - auto& node_detail_coefficients = node.data(); - Coefficients::CoefficientsArray child_scale_coefficients = - Transform::backward({node_scale_coefficient, node_detail_coefficients}); - - // Handle each child - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - Coefficients::Scale& child_scale = child_scale_coefficients[child_idx]; - if (auto child_node = node.getChild(child_idx); child_node) { - recursiveThreshold(*child_node, child_scale); - } else { - child_scale = std::clamp(child_scale, min_log_odds_, max_log_odds_); - } - } - - // Compress - const auto [new_scale, new_details] = - Transform::forward(child_scale_coefficients); - node_detail_coefficients = new_details; - node_scale_coefficient = new_scale; -} - -void HashedWaveletOctreeBlock::recursivePrune( // NOLINT - HashedWaveletOctreeBlock::OctreeType::NodeRefType node) { - bool has_at_least_one_child = false; - for (NdtreeIndexRelativeChild child_idx = 0; - child_idx < OctreeIndex::kNumChildren; ++child_idx) { - if (OctreeType::NodePtrType child_node = node.getChild(child_idx); - child_node) { - recursivePrune(*child_node); - if (!child_node->hasChildrenArray() && - !child_node->hasNonzeroData(1e-3f)) { - node.eraseChild(child_idx); - } else { - has_at_least_one_child = true; - } - } - } - if (!has_at_least_one_child) { - node.deleteChildrenArray(); - } -} -} // namespace wavemap diff --git a/library/cpp/src/io/stream_conversions.cc b/library/cpp/src/io/stream_conversions.cc index 364260678..0abaf5233 100644 --- a/library/cpp/src/io/stream_conversions.cc +++ b/library/cpp/src/io/stream_conversions.cc @@ -6,29 +6,28 @@ namespace wavemap::io { bool mapToStream(const MapBase& map, std::ostream& ostream) { - // Call the appropriate mapToStream converter based on the map's derived type - if (const auto* hashed_blocks = dynamic_cast(&map); - hashed_blocks) { - return io::mapToStream(*hashed_blocks, ostream); - } - if (const auto* wavelet_octree = dynamic_cast(&map); - wavelet_octree) { - return io::mapToStream(*wavelet_octree, ostream); - } - if (const auto* hashed_wavelet_octree = - dynamic_cast(&map); - hashed_wavelet_octree) { - return io::mapToStream(*hashed_wavelet_octree, ostream); - } - if (const auto* hashed_chunked_wavelet_octree = - dynamic_cast(&map); - hashed_chunked_wavelet_octree) { - return io::mapToStream(*hashed_chunked_wavelet_octree, ostream); + // Call the appropriate mapToStream converter based on the map's type. + switch (map.getMapType()) { + case MapType::kHashedBlocks: + return io::mapToStream(static_cast(map), ostream); + case MapType::kWaveletOctree: + return io::mapToStream(static_cast(map), ostream); + case MapType::kHashedWaveletOctree: + return io::mapToStream(static_cast(map), + ostream); + case MapType::kHashedChunkedWaveletOctree: + return io::mapToStream( + static_cast(map), ostream); + case MapType::kLayeredHashedWaveletOctree: + LOG(WARNING) << "Layered hashed wavelet octree maps require typed IO " + "with an explicit cell data serializer."; + return false; + case MapType::kOctree: + default: + LOG(WARNING) << "Could not serialize requested map to stream. " + "Map type not yet supported."; + return false; } - - LOG(WARNING) << "Could not serialize requested map to stream. " - "Map type not yet supported."; - return false; } bool streamToMap(std::istream& istream, MapBase::Ptr& map) { diff --git a/library/cpp/test/src/core/CMakeLists.txt b/library/cpp/test/src/core/CMakeLists.txt index cb246e0ec..2deb21e01 100644 --- a/library/cpp/test/src/core/CMakeLists.txt +++ b/library/cpp/test/src/core/CMakeLists.txt @@ -19,6 +19,7 @@ target_sources(test_wavemap_core PRIVATE integrator/test_range_image_intersector.cc map/test_haar_cell.cc map/test_hashed_blocks.cc + map/test_layered_map_definition.cc map/test_map.cc map/test_volumetric_octree.cc utils/bits/test_bit_operations.cc @@ -36,6 +37,7 @@ target_sources(test_wavemap_core PRIVATE utils/neighbors/test_ndtree_adjacency.cc utils/profile/test_resource_monitor.cc utils/query/test_classified_map.cc + utils/query/test_local_elevation_classifier.cc utils/query/test_map_interpolator.cpp utils/query/test_occupancy_classifier.cc utils/query/test_probability_conversions.cc diff --git a/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc b/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc index 7afd0e072..163012142 100644 --- a/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc +++ b/library/cpp/test/src/core/integrator/test_pointcloud_integrators.cc @@ -65,12 +65,17 @@ struct IntegratorDataStructurePair { using DataStructureType = DataStructureT; }; +using LayeredChunkedTestVoxel = VoxelData; + using IntegratorTypes = ::testing::Types< IntegratorDataStructurePair, IntegratorDataStructurePair, IntegratorDataStructurePair, IntegratorDataStructurePair>; + HashedChunkedWaveletOctree>, + IntegratorDataStructurePair< + HashedChunkedWaveletIntegratorT, + HashedChunkedWaveletOctreeT>>; TYPED_TEST_SUITE(PointcloudIntegratorTypedTest, IntegratorTypes, ); TYPED_TEST(PointcloudIntegratorTypedTest, diff --git a/library/cpp/test/src/core/map/test_layered_map_definition.cc b/library/cpp/test/src/core/map/test_layered_map_definition.cc new file mode 100644 index 000000000..188e96e8f --- /dev/null +++ b/library/cpp/test/src/core/map/test_layered_map_definition.cc @@ -0,0 +1,402 @@ +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace wavemap::layered { +namespace { + +struct ReflectivityLayer + : schema::ContinuousLayer> { + static constexpr std::string_view name = "reflectivity"; +}; + +struct TemperatureLayer + : schema::ContinuousLayer> { + static constexpr std::string_view name = "temperature"; +}; + +struct ClassLayer + : schema::DiscreteLayer> { + static constexpr std::string_view name = "class"; +}; + +struct AveragedLayer + : schema::StatefulContinuousLayer { + static constexpr std::string_view name = "averaged"; +}; + +struct CustomDampedPolicy { + float operator()(float current, + const LayerObservation& observation) const { + return 0.75f * current + 0.25f * observation.value; + } +}; + +struct UnsupportedContinuousValue { + float value = 0.f; +}; + +static_assert(kIsContinuousLayerUpdatePolicy); +static_assert(kIsDiscreteLayerUpdatePolicy, int>); +static_assert(schema::kHasContinuousValueTraits); +static_assert(schema::kHasContinuousValueTraits); +static_assert(kHasLayerStreamCodec); +static_assert(kHasLayerStreamCodec); +static_assert(!schema::kHasContinuousValueTraits); +static_assert(!kHasLayerStreamCodec); + +using ContinuousDefinition = + LayeredMapDefinition>; +using MixedDefinition = + LayeredMapDefinition>; +using DiscreteDefinition = + LayeredMapDefinition>; +using EmptyDefinition = LayeredMapDefinition>; +using WeightedDefinition = + LayeredMapDefinition>; +using ChunkedMixedDefinition = LayeredMapDefinition< + schema::LayerSchema, + HashedChunkedWaveletOctreeBackend>; + +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(ContinuousDefinition::Schema::continuousLayerCount == 2u); +static_assert(MixedDefinition::Schema::continuousLayerCount == 1u); +static_assert(MixedDefinition::Schema::discreteLayerCount == 1u); +static_assert(DiscreteDefinition::Schema::continuousLayerCount == 0u); +static_assert(EmptyDefinition::Schema::size == 0u); +static_assert( + std::is_same_v, FloatingPoint>); +static_assert( + std::is_same_v, WeightedMeanState>); +static_assert(schema::kHasContinuousValueTraits); +static_assert(kHasLayerStreamCodec); +static_assert(std::is_same_v< + ChunkedMixedDefinition::ContinuousMap, + HashedChunkedWaveletOctreeT>); + +template +typename DefinitionT::Config makeConfig() { + typename DefinitionT::Config config; + config.continuous_map.min_cell_width = 0.25f; + config.continuous_map.min_log_odds = -2.f; + config.continuous_map.max_log_odds = 4.f; + config.continuous_map.tree_height = 4; + return config; +} + +TEST(LayeredMapDefinition, GeneratesContinuousStorage) { + ContinuousDefinition::Map map(makeConfig()); + const Index3D index{1, 2, 3}; + + ContinuousDefinition::Voxel voxel; + voxel.occupancy = 0.7f; + voxel.data.get() = 0.25f; + voxel.data.get() = 18.f; + map.continuousMap().setVoxelValue(index, voxel); + + const auto stored = map.continuousMap().getVoxelValue(index); + EXPECT_FLOAT_EQ(stored.occupancy, 0.7f); + EXPECT_FLOAT_EQ(stored.data.get(), 0.25f); + EXPECT_FLOAT_EQ(stored.data.get(), 18.f); +} + +TEST(LayeredMapDefinition, GeneratesMixedContinuousAndDiscreteStorage) { + MixedDefinition::Map map(makeConfig()); + const Index3D index{-2, 1, 4}; + + MixedDefinition::Voxel voxel; + voxel.occupancy = 0.4f; + voxel.data.get() = 0.8f; + map.continuousMap().setVoxelValue(index, voxel); + map.discreteLayers().get().setValue(index, 3); + + EXPECT_FLOAT_EQ(map.continuousMap() + .getVoxelValue(index) + .data.get(), + 0.8f); + ASSERT_TRUE(map.discreteLayers().get().getValue(index)); + EXPECT_EQ(*map.discreteLayers().get().getValue(index), 3); +} + +TEST(LayeredMapDefinition, SupportsChunkedContinuousBackend) { + ChunkedMixedDefinition::Map map(makeConfig()); + const Index3D index{-3, 5, 9}; + + ChunkedMixedDefinition::Voxel voxel; + voxel.occupancy = 0.6f; + voxel.data.get() = 0.35f; + map.continuousMap().setVoxelValue(index, voxel); + map.discreteLayers().get().setValue(index, 2); + map.continuousMap().threshold(); + map.continuousMap().prune(); + + const auto stored = map.continuousMap().getVoxelValue(index); + EXPECT_FLOAT_EQ(stored.occupancy, 0.6f); + EXPECT_FLOAT_EQ(stored.data.get(), 0.35f); + ASSERT_TRUE(map.discreteLayers().get().getValue(index)); + EXPECT_EQ(*map.discreteLayers().get().getValue(index), 2); +} + +TEST(LayeredMapDefinition, ChunkedBackendPersistenceRoundTrip) { + const auto config = makeConfig(); + ChunkedMixedDefinition::Map source(config); + ChunkedMixedDefinition::Map restored(config); + const Index3D index{-3, 5, 9}; + + ChunkedMixedDefinition::Voxel voxel; + voxel.occupancy = 0.6f; + voxel.data.get() = 0.35f; + source.continuousMap().setVoxelValue(index, voxel); + source.discreteLayers().get().setValue(index, 2); + + const auto file_path = + std::filesystem::temp_directory_path() / + "wavemap_chunked_layered_round_trip.wvmp"; + ASSERT_TRUE(ChunkedMixedDefinition::MapIo::save(file_path, source)); + std::string error_message; + ASSERT_TRUE(ChunkedMixedDefinition::MapIo::load( + file_path, restored, &error_message)) + << error_message; + std::filesystem::remove(file_path); + + const auto stored = restored.continuousMap().getVoxelValue(index); + EXPECT_NEAR(stored.occupancy, voxel.occupancy, 1e-5f); + EXPECT_NEAR(stored.data.get(), + voxel.data.get(), 1e-5f); + ASSERT_TRUE(restored.discreteLayers().get().getValue(index)); + EXPECT_EQ(*restored.discreteLayers().get().getValue(index), 2); +} + +TEST(LayeredMapDefinition, IntegratesHeterogeneousObservationBatch) { + MixedDefinition::Map map(makeConfig()); + LayeredPipeline pipeline(map); + LayerObservationBatch batch; + const Point3D position(0.5f, 0.25f, -0.25f); + + batch.add(position, 0.65f); + batch.add(position, 4); + EXPECT_EQ(batch.size(), 2u); + + const auto result = pipeline.integrateBatch(batch); + EXPECT_EQ(result.received, 2u); + EXPECT_EQ(result.integrated, 2u); + EXPECT_EQ(result.rejected, 0u); + + const Index3D index = convert::pointToNearestIndex( + position, 1.f / map.continuousMap().getMinCellWidth()); + EXPECT_FLOAT_EQ(map.continuousMap() + .getVoxelValue(index) + .data.get(), + 0.65f); + ASSERT_TRUE(map.discreteLayers().get().getValue(index)); + EXPECT_EQ(*map.discreteLayers().get().getValue(index), 4); +} + +TEST(LayeredMapDefinition, TransformsAllObservationBatchPositions) { + LayerObservationBatch batch; + batch.add(Point3D(1.f, 2.f, 3.f), 0.5f); + batch.add(Point3D(-1.f, 0.f, 1.f), 2); + + const Transformation3D transform(Rotation3D(), Point3D(2.f, -1.f, 0.5f)); + batch.transformPositions(transform); + + EXPECT_TRUE(batch.observations()[0].position.isApprox( + Point3D(3.f, 1.f, 3.5f))); + EXPECT_TRUE(batch.observations()[0].position.isApprox( + Point3D(1.f, -1.f, 1.5f))); +} + +TEST(LayeredMapDefinition, StoresTrueWeightedMeanFusionState) { + WeightedDefinition::Map map(makeConfig()); + LayeredPipeline pipeline(map); + const Point3D position(0.5f, 0.25f, -0.25f); + + LayerObservation first(position, 0.2f); + first.confidence = 1.f; + LayerObservation second(position, 0.8f); + second.confidence = 3.f; + const std::vector> observations{first, second}; + const auto result = pipeline.integrate(observations); + + EXPECT_EQ(result.integrated, 2u); + EXPECT_EQ(result.updated_voxels, 1u); + const Index3D index = convert::pointToNearestIndex( + position, 1.f / map.continuousMap().getMinCellWidth()); + const auto state = + map.continuousMap().getVoxelValue(index).data.get(); + EXPECT_FLOAT_EQ(state.weighted_sum, 2.6f); + EXPECT_FLOAT_EQ(state.total_weight, 4.f); + EXPECT_FLOAT_EQ(LayerStateConversion::value(state), 0.65f); +} + +TEST(LayeredMapDefinition, WeightedMeanHasExplicitUnobservedState) { + const WeightedMeanState unobserved; + EXPECT_FLOAT_EQ(unobserved.total_weight, 0.f); + EXPECT_FLOAT_EQ(unobserved.valueOr(-1.f), -1.f); + + LayerObservation observation(Point3D::Zero(), 0.8f); + const auto initialized = + WeightedMeanLayerUpdatePolicy{}(unobserved, observation); + EXPECT_FLOAT_EQ(initialized.weighted_sum, 0.8f); + EXPECT_FLOAT_EQ(initialized.total_weight, 1.f); + EXPECT_FLOAT_EQ(initialized.valueOr(), 0.8f); +} + +TEST(LayeredMapDefinition, WeightedMeanFusionStateStreamRoundTrip) { + const WeightedMeanState expected{2.6f, 4.f}; + std::stringstream stream; + LayerStreamCodec::write(stream, expected); + const auto restored = LayerStreamCodec::read(stream); + EXPECT_FLOAT_EQ(restored.weighted_sum, expected.weighted_sum); + EXPECT_FLOAT_EQ(restored.total_weight, expected.total_weight); + EXPECT_FLOAT_EQ(restored.valueOr(), 0.65f); +} + +TEST(LayeredMapDefinition, SupportsDiscreteOnlyAndEmptySchemas) { + DiscreteDefinition::Map discrete_map(makeConfig()); + EmptyDefinition::Map empty_map(makeConfig()); + const Index3D index{0, 0, 0}; + + discrete_map.discreteLayers().get().setValue(index, 7); + discrete_map.continuousMap().setCellValue(index, 0.5f); + empty_map.continuousMap().setCellValue(index, 0.6f); + + EXPECT_EQ(*discrete_map.discreteLayers().get().getValue(index), + 7); + EXPECT_FLOAT_EQ(discrete_map.continuousMap().getCellValue(index), 0.5f); + EXPECT_FLOAT_EQ(empty_map.continuousMap().getCellValue(index), 0.6f); +} + +TEST(LayeredMapDefinition, UnchangedDiscreteAssignmentDoesNotBecomeDirty) { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + const Index3D index{2, 3, 4}; + + layer.setValue(index, 7); + ASSERT_EQ(layer.dirtyParentKeys().size(), 1u); + layer.clearDirtyParentKeys(); + + layer.setValue(index, 7); + EXPECT_TRUE(layer.dirtyParentKeys().empty()); + + layer.setValue(index, 8); + EXPECT_EQ(layer.dirtyParentKeys().size(), 1u); + EXPECT_EQ(layer.getValue(index), 8); +} + +TEST(LayeredMapDefinition, BatchedDiscreteAssignmentMarksOnlyChangedParents) { + DiscreteLayer layer(DiscreteCompressionConfig{1}); + const Index3D unchanged{0, 0, 0}; + const Index3D changed{8, 0, 0}; + + layer.setValues({{unchanged, 1}, {changed, 2}}); + layer.clearDirtyParentKeys(); + layer.setValues({{unchanged, 1}, {changed, 3}}); + + ASSERT_EQ(layer.dirtyParentKeys().size(), 1u); + EXPECT_EQ(layer.getValue(unchanged), 1); + EXPECT_EQ(layer.getValue(changed), 3); + + layer.clearDirtyParentKeys(); + layer.setValues({{unchanged, 1}, {changed, 3}}); + EXPECT_TRUE(layer.dirtyParentKeys().empty()); +} + +TEST(LayeredMapDefinition, ConfigBuilderPopulatesExistingConfig) { + LayeredMapConfigBuilder builder; + builder.map() + .minCellWidth(0.2f) + .occupancyLogOdds(-3.f, 5.f) + .treeHeight(7) + .onlyPruneBlocksIfUnusedFor(8.f); + builder.occupancy().pruningScale(0.002f).pruningWeight(2.f); + builder.layer() + .storageBounds(0.f, 1.f) + .pruningScale(0.01f) + .pruningWeight(0.5f); + builder.combinedPruningThreshold(1.5f).discreteBlockHeight(2); + + const auto config = builder.build(); + EXPECT_FLOAT_EQ(config.continuous_map.min_cell_width, 0.2f); + EXPECT_FLOAT_EQ(config.continuous_map.min_log_odds, -3.f); + EXPECT_FLOAT_EQ(config.continuous_map.max_log_odds, 5.f); + EXPECT_EQ(config.continuous_map.tree_height, 7); + EXPECT_FLOAT_EQ(config.continuous_map.only_prune_blocks_if_unused_for, 8.f); + EXPECT_FLOAT_EQ(config.continuous_pruning.occupancy.scale, 0.002f); + EXPECT_FLOAT_EQ(config.continuous_pruning.occupancy.weight, 2.f); + + const auto& reflectivity_bounds = + config.continuous_threshold.data.get(); + const auto& reflectivity_pruning = + config.continuous_pruning.data.get(); + EXPECT_FLOAT_EQ(reflectivity_bounds.min, 0.f); + EXPECT_FLOAT_EQ(reflectivity_bounds.max, 1.f); + EXPECT_FLOAT_EQ(reflectivity_pruning.scale, 0.01f); + EXPECT_FLOAT_EQ(reflectivity_pruning.weight, 0.5f); + EXPECT_FLOAT_EQ(config.continuous_pruning.combined_threshold, 1.5f); + EXPECT_FLOAT_EQ(config.continuous_pruning.data.combined_threshold, 1.5f); + EXPECT_EQ(config.discrete_compression.block_height, 2); +} + +TEST(LayeredUpdatePolicy, ProvidesStandardAndCustomPolicies) { + const LayerObservation low(Point3D::Zero(), 0.2f); + const LayerObservation high(Point3D::Zero(), 0.8f); + + EXPECT_FLOAT_EQ(ReplaceLayerUpdatePolicy{}(0.5f, high), 0.8f); + EXPECT_FLOAT_EQ(MinimumLayerUpdatePolicy{}(0.5f, low), 0.2f); + EXPECT_FLOAT_EQ(MaximumLayerUpdatePolicy{}(0.5f, high), 0.8f); + EXPECT_FLOAT_EQ(AccumulateLayerUpdatePolicy{}(0.5f, low), 0.7f); + EXPECT_FLOAT_EQ(CustomDampedPolicy{}(0.4f, high), 0.5f); + + using ExponentialPolicy = ExponentialScalarLayerUpdatePolicy<1, 5>; + static_assert(kIsContinuousLayerUpdatePolicy); + EXPECT_FLOAT_EQ(ExponentialPolicy::observationWeight(), 0.2f); + EXPECT_FLOAT_EQ(ExponentialPolicy{}(0.5f, high), 0.56f); + + float bounded_value = 0.f; + for (int update_idx = 0; update_idx < 100; ++update_idx) { + const auto& observation = update_idx % 2 ? low : high; + bounded_value = ExponentialPolicy{}(bounded_value, observation); + EXPECT_GE(bounded_value, 0.f); + EXPECT_LE(bounded_value, 1.f); + } + + EXPECT_TRUE(LogicalOrLayerUpdatePolicy{}( + false, LayerObservation(Point3D::Zero(), true))); + EXPECT_FALSE(LogicalAndLayerUpdatePolicy{}( + true, LayerObservation(Point3D::Zero(), false))); +} + +TEST(LayeredUpdatePolicy, ConfidenceBlendUsesObservationConfidence) { + struct FloatArithmetic { + static float add(float lhs, float rhs) { return lhs + rhs; } + static float scale(float value, float factor) { return value * factor; } + }; + + LayerObservation observation(Point3D::Zero(), 1.f); + observation.confidence = 0.25f; + const ConfidenceBlendLayerUpdatePolicy policy; + EXPECT_FLOAT_EQ(policy(0.f, observation), 0.25f); + + observation.confidence.reset(); + const ConfidenceBlendLayerUpdatePolicy + default_policy(0.4f); + EXPECT_FLOAT_EQ(default_policy(0.f, observation), 0.4f); +} + +} // namespace +} // namespace wavemap::layered diff --git a/library/cpp/test/src/core/map/test_map.cc b/library/cpp/test/src/core/map/test_map.cc index 2d397f860..f36d7e0f3 100644 --- a/library/cpp/test/src/core/map/test_map.cc +++ b/library/cpp/test/src/core/map/test_map.cc @@ -205,6 +205,33 @@ TYPED_TEST(MapTest, InsertionAndLeafVisitor) { } } +TEST(HashedChunkedWaveletOctreeTest, StoresTypedVoxelData) { + using Voxel = VoxelData; + HashedChunkedWaveletOctreeConfig config; + config.tree_height = 6; + HashedChunkedWaveletOctreeT map(config); + + const Index3D index(3, -2, 7); + map.setVoxelValue(index, Voxel{/*occupancy=*/1.25f, /*data=*/0.4f}); + + Voxel voxel = map.getVoxelValue(index); + EXPECT_FLOAT_EQ(voxel.occupancy, 1.25f); + EXPECT_FLOAT_EQ(voxel.data, 0.4f); + EXPECT_FLOAT_EQ(map.getCellValue(index), voxel.occupancy); + + map.addToVoxelValue(index, + Voxel{/*occupancy=*/0.5f, /*data=*/0.1f}); + voxel = map.getVoxelValue(index); + EXPECT_NEAR(voxel.occupancy, 1.75f, 1e-5f); + EXPECT_NEAR(voxel.data, 0.5f, 1e-5f); + + map.threshold(); + map.prune(); + voxel = map.getVoxelValue(index); + EXPECT_NEAR(voxel.occupancy, 1.75f, 1e-5f); + EXPECT_NEAR(voxel.data, 0.5f, 1e-5f); +} + // TODO(victorr): For classes derived from VolumetricOctreeInterface, test // NodeIndex based setters and getters (incl. whether values of // all children are updated but nothing spills to the diff --git a/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc b/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc new file mode 100644 index 000000000..28ac297fc --- /dev/null +++ b/library/cpp/test/src/core/utils/query/test_local_elevation_classifier.cc @@ -0,0 +1,109 @@ +#include + +#include + +namespace wavemap::layered { +namespace { + +PosedPointcloud<> makePointcloud(std::vector points, + const Transformation3D& pose = {}) { + return PosedPointcloud<>(pose, Pointcloud<>(points)); +} + +TEST(LocalElevationClassifierTest, ValidatesDefaultConfig) { + EXPECT_TRUE(LocalElevationClassifierConfig{}.isValid()); + + LocalElevationClassifierConfig config; + config.obstacle_height = config.ground_tolerance; + EXPECT_FALSE(config.isValid()); +} + +TEST(LocalElevationClassifierTest, ClassifiesSupportedCellByRelativeHeight) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + const auto cloud = makePointcloud({Point3D(0.1f, 0.1f, 1.f), + Point3D(0.2f, 0.1f, 1.1f), + Point3D(0.3f, 0.1f, 1.3f), + Point3D(0.4f, 0.1f, 1.5f)}); + + const auto classes = classifier.classify(cloud); + + ASSERT_EQ(classes.size(), 4u); + EXPECT_EQ(classes[0], GeometricClass::kGround); + EXPECT_EQ(classes[1], GeometricClass::kGround); + EXPECT_EQ(classes[2], GeometricClass::kUnknown); + EXPECT_EQ(classes[3], GeometricClass::kObstacle); +} + +TEST(LocalElevationClassifierTest, LeavesSparseCellsUnknown) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + const auto cloud = makePointcloud( + {Point3D(-0.1f, -0.1f, 0.f), Point3D(-0.2f, -0.1f, 1.f)}); + + const auto classes = classifier.classify(cloud); + + ASSERT_EQ(classes.size(), 2u); + EXPECT_EQ(classes[0], GeometricClass::kUnknown); + EXPECT_EQ(classes[1], GeometricClass::kUnknown); +} + +TEST(LocalElevationClassifierTest, UsesWorldCoordinates) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 2u; + const LocalElevationClassifier classifier(config); + Transformation3D pose; + pose.getPosition() = Point3D(2.f, 3.f, 4.f); + const auto cloud = makePointcloud( + {Point3D(0.1f, 0.1f, 0.f), Point3D(0.2f, 0.1f, 0.5f)}, pose); + + const auto classes = classifier.classify(cloud); + + ASSERT_EQ(classes.size(), 2u); + EXPECT_EQ(classes[0], GeometricClass::kGround); + EXPECT_EQ(classes[1], GeometricClass::kObstacle); +} + +TEST(LocalElevationClassifierTest, CreatesOnlyObservedWorldFrameUpdates) { + LocalElevationClassifierConfig config; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + Transformation3D pose; + pose.getPosition() = Point3D(2.f, 3.f, 4.f); + const auto cloud = makePointcloud({Point3D(0.1f, 0.1f, 0.f), + Point3D(0.2f, 0.1f, 0.3f), + Point3D(0.3f, 0.1f, 0.5f)}, + pose); + + const auto observations = + makeGeometricClassObservations(cloud, classifier); + + ASSERT_EQ(observations.size(), 2u); + EXPECT_EQ(observations[0].value, + static_cast(GeometricClass::kGround)); + EXPECT_EQ(observations[1].value, + static_cast(GeometricClass::kObstacle)); + EXPECT_TRUE(observations[0].position.isApprox(Point3D(2.1f, 3.1f, 4.f))); + EXPECT_TRUE(observations[1].position.isApprox(Point3D(2.3f, 3.1f, 4.5f))); +} + +TEST(LocalElevationClassifierTest, FiltersObservationsBySensorRange) { + LocalElevationClassifierConfig config; + config.grid_cell_width = 10.f; + config.min_points_per_cell = 3u; + const LocalElevationClassifier classifier(config); + const auto cloud = makePointcloud({Point3D(0.1f, 0.f, 0.f), + Point3D(1.f, 0.f, 0.f), + Point3D(2.f, 0.f, 0.5f)}); + + const auto observations = makeGeometricClassObservations( + cloud, classifier, EndpointRange{0.5f, 1.5f}); + + ASSERT_EQ(observations.size(), 1u); + EXPECT_TRUE(observations.front().position.isApprox(Point3D(1.f, 0.f, 0.f))); +} + +} // namespace +} // namespace wavemap::layered