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 + +
+Shows the Beluga logo. +
+ +## 🌐 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). + +![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/). 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