diff --git a/DEVELOPING.md b/DEVELOPING.md
index 6f2c077d3..c6b99ab2b 100644
--- a/DEVELOPING.md
+++ b/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
diff --git a/README.md b/README.md
index a4ad2306a..8b5f5a163 100644
--- a/README.md
+++ b/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
diff --git a/beluga_vdb/README.md b/beluga_vdb/README.md
new file mode 100644
index 000000000..ed4ffd505
--- /dev/null
+++ b/beluga_vdb/README.md
@@ -0,0 +1,167 @@
+# Beluga VDB
+
+
+

+
+
+## 🌐 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
+#include
+
+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(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
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+
+int main()
+{
+ openvdb::initialize();
+
+ pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
+
+ // Open PCD file
+ if (pcl::io::loadPCDFile ("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).
+
+
+
+## 📦 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/).
diff --git a/beluga_vdb/images/57fe338a-3e7e-46b3-ad2f-17011d6d306a.png b/beluga_vdb/images/57fe338a-3e7e-46b3-ad2f-17011d6d306a.png
new file mode 100755
index 000000000..156f3c485
Binary files /dev/null and b/beluga_vdb/images/57fe338a-3e7e-46b3-ad2f-17011d6d306a.png differ
diff --git a/beluga_vdb/images/example_level_set_map.png b/beluga_vdb/images/example_level_set_map.png
new file mode 100755
index 000000000..8d977e981
Binary files /dev/null and b/beluga_vdb/images/example_level_set_map.png differ
diff --git a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp
index b841f2a75..ea4514d21 100644
--- a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp
+++ b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp
@@ -24,8 +24,6 @@
#include
-#include
-#include
#include
#include
#include
@@ -133,7 +131,7 @@ class LikelihoodFieldModel3 {
ranges::to();
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;
diff --git a/docker/images/humble/Dockerfile b/docker/images/humble/Dockerfile
index a61d25886..fa40e54c1 100644
--- a/docker/images/humble/Dockerfile
+++ b/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