Ensure and document OpenVDB support

DEVELOPING.md

@@ -66,6 +66,10 @@ Within a development environment:
     rosdep install --from-path src
     ```
 
+
+> [!IMPORTANT]
+> `beluga_vdb` requires OpenVDB to be installed in order to be used. For Ubuntu distributions previous to `Noble` OpenVDB needs to be installed from sources before building. Installation instructions can be found [here](beluga_vdb/README.md). If you don't need `beluga_vdb`, you can skip it using `colcon build --symlink-install --packages-ignore beluga_vdb`.
+
 For more advanced tooling, check repository [tools](./tools).
 
 ## CI/CD

README.md

@@ -43,6 +43,7 @@ This repository contains the following packages:
 | [`beluga_example`](beluga_example)           | Example launch files, showing how to run Beluga-based nodes.                                                            |
 | [`beluga_benchmark`](beluga_benchmark)       | Scripts to benchmark, profile and also compare Beluga with other MCL implementations.                                   |
 | [`beluga_system_tests`](beluga_system_tests) | System integration tests for Beluga.                                                                                    |
+| [`beluga_vdb`](beluga_vdb) | A library extension for `beluga` facilitating the use of OpenVDB for 3D localization.                                                                                    |
 
 ## ⚙️ First Steps
 

beluga_vdb/README.md

@@ -0,0 +1,167 @@
+# Beluga VDB
+
+<div align="center">
+<img alt="Shows the Beluga logo." src="images/57fe338a-3e7e-46b3-ad2f-17011d6d306a.png" height="200">
+</div>
+
+## 🌐 Overview
+
+BelugaVDB is a library extension for `beluga` that integrates [OpenVDB](https://www.openvdb.org/), enabling advanced 3D localization capabilities. Currently, this extension uses OpenVDB to efficiently process 3D maps and pointcloud data.
+
+## 🔰 Mapping
+
+BelugaVDB requires **level-set** maps in `vdb` format in order to work properly. VDB maps can be generated using suitables third party packages such as [VDB Mapping](https://github.com/fzi-forschungszentrum-informatik/vdb_mapping), but the resulting map must be converted to a level-set map.
+
+A simple post-processing code for adapting a map generated by VDB Mapping into a `vdb` level-set map is shown below:
+
+```cpp
+#include <openvdb/openvdb.h>
+#include <openvdb/tools/TopologyToLevelSet.h>
+
+int main()
+{
+    openvdb::initialize();
+
+    // Create a VDB file object.
+    openvdb::io::File file("vdb_mapping_map.vdb");
+
+    // Open the file.  This reads the file header, but not any grids.
+    file.open();
+
+    // Print the names of the grid
+    for (openvdb::io::File::NameIterator nameIter = file.beginName(); nameIter != file.endName(); ++nameIter)
+    {
+        std::cout << "Grid name: " << nameIter.gridName() << std::endl;
+
+    }
+
+    // Retrieve a shared pointer
+    openvdb::GridBase::Ptr baseGrid;
+    baseGrid = file.readGrid("[0]");
+
+    // Close the file
+    file.close();
+
+    // Cast the generic grid pointer to a FloatGrid pointer.
+    openvdb::FloatGrid::Ptr grid = openvdb::gridPtrCast<openvdb::FloatGrid>(baseGrid);
+
+    // Transform to level set
+    openvdb::FloatGrid::Ptr grid_levelset = nullptr;
+    grid_levelset = openvdb::tools::topologyToLevelSet(*grid, 3, 1, 0, 0);
+
+    // Save new grid
+    openvdb::io::File("levle_set_map.vdb").write({grid_levelset});
+}
+```
+
+Also, `vdb` level-set maps can be generated from `pcd` maps (created from SLAM libraries such as [FAST-LIO](https://github.com/hku-mars/FAST_LIO)).
+
+A simple code to generate a `vdb` level-set map from a `pcd` file is shown below:
+
+```cpp
+#include <openvdb/openvdb.h>
+#include <openvdb/tree/Tree.h>
+#include <openvdb/tools/TopologyToLevelSet.h>
+#include <openvdb/math/Transform.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include <iostream>
+
+int main()
+{
+    openvdb::initialize();
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+
+    // Open PCD file
+    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("map.pcd", *cloud) == -1) //* load the file
+    {
+        PCL_ERROR ("Couldn't read file map.pcd \n");
+        return (-1);
+    }
+    std::cout << "Loaded cloud succesfuly"<< std::endl;
+
+
+    // Create VDB grid of resolution 0.5
+    openvdb::FloatGrid::Ptr grid = openvdb::FloatGrid::create();
+    grid->setTransform(openvdb::math::Transform::createLinearTransform(0.5));
+    grid->setGridClass(openvdb::GRID_LEVEL_SET);
+
+    // Get an accessor for coordinate-based access to voxels.
+    openvdb::FloatGrid::Accessor accessor = grid->getAccessor();
+
+    // Fill the grid
+    for (const auto& point: *cloud)
+    {
+        // World coordinates
+        openvdb::Vec3f world_vect(point.x, point.y, point.z);
+
+        // Transform to index world
+        openvdb::math::Transform transform;
+        openvdb::math::Coord ijk = transform.worldToIndexCellCentered(world_vect);
+
+        // Set the voxel to 1
+        accessor.setValue(ijk, 1.0);
+        // Activate the voxel
+        accessor.setActiveState(ijk);
+    }
+
+    // Transform to level set
+    openvdb::FloatGrid::Ptr grid_levelset = nullptr;
+    grid_levelset = openvdb::tools::topologyToLevelSet(*grid, 3, 1, 0, 0);
+
+    // Name the grid "Map".
+    grid_levelset->setName("Map");
+
+    // Save new grid
+    openvdb::io::File("pcdgrid.vdb").write({grid_levelset});
+}
+```
+
+The resulting level-set maps can be visualize using [OpenVDBViewer](https://github.com/JimJam42/OpenVDBViewer).
+
+![Level-set map](images/example_level_set_map.png)
+
+## 📦 External Dependencies
+
+### OpenVDB
+
+For Ubuntu 24.04 and newer distributions, OpenVDB can be installed using `apt`:
+
+```bash
+sudo apt install libopenvdb-dev
+```
+
+For older distributions, it must be installed from sources as follows:
+
+#### Step 1: Download OpenVDB
+
+Clone [OpenVDB](https://github.com/AcademySoftwareFoundation/openvdb) into your machine (for Jammy version 8.2.0 is recommended):
+
+```bash
+git clone -b v8.2.0 https://github.com/AcademySoftwareFoundation/openvdb.git
+```
+
+#### Step 2: Compile OpenVDB
+
+Now you need to build OpenVDB:
+
+```bash
+cd openvdb && \
+mkdir build && cd build && \
+cmake .. && \
+make -j$(nproc)
+```
+
+#### Step 3: Install OpenVDB
+
+You can now install OpenVDB using:
+
+```bash
+sudo make install
+```
+
+> [!NOTE]
+> For more information about OpenVDB please refer to the [official documentation](https://www.openvdb.org/documentation/doxygen/).

beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp

@@ -24,8 +24,6 @@
 
 #include <Eigen/Core>
 
-#include <range/v3/algorithm/fold_left.hpp>
-#include <range/v3/algorithm/for_each.hpp>
 #include <range/v3/numeric/accumulate.hpp>
 #include <range/v3/range/conversion.hpp>
 #include <range/v3/view/all.hpp>
@@ -133,7 +131,7 @@ class LikelihoodFieldModel3 {
                               ranges::to<std::vector>();
 
     return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type {
-      return ranges::fold_left(
+      return ranges::accumulate(
           points |  //
               ranges::views::transform([this, &state](const auto& point) {
                 const Eigen::Vector3d point_in_state_frame = state * point;

docker/images/humble/Dockerfile

@@ -32,6 +32,24 @@ RUN git clone -b develop https://github.com/NERSC/timemory.git \
     . \
   && cmake --build ./build --target timem
 
+FROM ros:humble-ros-base-jammy AS openvdb-builder
+
+RUN apt-get update \
+  && apt-get install --no-install-recommends -y \
+    git \
+    libboost-all-dev \
+    libtbb-dev \
+    libblosc-dev \
+  && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /opt
+
+RUN git clone -b v8.2.0 https://github.com/AcademySoftwareFoundation/openvdb.git \
+  && cd openvdb \
+  && mkdir build && cd build \
+  && cmake .. \
+  && make -j$(nproc)
+
 FROM ros:humble-ros-base-jammy AS builder
 
 ENV DEBIAN_FRONTEND noninteractive
@@ -106,6 +124,11 @@ RUN sudo apt-get update \
 
 COPY --from=cacher --chown=$USER:$GROUP /ws/ $USER_WS/
 
+COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/openvdb/ /opt/openvdb
+RUN cd /opt/openvdb/build \
+    && sudo make install \
+    && sudo rm -rf /opt/openvdb
+
 ENV WITHIN_DEV 1
 
 ENV SHELL /bin/bash