Add likelihood field 3d model version

beluga/CMakeLists.txt

@@ -18,6 +18,14 @@ project(beluga VERSION 1.0)
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
+option(
+  USE_OPENVDB
+  "Compiles with OpenVDB, required for the 3D likelihood field sensor model"
+  Off)
+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
@@ -48,6 +56,36 @@ find_package(range-v3 REQUIRED)
 find_package(Sophus REQUIRED)
 find_package(TBB REQUIRED)
 
+# If USE_OPENVDB is enabled, proceed with finding OpenVDB
+if(USE_OPENVDB)
+  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()
+endif()
+
 add_library(${PROJECT_NAME} INTERFACE)
 target_include_directories(
   ${PROJECT_NAME}
@@ -60,10 +98,12 @@ target_link_libraries(
             ${HDF5_CXX_LIBRARIES}
             Sophus::Sophus
             TBB::tbb
-            range-v3::range-v3)
+            range-v3::range-v3
+            $<$<BOOL:${USE_OPENVDB}>:OpenVDB::openvdb>)
 target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
-target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG
-                                                     SOPHUS_USE_BASIC_LOGGING)
+target_compile_definitions(
+  ${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING
+                            $<$<BOOL:${USE_OPENVDB}>:BELUGA_OPENVDB_SUPPORT>)
 
 add_executable(clang_tidy_findable)
 target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp)

beluga/cmake/Config.cmake.in

@@ -7,6 +7,11 @@ find_dependency(HDF5 COMPONENTS CXX REQUIRED)
 find_dependency(Sophus REQUIRED)
 find_dependency(TBB REQUIRED)
 
+if("@USE_OPENVDB@")
+    list(APPEND CMAKE_MODULE_PATH "@OPENVDB_CMAKE_MODULE_PATH@")
+    find_dependency(OpenVDB REQUIRED)
+endif()
+
 include("${CMAKE_CURRENT_LIST_DIR}/@[email protected]")
 
 set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@::@PROJECT_NAME@)

beluga/include/beluga/sensor.hpp

@@ -54,6 +54,7 @@
  *
  * \section SensorModelLinks See also
  * - beluga::LikelihoodFieldModel
+ * - beluga::Likelihood3DFieldModel
  * - beluga::BeamSensorModel
  * - beluga::LandmarkSensorModel
  * - beluga::BearingSensorModel
@@ -65,4 +66,8 @@
 #include <beluga/sensor/likelihood_field_model.hpp>
 #include <beluga/sensor/ndt_sensor_model.hpp>
 
+#ifdef BELUGA_OPENVDB_SUPPORT
+#include <beluga/sensor/likelihood_field_model3.hpp>
+#endif
+
 #endif

beluga/include/beluga/sensor/likelihood_field_model3.hpp

@@ -0,0 +1,168 @@
+// 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_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP
+#define BELUGA_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP
+
+#include <algorithm>
+#include <cmath>
+#include <random>
+#include <vector>
+
+#include <openvdb/openvdb.h>
+
+#include <Eigen/Core>
+
+#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 {
+
+/// 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 Likelihood3DFieldModel instance.
+  /**
+   * \param params Parameters to configure this instance.
+   *  See beluga::Likelihood3DFieldModelParam 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 {
+    const size_t pointcloud_size = measurement.points().size();
+    // Transform each point from the sensor frame to the origin frame
+    auto transformed_points = ranges::views::all(measurement.points()) |
+                              ranges::views::transform([&](const auto& point) {
+                                return measurement.origin() * point.template cast<double>();
+                              }) |
+                              ranges::to<std::vector>();
+
+    return [this, pointcloud_size, points = std::move(transformed_points)](const state_type& state) -> weight_type {
+      std::vector<float> distances;
+      distances.reserve(pointcloud_size);
+
+      // Transform each point to every particle state
+      std::transform(points.begin(), points.end(), std::back_inserter(distances), [this, state](const auto& point) {
+        // OpenVDB grid already in world coordinates
+        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()));
+        return accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_;
+      });
+
+      // Calculates the probality based on the distance
+      return std::transform_reduce(
+          distances.cbegin(), distances.cend(), 1.0, std::plus{}, [this](const auto& distance) {
+            return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_;
+          });
+    };
+  }
+
+ 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
+
+#endif

beluga/package.xml

@@ -15,7 +15,10 @@
   <buildtool_depend>cmake</buildtool_depend>
 
   <depend>eigen</depend>
+  <depend>libboost-iostreams-dev</depend>
+  <depend>libboost-thread-dev</depend>
   <depend>libhdf5-dev</depend>
+  <depend>libopenvdb-dev</depend>
   <depend>range-v3</depend>
   <depend>sophus</depend>
   <depend>tbb</depend>

beluga/test/beluga/CMakeLists.txt

@@ -16,6 +16,7 @@ file(COPY test_data DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
 
 add_executable(
   test_beluga
+  $<IF:$<BOOL:${USE_OPENVDB}>,sensor/test_likelihood_3d_field_model.cpp,>
   actions/test_assign.cpp
   actions/test_normalize.cpp
   actions/test_overlay.cpp

beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp

@@ -0,0 +1,45 @@
+// 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_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP
+#define BELUGA_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP
+
+#include <range/v3/view/iota.hpp>
+#include <sophus/se3.hpp>
+
+namespace beluga::testing {
+
+template <typename T>
+class SimpleSparsePointCloud3 {
+ public:
+  explicit SimpleSparsePointCloud3(std::vector<Eigen::Vector3<T>> points, Sophus::SE3d origin = Sophus::SE3d{})
+      : points_(std::move(points)), origin_(std::move(origin)) {}
+
+  /// Get the point cloud frame origin in the filter frame.
+  [[nodiscard]] const auto& origin() const { return origin_; }
+
+  /// Get the 3D points collection.
+  [[nodiscard]] const auto& points() const { return points_; }
+
+ private:
+  std::vector<Eigen::Vector3<T>> points_;
+  Sophus::SE3d origin_;
+};
+
+using SimpleSparsePointCloud3d = SimpleSparsePointCloud3<double>;
+using SimpleSparsePointCloud3f = SimpleSparsePointCloud3<float>;
+
+}  // namespace beluga::testing
+
+#endif