Add likelihood 3D field sensor model

Signed-off-by: Pablo Vela <[email protected]>

beluga/include/beluga/sensor/likelihood_3d_field_model.hpp

@@ -20,14 +20,16 @@
 #include <random>
 #include <vector>
 
-#include <beluga/algorithm/distance_map.hpp>
-#include <beluga/sensor/data/occupancy_grid.hpp>
-#include <beluga/sensor/data/value_grid.hpp>
+#include <openvdb/openvdb.h>
+#include <openvdb/tools/VolumeToSpheres.h>
+
+#include <Eigen/Core>
+
 #include <range/v3/range/conversion.hpp>
 #include <range/v3/view/all.hpp>
 #include <range/v3/view/transform.hpp>
-#include <sophus/se2.hpp>
-#include <sophus/so2.hpp>
+#include <sophus/se3.hpp>
+#include <sophus/so3.hpp>
 
 /**
  * \file
@@ -73,89 +75,86 @@ struct Likelihood3DFieldModelParam {
  *
  * \note This class satisfies \ref SensorModelPage.
  *
- * \tparam OccupancyGrid Type representing an occupancy grid.
- *  It must satisfy \ref OccupancyGrid2Page.
+ * \tparam OpenVDB grid type.
  */
-template <typename T, class U>
+template <typename GridT, typename U>
 class Likelihood3DFieldModel {
  public:
   /// State type of a particle.
-  // planar movement but add Z
   using state_type = Sophus::SE3d;
 
   /// Weight type of the particle.
   using weight_type = double;
 
-  /// Measurement type of the sensor: a point cloud for the XXXXXXXXXXX.
+  /// Measurement type given by the interface.
   using measurement_type = U;
 
   /// Map representation type.
-  // openvdb, algo asi
-  using map_type = T;
+  using map_type = GridT;
 
   /// Parameter type that the constructor uses to configure the likelihood field model.
-  // nombre? Likelihood3DFieldModel?? el modelo es el mismo solo que los ptos son 3d
-  // pero el modelo sigue siendo 2D
-  using param_type = Likelihood3DFieldModel;
+  using param_type = Likelihood3DFieldModelParam;
 
   /// Constructs a Likelihood3DFieldModel instance.
   /**
    * \param params Parameters to configure this instance.
    *  See beluga::Likelihood3DFieldModelParam for details.
-   * \param grid Occupancy grid representing the static map that the sensor model
+   * \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 Likelihood3DFieldModel(const param_type& params, const map_type& map)
-      : params_{params}, likelihood_field_map_{std::move(map)} {
-    // encontrar el punto mas cercano usnado
-    // probar esto sacarlo dela funcion
-    // DualGridSmpler
-    looker.create(likelihood_field_map_);
+  explicit Likelihood3DFieldModel(const param_type& params, const map_type& grid)
+      : params_{params},
+        looker_{openvdb::tools::ClosestSurfacePoint<map_type>::create(grid)},
+        squared_sigma_{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} {
+    /// Pre-computed parameters
+    assert(squared_sigma_ > 0.0);
+    assert(amplitude_ > 0.0);
+    // Remember this OpenVDB DualGridSmpler
   }
 
-  /// Returns the likelihood field, constructed from the provided map.
-  [[nodiscard]] const auto& likelihood_field() const { return likelihood_field_map_; }
-
-  /// Returns a state weighting function conditioned on 2D lidar hits.
+  /// Returns a state weighting function conditioned on 3D lidar hits.
   /**
-   * \param points 2D lidar hit points in the reference frame of particle states.
+   * \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&& points) const {
-    return [this, points = std::move(points)](const state_type& state) -> weight_type {
-      // map already in world coordinates
-      const auto unknown_space_occupancy_prob = static_cast<float>(1. / params_.max_laser_distance);
-
+  [[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
+    return [this,
+            points = std::move(measurement.origin() * measurement.points())](const state_type& state) -> weight_type {
+      std::vector<float> nb_distances;
+      std::vector<openvdb::Vec3R> vdb_points;
+      vdb_points.resize(pointcloud_size);
+
+      // Transform each point to every particle state
+      std::transform(points.cbegin(), points.cend(), vdb_points.begin(), [this, state](const auto& point) {
+        // OpenVDB grid already in world coordinates
+        const Eigen::Vector3d point_in_state_frame = state * point;
+        return openvdb::Vec3R{point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z()};
+      });
+
+      // Extract the distance to the closest surface for each point
+      looker_.search(vdb_points, nb_distances);
+
+      // Calculates the probality based on the distance
       return std::transform_reduce(
-          points.cbegin(), points.cend(), 1.0, std::plus{},
-          [this, state, unknown_space_occupancy_prob](const auto& point) {
-            const auto result = state * point;
-
-            std::vector<float> distances;
-            std::vector<Vec3R> points;
-            // output list of closest surface point distances
-            // looker.search(points, distances);
-
-            return 0.1;
+          nb_distances.cbegin(), nb_distances.cend(), 1.0, std::plus{}, [this](const auto& distance) {
+            return amplitude_ * std::exp(-(distance * distance) / squared_sigma_) + offset_;
           });
     };
   }
 
-  /// Update the sensor model with a new occupancy grid map.
-  /**
-   * This method re-computes the underlying likelihood field.
-   *
-   * \param grid New occupancy grid representing the static map.
-   */
-  void update_map(const map_type& map) { likelihood_field_map_{std::move(map)}; }
-
  private:
   param_type params_;
-  T likelihood_field_map_;
-  Sophus::SE3d world_to_likelihood_field_map_transform_;
-  Openvdb::ClosestSurfacePoint<map_type> looker;
+  typename openvdb::tools::ClosestSurfacePoint<map_type>::Ptr looker_;
+  const double squared_sigma_;
+  const double amplitude_;
+  const double offset_;
 };
 
 }  // namespace beluga

beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp

@@ -18,22 +18,28 @@
 #include <utility>
 #include <vector>
 
+#include <openvdb/math/Mat.h>
 #include <openvdb/openvdb.h>
+#include <openvdb/tools/Diagnostics.h>
 #include <openvdb/tools/LevelSetSphere.h>
+#include <openvdb/tools/LevelSetTracker.h>
 #include <openvdb/tools/TopologyToLevelSet.h>
 
 #include <range/v3/range/conversion.hpp>
 #include <range/v3/view/transform.hpp>
 #include <sophus/common.hpp>
 
+#include "beluga/sensor/likelihood_3d_field_model.hpp"
+#include "beluga/test/simple_pointcloud_interface.hpp"
+
 namespace {
 
-template <typename T, typename TT>
-auto make_map(const double voxel_size, const std::vector<TT>& world_points) {
+template <typename GridT, typename T>
+auto make_map(const double voxel_size, const std::vector<T>& world_points) {
   // Create the grid
-  T::Ptr grid = openvdb::tools::createLevelSet<T>(voxel_size);
+  typename GridT::Ptr grid = openvdb::createLevelSet<GridT>(voxel_size);
   // Get an accessor for coordinate-based access to voxels
-  const T::Accessor accessor = grid->getAccessor();
+  typename GridT::Accessor accessor = grid->getAccessor();
 
   // Fill the grid
   const openvdb::math::Transform transform;
@@ -48,16 +54,17 @@ auto make_map(const double voxel_size, const std::vector<TT>& world_points) {
 
   // Prune the grid
   // Set voxels that are outside the narrow band to the background value and prune the grid
-  openvdb::tools::LevelSetTracker<T> pruner(grid);
-  pruner.setTrimming(openvdb::TrimMode::kAll);
+  openvdb::tools::LevelSetTracker<GridT> pruner(*grid);
+  pruner.setTrimming(openvdb::tools::lstrack::TrimMode::kAll);
   pruner.prune();
 
   // Check grid
-  openvdb::tools::CheckLevelSet checker(grid);
+  openvdb::tools::CheckLevelSet<GridT> checker(*grid);
   // Check inactive values have a magnitude equal to the background value
   assert(checker.checkInactiveValues() == "");
 
   // Associate metadata
+  // just a terminal command to remember
   // vdb_print -l pcdgrid.vdb
   grid->setName("map");
   grid->setGridClass(openvdb::GridClass::GRID_LEVEL_SET);
@@ -66,19 +73,23 @@ auto make_map(const double voxel_size, const std::vector<TT>& world_points) {
   return grid;
 }
 
-TEST(Likelihood3DFieldModel, LikelihoodField) {
+TEST(Likelihood3DFieldModel, Likelihood3DField) {
   openvdb::initialize();
   // Create Grid
   const double voxel_size = 0.07;
-  const std::vector<openvdb::Vec3f> world_points{(1.0F, 1.0F, 1.0F)};
-  auto map = make_map<openvdb::FloatGrid, openvdb::Vec3f>(voxel_size, world_points);
+  const std::vector<openvdb::math::Vec3s> world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)};
+  auto map = make_map<openvdb::FloatGrid, openvdb::math::Vec3s>(voxel_size, world_points);
 
-  const auto params = beluga::LikelihoodFieldModelParam{0.07, 2.0, 20.0, 0.5, 0.5, 0.2};
-  auto sensor_model = beluga::Likelihood3DFieldModel<openvdb::FloatGrid, beluga_ros::SparsePointCloud3>{params, map};
+  const auto params = beluga::Likelihood3DFieldModelParam{voxel_size, 2.0, 20.0, 0.5, 0.5, 0.2};
+  const auto pointcloud_measurement =
+      beluga::testing::SparsePointCloud3{std::vector<Eigen::Vector3<float>>{{1.0F, 1.0F, 1.0F}}};
+  auto sensor_model =
+      beluga::Likelihood3DFieldModel<openvdb::FloatGrid, beluga::testing::SparsePointCloud3<float>>{params, *map};
 
+  auto asd = sensor_model(std::move(pointcloud_measurement));
   /*ASSERT_THAT(
-      sensor_model.likelihood_field().data(),
-      testing::Pointwise(testing::DoubleNear(0.003), expected_cubed_likelihood));*/
+      sensor_model(pointcloud_measurement),
+      testing::Pointwise(testing::DoubleNear(0.003), 1.0));*/
 }
 
 }  // namespace