Add likelihood field 3d model version (#440)

### Proposed changes

_Describe the big picture of your changes here to communicate to the
maintainers why we should accept this pull request.
If it fixes a bug or resolves a feature request, be sure to link to that
issue._

#### Type of change

- [ ] 🐛 Bugfix (change which fixes an issue)
- [x] 🚀 Feature (change which adds functionality)
- [ ] 📚 Documentation (change which fixes or extends documentation)

💥 **Breaking change!** _Explain why a non-backwards compatible change is
necessary or remove this line entirely if not applicable._

### Checklist

_Put an `x` in the boxes that apply. This is simply a reminder of what
we will require before merging your code._

- [x] Lint and unit tests (if any) pass locally with my changes
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have added necessary documentation (if appropriate)
- [x] All commits have been signed for
[DCO](https://developercertificate.org/)

### Additional comments

_Anything worth mentioning to the reviewers._

---------

Signed-off-by: Pablo Vela <pablovela@naver.com>
Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com>

beluga_vdb/CMakeLists.txt

@@ -0,0 +1,131 @@
+# Copyright 2024 Ekumen, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.16)
+
+project(beluga_vdb VERSION 1.0)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+set(OPENVDB_CMAKE_MODULE_PATH
+    ${OPENVDB_CMAKE_MODULE_PATH}
+    CACHE PATH "Path to OpenVDB CMake module")
+
+if(NOT CMAKE_BUILD_TYPE)
+  message(STATUS "Setting build type to 'Release' as none was specified.")
+  set(CMAKE_BUILD_TYPE
+      "Release"
+      CACHE STRING "Build type" FORCE)
+endif()
+
+find_package(beluga REQUIRED)
+
+find_package(Eigen3 REQUIRED NO_MODULE)
+find_package(range-v3 REQUIRED)
+find_package(Sophus REQUIRED)
+
+if(NOT OPENVDB_CMAKE_MODULE_PATH)
+  file(
+    GLOB_RECURSE
+    OPENVDB_MODULES
+    /usr/lib/*/FindOpenVDB.cmake
+    /usr/local/lib/*/FindOpenVDB.cmake)
+  list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES)
+  if(NUM_OPENVDB_MODULES EQUAL 1)
+    list(
+      GET
+      OPENVDB_MODULES
+      0
+      OPENVDB_MODULE)
+    get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE}
+                           DIRECTORY)
+  endif()
+  unset(NUM_OPENVDB_MODULES)
+  unset(OPENVDB_MODULES)
+endif()
+
+if(OPENVDB_CMAKE_MODULE_PATH)
+  list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH})
+endif()
+
+find_package(OpenVDB REQUIRED)
+
+if(OPENVDB_CMAKE_MODULE_PATH)
+  list(POP_BACK CMAKE_MODULE_PATH)
+endif()
+
+add_library(${PROJECT_NAME} INTERFACE)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  target_compile_options(
+    ${PROJECT_NAME}
+    INTERFACE -Wall
+              -Wconversion
+              -Wextra
+              -Werror
+              -Wpedantic)
+endif()
+if(CMAKE_BUILD_TYPE MATCHES "Debug")
+  target_compile_options(${PROJECT_NAME} INTERFACE -fno-inline)
+endif()
+
+target_include_directories(
+  ${PROJECT_NAME}
+  INTERFACE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+            $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_link_libraries(${PROJECT_NAME} INTERFACE beluga::beluga OpenVDB::openvdb)
+target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
+target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG
+                                                     SOPHUS_USE_BASIC_LOGGING)
+
+add_executable(clang_tidy_findable)
+target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp)
+target_link_libraries(clang_tidy_findable PRIVATE ${PROJECT_NAME})
+
+option(BUILD_TESTING "Build the testing tree." ON)
+if(BUILD_TESTING)
+  message(STATUS "Build testing enabled.")
+  enable_testing()
+  add_subdirectory(test)
+endif()
+
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}Targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+set(INSTALL_CMAKEDIR ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cmake)
+
+install(
+  EXPORT ${PROJECT_NAME}Targets
+  FILE ${PROJECT_NAME}Targets.cmake
+  NAMESPACE ${PROJECT_NAME}::
+  DESTINATION ${INSTALL_CMAKEDIR})
+
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+  ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
+  INSTALL_DESTINATION ${INSTALL_CMAKEDIR})
+write_basic_package_version_file(
+  ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake
+  COMPATIBILITY SameMajorVersion)
+
+install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
+              "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+        DESTINATION ${INSTALL_CMAKEDIR})

beluga_vdb/cmake/Config.cmake.in

@@ -0,0 +1,42 @@
+@PACKAGE_INIT@
+
+include(CMakeFindDependencyMacro)
+find_dependency(Eigen3 REQUIRED NO_MODULE)
+find_dependency(range-v3 REQUIRED)
+find_dependency(Sophus REQUIRED)
+find_dependency(beluga REQUIRED)
+
+set(OPENVDB_CMAKE_MODULE_PATH
+    ${OPENVDB_CMAKE_MODULE_PATH}
+    CACHE PATH "Path to OpenVDB CMake module")
+
+if(NOT OPENVDB_CMAKE_MODULE_PATH)
+  file(
+    GLOB_RECURSE
+    OPENVDB_MODULES
+    /usr/lib/*/FindOpenVDB.cmake
+    /usr/local/lib/*/FindOpenVDB.cmake)
+  list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES)
+  if(NUM_OPENVDB_MODULES EQUAL 1)
+    list(GET OPENVDB_MODULES 0 OPENVDB_MODULE)
+    get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} DIRECTORY)
+  endif()
+  unset(NUM_OPENVDB_MODULES)
+  unset(OPENVDB_MODULES)
+endif()
+
+if(OPENVDB_CMAKE_MODULE_PATH)
+  list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH})
+endif()
+
+find_dependency(OpenVDB REQUIRED)
+
+if(OPENVDB_CMAKE_MODULE_PATH)
+  list(POP_BACK CMAKE_MODULE_PATH)
+endif()
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake")
+
+set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@::@PROJECT_NAME@)
+
+check_required_components($@PROJECT_NAME@)

beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp

@@ -0,0 +1,162 @@
+// Copyright 2024 Ekumen, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BELUGA_VDB_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP
+#define BELUGA_VDB_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP
+
+#include <algorithm>
+#include <cmath>
+#include <random>
+#include <vector>
+
+#include <openvdb/openvdb.h>
+
+#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>
+#include <range/v3/view/transform.hpp>
+#include <sophus/se3.hpp>
+#include <sophus/so3.hpp>
+
+/**
+ * \file
+ * \brief Implementation of a likelihood field sensor model for 3D Lidars.
+ */
+
+namespace beluga_vdb {
+
+/// Parameters used to construct a LikelihoodFieldModel3 instance.
+/**
+ * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3.
+ */
+struct LikelihoodFieldModel3Param {
+  /// Maximum distance to obstacle.
+  /**
+   * When creating a distance map, if the distance to an obstacle is higher than the value specified here,
+   * then this value will be used.
+   */
+  double max_obstacle_distance = 100.0;
+  /// Maximum range of a laser ray.
+  double max_laser_distance = 2.0;
+  /// Weight used to combine the probability of hitting an obstacle.
+  double z_hit = 0.5;
+  /// Weight used to combine the probability of random noise in perception.
+  double z_random = 0.5;
+  /// Standard deviation of a gaussian centered arounds obstacles.
+  /**
+   * Used to calculate the probability of the obstacle being hit.
+   */
+  double sigma_hit = 0.2;
+};
+
+/// Likelihood field sensor model for range finders.
+/**
+ * This model relies on a pre-computed likelihood map of the environment.
+ * It is less computationally intensive than the beluga::BeamSensorModel
+ * because no ray-tracing is required, and it can also provide better
+ * performance in environments with non-smooth occupation maps. See
+ * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4,
+ * for further reference.
+ *
+ * \note This class satisfies \ref SensorModelPage.
+ *
+ * \tparam OpenVDB grid type.
+ */
+template <typename GridT, typename PointCloud>
+class LikelihoodFieldModel3 {
+ public:
+  /// State type of a particle.
+  using state_type = Sophus::SE3d;
+
+  /// Weight type of the particle.
+  using weight_type = double;
+
+  /// Measurement type given by the interface.
+  using measurement_type = PointCloud;
+
+  /// Map representation type.
+  using map_type = GridT;
+
+  /// Parameter type that the constructor uses to configure the likelihood field model.
+  using param_type = LikelihoodFieldModel3Param;
+
+  /// Constructs a LikelihoodFieldModel3 instance.
+  /**
+   * \param params Parameters to configure this instance.
+   *  See beluga::LikelihoodFieldModel3Param for details.
+   * \param grid Narrow band Level set grid representing the static map that the sensor model
+   *  uses to compute a likelihood field for lidar hits and compute importance weights
+   *  for particle states.
+   *  Currently only supports OpenVDB Level sets.
+   */
+  explicit LikelihoodFieldModel3(const param_type& params, const map_type& grid)
+      : params_{params},
+        grid_{openvdb::gridPtrCast<map_type>(grid.deepCopyGrid())},
+        accessor_{grid_->getConstAccessor()},
+        transform_{grid_->transform()},
+        background_{grid_->background()},
+        two_squared_sigma_{2 * params.sigma_hit * params.sigma_hit},
+        amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants<double>::pi()))},
+        offset_{params.z_random / params.max_laser_distance} {
+    openvdb::initialize();
+    /// Pre-computed parameters
+    assert(two_squared_sigma_ > 0.0);
+    assert(amplitude_ > 0.0);
+  }
+
+  /// Returns a state weighting function conditioned on 3D lidar hits.
+  /**
+   * \param measurement 3D lidar measurement containing the hit points and the transform to the origin.
+   * \return a state weighting function satisfying \ref StateWeightingFunctionPage
+   *  and borrowing a reference to this sensor model (and thus their lifetime are bound).
+   */
+  [[nodiscard]] auto operator()(measurement_type&& measurement) const {
+    // Transform each point from the sensor frame to the origin frame
+    auto transformed_points = measurement.points() | ranges::views::transform([&](const auto& point) {
+                                return measurement.origin() * point.template cast<double>();
+                              }) |
+                              ranges::to<std::vector>();
+
+    return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type {
+      return ranges::fold_left(
+          points |  //
+              ranges::views::transform([this, &state](const auto& point) {
+                const Eigen::Vector3d point_in_state_frame = state * point;
+                const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered(
+                    openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()));
+                const auto distance = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_;
+                return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_;
+              }),
+          1.0, std::plus{});
+    };
+  }
+
+ private:
+  param_type params_;
+  const typename map_type::Ptr grid_;
+  const typename map_type::ConstAccessor accessor_;
+  const openvdb::math::Transform transform_;
+  const typename map_type::ValueType background_;
+  double two_squared_sigma_;
+  double amplitude_;
+  double offset_;
+};
+
+}  // namespace beluga_vdb
+
+#endif

beluga_vdb/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>beluga_vdb</name>
+  <version>1.0.0</version>
+
+  <description>A beluga extension to facilitate the use of OpenVDB.</description>
+
+  <maintainer email="glpuga@ekumenlabs.com">Gerardo Puga</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="nespinosa@ekumenlabs.com">Nahuel Espinosa</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>cmake</buildtool_depend>
+
+  <depend>beluga</depend>
+  <depend>eigen</depend>
+  <depend>libboost-iostreams-dev</depend>
+  <depend>libboost-thread-dev</depend>
+  <depend>libopenvdb-dev</depend>
+  <depend>range-v3</depend>
+  <depend>sophus</depend>
+  <depend>tbb</depend>
+
+  <test_depend>clang-format</test_depend>
+  <test_depend>clang-tidy</test_depend>
+  <test_depend>gtest</test_depend>
+  <test_depend>libgmock-dev</test_depend>
+
+  <doc_depend>doxygen</doc_depend>
+  <doc_depend>graphviz</doc_depend>
+  <doc_depend>texlive-latex-base</doc_depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+</package>