From 0d05297d843aae46394c8fc5c137b389409a070f Mon Sep 17 00:00:00 2001
From: Nahuel Espinosa <nespinosa@ekumenlabs.com>
Date: Sun, 27 Oct 2024 19:14:47 -0300
Subject: [PATCH 1/2] Clean up NDT base implementation and examples

Signed-off-by: Nahuel Espinosa <nespinosa@ekumenlabs.com>
---
 beluga/include/beluga/algorithm.hpp           |   1 +
 beluga/include/beluga/algorithm/amcl_core.hpp |  95 +++------
 .../include/beluga/sensor/data/ndt_cell.hpp   |  22 +--
 .../beluga/sensor/ndt_sensor_model.hpp        |  56 +++---
 .../test/beluga/algorithm/test_amcl_core.cpp  |  61 +-----
 .../test/beluga/sensor/data/test_ndt_cell.cpp |  12 +-
 beluga/test/beluga/sensor/test_ndt_model.cpp  |  48 ++---
 beluga_amcl/include/beluga_amcl/amcl_node.hpp |  21 +-
 .../include/beluga_amcl/ndt_amcl_node.hpp     |  84 ++------
 .../include/beluga_amcl/ndt_amcl_node_3d.hpp  |  67 ++-----
 beluga_amcl/src/amcl_node.cpp                 |  47 +----
 beluga_amcl/src/ndt_amcl_node.cpp             | 183 +++++-------------
 beluga_amcl/src/ndt_amcl_node_3d.cpp          | 164 +++++-----------
 beluga_amcl/test/test_ndt_amcl_3d_node.cpp    |  22 +--
 beluga_amcl/test/test_ndt_amcl_node.cpp       |  34 +---
 beluga_amcl/test/test_utils/node_testing.hpp  |  15 +-
 beluga_example/params/default_ndt.ros2.yaml   |  11 --
 .../params/default_ndt_3d.ros2.yaml           |  11 --
 beluga_ros/src/amcl.cpp                       |   2 +-
 19 files changed, 262 insertions(+), 694 deletions(-)

diff --git a/beluga/include/beluga/algorithm.hpp b/beluga/include/beluga/algorithm.hpp
index e43f32c9d..dc2f0b699 100644
--- a/beluga/include/beluga/algorithm.hpp
+++ b/beluga/include/beluga/algorithm.hpp
@@ -20,6 +20,7 @@
  * \brief Includes all beluga algorithms.
  */
 
+#include <beluga/algorithm/amcl_core.hpp>
 #include <beluga/algorithm/cluster_based_estimation.hpp>
 #include <beluga/algorithm/distance_map.hpp>
 #include <beluga/algorithm/effective_sample_size.hpp>
diff --git a/beluga/include/beluga/algorithm/amcl_core.hpp b/beluga/include/beluga/algorithm/amcl_core.hpp
index 63f6ac191..6a6fbc9cd 100644
--- a/beluga/include/beluga/algorithm/amcl_core.hpp
+++ b/beluga/include/beluga/algorithm/amcl_core.hpp
@@ -21,12 +21,25 @@
 #include <utility>
 #include <vector>
 
-#include <beluga/beluga.hpp>
-
-#include <range/v3/range/concepts.hpp>
-#include <range/v3/view/any_view.hpp>
+#include <range/v3/range/conversion.hpp>
 #include <range/v3/view/take_exactly.hpp>
-#include "beluga/policies/on_motion.hpp"
+
+#include <beluga/actions/assign.hpp>
+#include <beluga/actions/normalize.hpp>
+#include <beluga/actions/propagate.hpp>
+#include <beluga/actions/reweight.hpp>
+#include <beluga/algorithm/estimation.hpp>
+#include <beluga/algorithm/spatial_hash.hpp>
+#include <beluga/containers/circular_array.hpp>
+#include <beluga/containers/tuple_vector.hpp>
+#include <beluga/policies/every_n.hpp>
+#include <beluga/policies/on_effective_size_drop.hpp>
+#include <beluga/policies/on_motion.hpp>
+#include <beluga/primitives.hpp>
+#include <beluga/random/multivariate_normal_distribution.hpp>
+#include <beluga/views/random_intersperse.hpp>
+#include <beluga/views/sample.hpp>
+#include <beluga/views/take_while_kld.hpp>
 
 namespace beluga {
 
@@ -44,10 +57,6 @@ struct AmclParams {
   std::size_t min_particles = 500UL;
   /// Maximum number of particles in the filter at any point in time.
   std::size_t max_particles = 2000UL;
-  /// Used as part of the recovery mechanism when considering how many random particles to insert.
-  double alpha_slow = 0.001;
-  /// Used as part of the recovery mechanism when considering how many random particles to insert.
-  double alpha_fast = 0.1;
   /// Used as part of the kld sampling mechanism.
   double kld_epsilon = 0.05;
   /// Used as part of the kld sampling mechanism.
@@ -58,37 +67,19 @@ struct AmclParams {
 /**
  * \tparam MotionModel Class representing a motion model. Must satisfy \ref MotionModelPage.
  * \tparam SensorModel Class representing a sensor model. Must satisfy \ref SensorModelPage.
- * \tparam RandomStateGenerator A callable able to produce random states, optionally based on the current particles
- * state.
- * Class 'T' is a valid RandomStateGenerator if given 't' a possibly const instance of 'T' any of the following
- * conditions are true:
- * - t can be called without arguments returning an instance of 'SensorModel::state_type'
- *   representing a random state.
- * - t can be called with `const beluga::TupleVector<ParticleType>&> returning a callable
- *   that can be called without arguments and return an instance of 'SensorModel::state_type'.
- * \tparam WeightT Type to represent a weight of a particle.
  * \tparam ParticleType Full particle type, containing state, weight and possibly
  * other information .
- * \tparam ExecutionPolicy Execution policy for particles processing.
  */
 template <
     class MotionModel,
     class SensorModel,
-    class RandomStateGenerator,
-    typename WeightT = beluga::Weight,
-    class ParticleType = std::tuple<typename SensorModel::state_type, WeightT>,
-    class ExecutionPolicy = std::execution::sequenced_policy>
+    class Particle = std::tuple<typename SensorModel::state_type, beluga::Weight>>
 class Amcl {
-  static_assert(
-      std::is_same_v<ExecutionPolicy, std::execution::parallel_policy> or
-      std::is_same_v<ExecutionPolicy, std::execution::sequenced_policy>);
-
-  using particle_type = ParticleType;
+  using particle_type = Particle;
   using measurement_type = typename SensorModel::measurement_type;
   using state_type = typename SensorModel::state_type;
   using map_type = typename SensorModel::map_type;
   using spatial_hasher_type = spatial_hash<state_type>;
-  using random_state_generator_type = RandomStateGenerator;
   using estimation_type = std::invoke_result_t<beluga::detail::estimate_fn, std::vector<state_type>>;
 
  public:
@@ -96,28 +87,20 @@ class Amcl {
   /**
    * \param motion_model Motion model instance.
    * \param sensor_model Sensor model Instance.
-   * \param random_state_generator A callable able to produce random states, optionally based on the current particles
-   * state.
    * \param spatial_hasher A spatial hasher instance capable of computing a hash out of a particle state.
    * \param params Parameters for AMCL implementation.
-   * \param execution_policy Policy to use when processing particles.
    */
   Amcl(
       MotionModel motion_model,
       SensorModel sensor_model,
-      RandomStateGenerator random_state_generator,
       spatial_hasher_type spatial_hasher,
-      const AmclParams& params = AmclParams{},
-      ExecutionPolicy execution_policy = std::execution::seq)
+      const AmclParams& params = AmclParams{})
       : params_{params},
         motion_model_{std::move(motion_model)},
         sensor_model_{std::move(sensor_model)},
-        execution_policy_{std::move(execution_policy)},
         spatial_hasher_{std::move(spatial_hasher)},
-        random_probability_estimator_{params_.alpha_slow, params_.alpha_fast},
         update_policy_{beluga::policies::on_motion<state_type>(params_.update_min_d, params_.update_min_a)},
-        resample_policy_{beluga::policies::every_n(params_.resample_interval)},
-        random_state_generator_(std::move(random_state_generator)) {
+        resample_policy_{beluga::policies::every_n(params_.resample_interval)} {
     if (params_.selective_resampling) {
       resample_policy_ = resample_policy_ && beluga::policies::on_effective_size_drop;
     }
@@ -141,9 +124,9 @@ class Amcl {
    * \tparam CovarianceT type representing a covariance, compliant with state_type.
    * \throw std::runtime_error If the provided covariance is invalid.
    */
-  template <class CovarianceT>
-  void initialize(state_type pose, CovarianceT covariance) {
-    initialize(beluga::MultivariateNormalDistribution{pose, covariance});
+  template <class Covariance>
+  void initialize(state_type pose, Covariance covariance) {
+    initialize(beluga::MultivariateNormalDistribution{std::move(pose), std::move(covariance)});
   }
 
   /// Update the map used for localization.
@@ -171,22 +154,12 @@ class Amcl {
       return std::nullopt;
     }
 
-    particles_ |= beluga::actions::propagate(
-                      execution_policy_, motion_model_(control_action_window_ << std::move(control_action))) |  //
-                  beluga::actions::reweight(execution_policy_, sensor_model_(std::move(measurement))) |         //
-                  beluga::actions::normalize(execution_policy_);
-
-    const double random_state_probability = random_probability_estimator_(particles_);
+    particles_ |= beluga::actions::propagate(motion_model_(control_action_window_ << std::move(control_action))) |  //
+                  beluga::actions::reweight(sensor_model_(std::move(measurement))) |                                //
+                  beluga::actions::normalize;
 
     if (resample_policy_(particles_)) {
-      auto random_state = ranges::compose(beluga::make_from_state<particle_type>, get_random_state_generator());
-
-      if (random_state_probability > 0.0) {
-        random_probability_estimator_.reset();
-      }
-
       particles_ |= beluga::views::sample |
-                    beluga::views::random_intersperse(std::move(random_state), random_state_probability) |
                     beluga::views::take_while_kld(
                         spatial_hasher_,        //
                         params_.min_particles,  //
@@ -204,29 +177,17 @@ class Amcl {
   void force_update() { force_update_ = true; }
 
  private:
-  /// Gets a callable that will produce a random state.
-  [[nodiscard]] decltype(auto) get_random_state_generator() const {
-    if constexpr (std::is_invocable_v<random_state_generator_type>) {
-      return random_state_generator_;
-    } else {
-      return random_state_generator_(particles_);
-    }
-  }
   beluga::TupleVector<particle_type> particles_;
 
   AmclParams params_;
 
   MotionModel motion_model_;
   SensorModel sensor_model_;
-  ExecutionPolicy execution_policy_;
 
   spatial_hasher_type spatial_hasher_;
-  beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_;
   beluga::any_policy<state_type> update_policy_;
   beluga::any_policy<decltype(particles_)> resample_policy_;
 
-  random_state_generator_type random_state_generator_;
-
   beluga::RollingWindow<state_type, 2> control_action_window_;
 
   bool force_update_{true};
diff --git a/beluga/include/beluga/sensor/data/ndt_cell.hpp b/beluga/include/beluga/sensor/data/ndt_cell.hpp
index 76059ad2d..4d82fb35a 100644
--- a/beluga/include/beluga/sensor/data/ndt_cell.hpp
+++ b/beluga/include/beluga/sensor/data/ndt_cell.hpp
@@ -33,7 +33,7 @@ namespace beluga {
 
 /// Representation for a cell of a N dimensional NDT cell.
 template <int NDim, typename Scalar = double>
-struct NDTCell {
+struct NdtCell {
   static_assert(std::is_floating_point_v<Scalar>, "Scalar template parameter should be a floating point.");
   /// Number of dimensions of the cell's translation.
   static constexpr int num_dim = NDim;
@@ -46,7 +46,7 @@ struct NDTCell {
 
   /// Get the L2 likelihood at measurement, scaled by d1 and d2. It assumes the measurement is pre-transformed
   /// into the same frame as this cell instance.
-  [[nodiscard]] double likelihood_at(const NDTCell& measurement, double d1 = 1.0, double d2 = 1.0) const {
+  [[nodiscard]] double likelihood_at(const NdtCell& measurement, double d1 = 1.0, double d2 = 1.0) const {
     const Eigen::Vector<Scalar, NDim> error = measurement.mean - mean;
     const double rhs =
         std::exp((-d2 / 2.0) * error.transpose() * (measurement.covariance + covariance).inverse() * error);
@@ -54,36 +54,36 @@ struct NDTCell {
   }
 
   /// Ostream overload mostly for debugging purposes.
-  friend std::ostream& operator<<(std::ostream& os, const NDTCell& cell) {
+  friend std::ostream& operator<<(std::ostream& os, const NdtCell& cell) {
     os << "Mean \n" << cell.mean.transpose() << " \n\nCovariance: \n" << cell.covariance;
     return os;
   }
 
   /// Transform the normal distribution according to tf, both mean and covariance.
-  friend NDTCell operator*(const Sophus::SE2<scalar_type>& tf, const NDTCell& ndt_cell) {
+  friend NdtCell operator*(const Sophus::SE2<scalar_type>& tf, const NdtCell& ndt_cell) {
     static_assert(num_dim == 2, "Cannot transform a non 2D NDT Cell with a SE2 transform.");
     const Eigen::Vector2d uij = tf * ndt_cell.mean;
     const Eigen::Matrix2Xd cov = tf.so2().matrix() * ndt_cell.covariance * tf.so2().matrix().transpose();
-    return NDTCell{uij, cov};
+    return NdtCell{uij, cov};
   }
 
   /// Transform the normal distribution according to tf, both mean and covariance.
-  friend NDTCell operator*(const Sophus::SE3<scalar_type>& tf, const NDTCell& ndt_cell) {
+  friend NdtCell operator*(const Sophus::SE3<scalar_type>& tf, const NdtCell& ndt_cell) {
     static_assert(num_dim == 3, "Cannot transform a non 3D NDT Cell with a SE3 transform.");
     const Eigen::Vector3d uij = tf * ndt_cell.mean;
     const Eigen::Matrix3Xd cov = tf.so3().matrix() * ndt_cell.covariance * tf.so3().matrix().transpose();
-    return NDTCell{uij, cov};
+    return NdtCell{uij, cov};
   }
 };
 
 /// Convenience alias for a 2D NDT cell with double representation.
-using NDTCell2d = NDTCell<2, double>;
+using NdtCell2d = NdtCell<2, double>;
 /// Convenience alias for a 2D NDT cell with float representation.
-using NDTCell2f = NDTCell<2, float>;
+using NdtCell2f = NdtCell<2, float>;
 /// Convenience alias for a 3D NDT cell with double representation.
-using NDTCell3d = NDTCell<3, double>;
+using NdtCell3d = NdtCell<3, double>;
 /// Convenience alias for a 3D NDT cell with float representation.
-using NDTCell3f = NDTCell<3, float>;
+using NdtCell3f = NdtCell<3, float>;
 
 }  // namespace beluga
 
diff --git a/beluga/include/beluga/sensor/ndt_sensor_model.hpp b/beluga/include/beluga/sensor/ndt_sensor_model.hpp
index af5d9f54d..6534023b3 100644
--- a/beluga/include/beluga/sensor/ndt_sensor_model.hpp
+++ b/beluga/include/beluga/sensor/ndt_sensor_model.hpp
@@ -32,9 +32,9 @@
 #include <sophus/se3.hpp>
 #include <sophus/so2.hpp>
 
+#include <beluga/eigen_compatibility.hpp>
 #include <beluga/sensor/data/ndt_cell.hpp>
 #include <beluga/sensor/data/sparse_value_grid.hpp>
-#include "beluga/eigen_compatibility.hpp"
 
 namespace beluga {
 
@@ -57,7 +57,7 @@ struct CellHasher {
 
 /// Fit a vector of points to an NDT cell, by computing its mean and covariance.
 template <int NDim, typename Scalar = double>
-inline NDTCell<NDim, Scalar> fit_points(const std::vector<Eigen::Vector<Scalar, NDim>>& points) {
+inline NdtCell<NDim, Scalar> fit_points(const std::vector<Eigen::Vector<Scalar, NDim>>& points) {
   static constexpr double kMinVariance = 1e-5;
   Eigen::Map<const Eigen::Matrix<Scalar, NDim, Eigen::Dynamic>> points_view(
       reinterpret_cast<const Scalar*>(points.data()), NDim, static_cast<int64_t>(points.size()));
@@ -71,14 +71,14 @@ inline NDTCell<NDim, Scalar> fit_points(const std::vector<Eigen::Vector<Scalar,
   if constexpr (NDim == 3) {
     cov(2, 2) = std::max(cov(2, 2), kMinVariance);
   }
-  return NDTCell<NDim, Scalar>{mean, cov};
+  return NdtCell<NDim, Scalar>{mean, cov};
 }
 
 /// Given a number of N dimensional points and a resolution, constructs a vector of NDT cells, by clusterizing the
 /// points using 'resolution' and fitting a normal distribution to each of the resulting clusters if they contain a
 /// minimum number of points in them.
 template <int NDim, typename Scalar = double>
-inline std::vector<NDTCell<NDim, Scalar>> to_cells(
+inline std::vector<NdtCell<NDim, Scalar>> to_cells(
     const std::vector<Eigen::Vector<Scalar, NDim>>& points,
     const double resolution) {
   static constexpr int kMinPointsPerCell = 5;
@@ -86,7 +86,7 @@ inline std::vector<NDTCell<NDim, Scalar>> to_cells(
   const Eigen::Map<const Eigen::Matrix<Scalar, NDim, Eigen::Dynamic>> points_view(
       reinterpret_cast<const Scalar*>(points.data()), NDim, static_cast<int64_t>(points.size()));
 
-  std::vector<NDTCell<NDim, Scalar>> ret;
+  std::vector<NdtCell<NDim, Scalar>> ret;
   ret.reserve(static_cast<size_t>(points_view.cols()) / kMinPointsPerCell);
 
   std::unordered_map<Eigen::Vector<int, NDim>, std::vector<Eigen::Vector<Scalar, NDim>>, CellHasher<NDim>> cell_grid;
@@ -142,9 +142,9 @@ get_default_neighbors_kernel() {
 
 }  // namespace detail
 
-/// Parameters used to construct a NDTSensorModel instance.
+/// Parameters used to construct a NdtSensorModel instance.
 template <int NDim>
-struct NDTModelParam {
+struct NdtModelParam {
   static_assert(NDim == 2 or NDim == 3, "Only 2D or 3D is supported for the NDT sensor model.");
   /// Likelihood for measurements that lie inside cells that are not present in the map.
   double minimum_likelihood = 0;
@@ -158,10 +158,10 @@ struct NDTModelParam {
 };
 
 /// Convenience alias for a 2d parameters struct for the NDT sensor model.
-using NDTModelParam2d = NDTModelParam<2>;
+using NdtModelParam2d = NdtModelParam<2>;
 
 /// Convenience alias for a 3d parameters struct for the NDT sensor model.
-using NDTModelParam3d = NDTModelParam<3>;
+using NdtModelParam3d = NdtModelParam<3>;
 /// NDT sensor model for range finders.
 /**
  * This class satisfies \ref SensorModelPage.
@@ -169,7 +169,7 @@ using NDTModelParam3d = NDTModelParam<3>;
  * \tparam SparseGridT Type representing a sparse NDT grid, as a specialization of 'beluga::SparseValueGrid'.
  */
 template <typename SparseGridT>
-class NDTSensorModel {
+class NdtSensorModel {
  public:
   /// NDT Cell type.
   using ndt_cell_type = typename SparseGridT::mapped_type;
@@ -189,16 +189,16 @@ class NDTSensorModel {
   /// Map representation type.
   using map_type = SparseGridT;
   /// Parameter type that the constructor uses to configure the NDT sensor model.
-  using param_type = NDTModelParam<ndt_cell_type::num_dim>;
+  using param_type = NdtModelParam<ndt_cell_type::num_dim>;
 
-  /// Constructs a NDTSensorModel instance.
+  /// Constructs a NdtSensorModel instance.
   /**
    * \param params Parameters to configure this instance.
-   *  See beluga::NDTModelParam for details.
+   *  See beluga::NdtModelParam for details.
    * \param cells_data Sparse grid representing an NDT map, used for calculating importance weights for the different
    * particles.
    */
-  NDTSensorModel(param_type params, SparseGridT cells_data)
+  NdtSensorModel(param_type params, SparseGridT cells_data)
       : params_{std::move(params)}, cells_data_{std::move(cells_data)} {
     assert(params_.minimum_likelihood >= 0);
   }
@@ -233,8 +233,8 @@ class NDTSensorModel {
   }
 
  private:
-  const param_type params_;
-  const SparseGridT cells_data_;
+  param_type params_;
+  SparseGridT cells_data_;
 };
 
 namespace io {
@@ -247,18 +247,18 @@ namespace io {
 ///   of the cell.
 /// - "covariances": (NUM_CELLS, 2 / 3, 2 / 3) Contains the covariance for each cell.
 ///
-///  \tparam NDTMapRepresentation A specialized SparseValueGrid (see sensor/data/sparse_value_grid.hpp), where
-///  mapped_type == NDTCell2d / NDTCell3d, that will represent the NDT map as a mapping from 2D / 3D cells to NDTCells.
-template <typename NDTMapRepresentationT>
-NDTMapRepresentationT load_from_hdf5(const std::filesystem::path& path_to_hdf5_file) {
+///  \tparam NdtMap A specialized SparseValueGrid (see sensor/data/sparse_value_grid.hpp), where
+///  mapped_type == NdtCell2d / NdtCell3d, that will represent the NDT map as a mapping from 2D / 3D cells to NdtCells.
+template <typename NdtMap>
+NdtMap load_from_hdf5(const std::filesystem::path& path_to_hdf5_file) {
   static_assert(
-      std::is_same_v<typename NDTMapRepresentationT::mapped_type, NDTCell2d> or
-      std::is_same_v<typename NDTMapRepresentationT::mapped_type, NDTCell3d>);
+      std::is_same_v<typename NdtMap::mapped_type, NdtCell2d> or
+      std::is_same_v<typename NdtMap::mapped_type, NdtCell3d>);
   static_assert(
-      std::is_same_v<typename NDTMapRepresentationT::key_type, Eigen::Vector2i> or
-      std::is_same_v<typename NDTMapRepresentationT::key_type, Eigen::Vector3i>);
+      std::is_same_v<typename NdtMap::key_type, Eigen::Vector2i> or
+      std::is_same_v<typename NdtMap::key_type, Eigen::Vector3i>);
 
-  constexpr int kNumDim = NDTMapRepresentationT::key_type::RowsAtCompileTime;
+  constexpr int kNumDim = NdtMap::key_type::RowsAtCompileTime;
   if (!std::filesystem::exists(path_to_hdf5_file) || std::filesystem::is_directory(path_to_hdf5_file)) {
     std::stringstream ss;
     ss << "Couldn't find a valid HDF5 file at " << path_to_hdf5_file;
@@ -293,15 +293,15 @@ NDTMapRepresentationT load_from_hdf5(const std::filesystem::path& path_to_hdf5_f
 
   resolution_dataset.read(&resolution, H5::PredType::NATIVE_DOUBLE);
 
-  typename NDTMapRepresentationT::map_type map{};
+  typename NdtMap::map_type map{};
 
   // Note: Ranges::zip_view doesn't seem to work in old Eigen.
   for (size_t i = 0; i < covariances.size(); ++i) {
     map[cells_matrix.col(static_cast<Eigen::Index>(i))] =
-        NDTCell<kNumDim, double>{means_matrix.col(static_cast<Eigen::Index>(i)), covariances.at(i)};
+        NdtCell<kNumDim, double>{means_matrix.col(static_cast<Eigen::Index>(i)), covariances.at(i)};
   }
 
-  return NDTMapRepresentationT{std::move(map), resolution};
+  return NdtMap{std::move(map), resolution};
 }
 
 }  // namespace io
diff --git a/beluga/test/beluga/algorithm/test_amcl_core.cpp b/beluga/test/beluga/algorithm/test_amcl_core.cpp
index 577609421..5d6a557f7 100644
--- a/beluga/test/beluga/algorithm/test_amcl_core.cpp
+++ b/beluga/test/beluga/algorithm/test_amcl_core.cpp
@@ -33,6 +33,7 @@
 namespace {
 
 const auto kDummyControl = Sophus::SE2d{};
+
 const std::vector kDummyMeasurement = {
     std::make_pair(0.0, 0.0),
     std::make_pair(0.0, 0.0),
@@ -52,24 +53,21 @@ auto make_amcl(const beluga::AmclParams& params = {}) {
   // clang-format on
   const beluga::BeamModelParam param{};
 
-  auto random_state_maker = []() { return Sophus::SE2d{}; };
-
   beluga::spatial_hash<Sophus::SE2d> hasher{0.1, 0.1, 0.1};
 
   beluga::Amcl amcl{
       beluga::DifferentialDriveModel{beluga::DifferentialDriveModelParam{}},  //
       beluga::BeamSensorModel{param, map},                                    //
-      std::move(random_state_maker),
       std::move(hasher),
-      std::move(params),  //
-      std::execution::seq,
+      std::move(params),
   };
   return amcl;
 }
+
 }  // namespace
 
 namespace beluga {
-using beluga::testing::StaticOccupancyGrid;
+
 TEST(TestAmclCore, InitializeWithNoParticles) {
   auto amcl = make_amcl();
   ASSERT_EQ(amcl.particles().size(), 0);
@@ -124,43 +122,6 @@ TEST(TestAmclCore, UpdateWithParticlesForced) {
   ASSERT_TRUE(estimate.has_value());
 }
 
-TEST(TestAmclCore, ParticlesDependentRandomStateGenerator) {
-  auto params = beluga::AmclParams{};
-  params.max_particles = AmclParams{}.max_particles;
-
-  constexpr double kResolution = 0.5;
-  // clang-format off
-  const auto map = beluga::testing::StaticOccupancyGrid<5, 5>{{
-    false, false, false, false, false ,
-    false, false, false, false , false,
-    false, false, false , false, false,
-    false, false , false, false, false,
-    false , false, false, false, false},
-    kResolution};
-  // clang-format on
-
-  // Demonstrate how we can provide a generator that depends on the current filter state.
-  auto random_state_maker = [](const auto& particles) {
-    const auto last_particle_state = beluga::views::states(particles).back();
-    return [last_particle_state]() { return last_particle_state; };
-  };
-
-  beluga::spatial_hash<Sophus::SE2d> hasher{0.5, 0.5, 0.5};
-
-  beluga::Amcl amcl{
-      beluga::DifferentialDriveModel{beluga::DifferentialDriveModelParam{}},   //
-      beluga::LikelihoodFieldModel{beluga::LikelihoodFieldModelParam{}, map},  //
-      std::move(random_state_maker),
-      std::move(hasher),
-      std::move(params),  //
-      std::execution::seq,
-  };
-  amcl.initialize(Sophus::SE2d{}, Eigen::Vector3d::Ones().asDiagonal());
-  ASSERT_EQ(amcl.particles().size(), AmclParams{}.max_particles);
-  auto estimate = amcl.update(kDummyControl, kDummyMeasurement);
-  ASSERT_TRUE(estimate.has_value());
-}
-
 TEST(TestAmclCore, SelectiveResampleCanBeConstructed) {
   auto params = beluga::AmclParams{};
   params.selective_resampling = true;
@@ -171,18 +132,4 @@ TEST(TestAmclCore, SelectiveResampleCanBeConstructed) {
   ASSERT_TRUE(estimate.has_value());
 }
 
-TEST(TestAmclCore, TestRandomParticlesInserting) {
-  auto params = beluga::AmclParams{};
-  params.min_particles = 2;
-  params.max_particles = 100;
-  params.alpha_slow = 0.0;
-  params.alpha_fast = 100.0;  // Ensure we exercise random state generation
-  auto amcl = make_amcl(params);
-  amcl.initialize(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{1, 1}}, (Eigen::Vector3d::Ones()).asDiagonal());
-  for (int i = 0; i < 30; ++i) {
-    amcl.force_update();
-    amcl.update(kDummyControl, kDummyMeasurement);
-  }
-}
-
 }  // namespace beluga
diff --git a/beluga/test/beluga/sensor/data/test_ndt_cell.cpp b/beluga/test/beluga/sensor/data/test_ndt_cell.cpp
index 3d9f3ca6e..03f5ec2ed 100644
--- a/beluga/test/beluga/sensor/data/test_ndt_cell.cpp
+++ b/beluga/test/beluga/sensor/data/test_ndt_cell.cpp
@@ -32,23 +32,23 @@ Eigen::Matrix2Xd get_diagonal_covariance(double x_var = 0.5, double y_var = 0.8)
 
 }  // namespace
 
-TEST(NDTCellTests, Product) {
+TEST(NdtCellTests, Product) {
   {
-    const NDTCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
+    const NdtCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
     const Sophus::SE2d tf{};
     const auto transformed = tf * cell;
     ASSERT_TRUE(cell.mean.isApprox(transformed.mean));
     ASSERT_TRUE(cell.covariance.isApprox(transformed.covariance));
   }
   {
-    const NDTCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
+    const NdtCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
     const Sophus::SE2d tf{Sophus::SO2d{}, Eigen::Vector2d{1, 2}};
     const auto transformed = tf * cell;
     ASSERT_TRUE(cell.mean.isApprox(transformed.mean - Eigen::Vector2d{1, 2}));
     ASSERT_TRUE(cell.covariance.isApprox(transformed.covariance));
   }
   {
-    const NDTCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
+    const NdtCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
     const Sophus::SE2d tf{Sophus::SO2d{Sophus::Constants<double>::pi() / 2}, Eigen::Vector2d::Zero()};
 
     Eigen::Matrix<double, 2, 2> expected_transformed_cov;
@@ -62,8 +62,8 @@ TEST(NDTCellTests, Product) {
   }
 }
 
-TEST(NDTCellTests, Ostream) {
-  const NDTCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
+TEST(NdtCellTests, Ostream) {
+  const NdtCell2d cell{Eigen::Vector2d{1, 2}, get_diagonal_covariance()};
   std::stringstream ss;
   ss << cell;
   ASSERT_GT(ss.str().size(), 0);
diff --git a/beluga/test/beluga/sensor/test_ndt_model.cpp b/beluga/test/beluga/sensor/test_ndt_model.cpp
index d3b7f5871..865ce8531 100644
--- a/beluga/test/beluga/sensor/test_ndt_model.cpp
+++ b/beluga/test/beluga/sensor/test_ndt_model.cpp
@@ -34,19 +34,19 @@ Eigen::Matrix2Xd get_diagonal_covariance_2d(double x_var = 0.5, double y_var = 0
   return Eigen::Vector2d{x_var, y_var}.asDiagonal();
 }
 
-using sparse_grid_2d_t = SparseValueGrid2<std::unordered_map<Eigen::Vector2i, NDTCell2d, detail::CellHasher<2>>>;
-using sparse_grid_3d_t = SparseValueGrid3<std::unordered_map<Eigen::Vector3i, NDTCell3d, detail::CellHasher<3>>>;
+using sparse_grid_2d_t = SparseValueGrid2<std::unordered_map<Eigen::Vector2i, NdtCell2d, detail::CellHasher<2>>>;
+using sparse_grid_3d_t = SparseValueGrid3<std::unordered_map<Eigen::Vector3i, NdtCell3d, detail::CellHasher<3>>>;
 }  // namespace
 
-TEST(NDTSensorModel2DTests, CanConstruct) {
-  NDTSensorModel{NDTModelParam2d{}, sparse_grid_2d_t{}};
+TEST(NdtSensorModel2DTests, CanConstruct) {
+  NdtSensorModel{NdtModelParam2d{}, sparse_grid_2d_t{}};
 }
 
-TEST(NDTSensorModel2DTests, MinLikelihood) {
+TEST(NdtSensorModel2DTests, MinLikelihood) {
   constexpr double kMinimumLikelihood = 1e-6;
 
-  const NDTModelParam2d param{kMinimumLikelihood};
-  const NDTSensorModel model{param, sparse_grid_2d_t{}};
+  const NdtModelParam2d param{kMinimumLikelihood};
+  const NdtSensorModel model{param, sparse_grid_2d_t{}};
 
   for (const auto& val : {
            Eigen::Vector2d{0.1, 0.1},
@@ -63,7 +63,7 @@ TEST(NDTSensorModel2DTests, MinLikelihood) {
   }
 }
 
-TEST(NDTSensorModel2DTests, Likelihoood) {
+TEST(NdtSensorModel2DTests, Likelihoood) {
   typename sparse_grid_2d_t::map_type map;
   {
     Eigen::Array<double, 2, 2> cov;
@@ -72,7 +72,7 @@ TEST(NDTSensorModel2DTests, Likelihoood) {
            0.0, 0.3;
     // clang-format on
     const Eigen::Vector2d mean(0.5, 0.5);
-    map[Eigen::Vector2i(0, 0)] = NDTCell2d{mean, cov};
+    map[Eigen::Vector2i(0, 0)] = NdtCell2d{mean, cov};
   }
   {
     Eigen::Array<double, 2, 2> cov;
@@ -81,13 +81,13 @@ TEST(NDTSensorModel2DTests, Likelihoood) {
            0.0, 0.5;
     // clang-format on
     const Eigen::Vector2d mean(1.5, 1.5);
-    map[Eigen::Vector2i(1, 1)] = NDTCell2d{mean, cov};
+    map[Eigen::Vector2i(1, 1)] = NdtCell2d{mean, cov};
   }
   sparse_grid_2d_t grid{std::move(map), 1.0};
 
   constexpr double kMinimumLikelihood = 1e-6;
-  const NDTModelParam2d param{kMinimumLikelihood};
-  const NDTSensorModel model{param, std::move(grid)};
+  const NdtModelParam2d param{kMinimumLikelihood};
+  const NdtSensorModel model{param, std::move(grid)};
 
   EXPECT_DOUBLE_EQ(model.likelihood_at({{0.5, 0.5}, get_diagonal_covariance_2d()}), 1.3678794411714423);
   EXPECT_DOUBLE_EQ(model.likelihood_at({{0.8, 0.5}, get_diagonal_covariance_2d()}), 1.4307317817730123);
@@ -98,7 +98,7 @@ TEST(NDTSensorModel2DTests, Likelihoood) {
   EXPECT_DOUBLE_EQ(model.likelihood_at({{1.5, 1.8}, get_diagonal_covariance_2d()}), 1.1669230426687498);
 }
 
-TEST(NDTSensorModel2DTests, FitPoints) {
+TEST(NdtSensorModel2DTests, FitPoints) {
   {
     const std::vector meas{
         Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2},
@@ -121,7 +121,7 @@ TEST(NDTSensorModel2DTests, FitPoints) {
   }
 }
 
-TEST(NDTSensorModel2DTests, ToCellsNotEnoughPointsInCell) {
+TEST(NdtSensorModel2DTests, ToCellsNotEnoughPointsInCell) {
   constexpr double kMapResolution = 0.5;
   const std::vector map_data{
       Eigen::Vector2d{0.1, 0.2},
@@ -132,7 +132,7 @@ TEST(NDTSensorModel2DTests, ToCellsNotEnoughPointsInCell) {
   ASSERT_EQ(cells.size(), 0UL);
 }
 
-TEST(NDTSensorModel3DTests, ToCellsNotEnoughPointsInCell) {
+TEST(NdtSensorModel3DTests, ToCellsNotEnoughPointsInCell) {
   constexpr double kMapResolution = 0.5;
   const std::vector map_data{
       Eigen::Vector3d{0.1, 0.2, 0.0},
@@ -143,7 +143,7 @@ TEST(NDTSensorModel3DTests, ToCellsNotEnoughPointsInCell) {
   ASSERT_EQ(cells.size(), 0UL);
 }
 
-TEST(NDTSensorModel3DTests, FitPoints) {
+TEST(NdtSensorModel3DTests, FitPoints) {
   {
     const std::vector meas{
         Eigen::Vector3d{0.1, 0.2, 0.3}, Eigen::Vector3d{0.1, 0.2, 0.3}, Eigen::Vector3d{0.1, 0.2, 0.3},
@@ -167,7 +167,7 @@ TEST(NDTSensorModel3DTests, FitPoints) {
   }
 }
 
-TEST(NDTSensorModel3DTests, SensorModel) {
+TEST(NdtSensorModel3DTests, SensorModel) {
   constexpr double kMapResolution = 0.5;
   const std::vector map_data{
       Eigen::Vector3d{0.1, 0.2, 0.0},  Eigen::Vector3d{0.112, 0.22, 0.1}, Eigen::Vector3d{0.15, 0.23, 0.1},
@@ -182,7 +182,7 @@ TEST(NDTSensorModel3DTests, SensorModel) {
   }
 
   std::vector perfect_measurement = map_data;
-  const NDTSensorModel model{{}, sparse_grid_3d_t{map_cells_data, kMapResolution}};
+  const NdtSensorModel model{{}, sparse_grid_3d_t{map_cells_data, kMapResolution}};
   auto state_weighing_fn = model(std::move(perfect_measurement));
 
   {
@@ -210,7 +210,7 @@ TEST(NDTSensorModel3DTests, SensorModel) {
   ASSERT_DOUBLE_EQ(state_weighing_fn(Sophus::SE3d{Sophus::SO3d{}, Eigen::Vector3d{0, 0, 1.0}}), 1.0);
 }
 
-TEST(NDTSensorModel2DTests, SensorModel) {
+TEST(NdtSensorModel2DTests, SensorModel) {
   constexpr double kMapResolution = 0.5;
   const std::vector map_data{
       Eigen::Vector2d{0.1, 0.2},  Eigen::Vector2d{0.112, 0.22}, Eigen::Vector2d{0.15, 0.23},
@@ -223,7 +223,7 @@ TEST(NDTSensorModel2DTests, SensorModel) {
     map_cells_data[(cell.mean.array() / kMapResolution).cast<int>()] = cell;
   }
   std::vector perfect_measurement = map_data;
-  const NDTSensorModel model{{}, sparse_grid_2d_t{map_cells_data, kMapResolution}};
+  const NdtSensorModel model{{}, sparse_grid_2d_t{map_cells_data, kMapResolution}};
   auto state_weighing_fn = model(std::move(perfect_measurement));
   // This is a perfect hit, so we should expect weight to be 1 + num_cells == 2.
   ASSERT_DOUBLE_EQ(state_weighing_fn(Sophus::SE2d{}), 2);
@@ -235,20 +235,20 @@ TEST(NDTSensorModel2DTests, SensorModel) {
   ASSERT_DOUBLE_EQ(state_weighing_fn(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{-10, -10}}), 1.0);
 }
 
-TEST(NDTSensorModel2DTests, LoadFromHDF5HappyPath) {
+TEST(NdtSensorModel2DTests, LoadFromHDF5HappyPath) {
   const auto ndt_map_representation = io::load_from_hdf5<sparse_grid_2d_t>("./test_data/turtlebot3_world.hdf5");
   ASSERT_EQ(ndt_map_representation.size(), 30UL);
 }
 
-TEST(NDTSensorModel2DTests, LoadFromHDF5NonExistingFile) {
+TEST(NdtSensorModel2DTests, LoadFromHDF5NonExistingFile) {
   ASSERT_THROW(io::load_from_hdf5<sparse_grid_2d_t>("bad_file.hdf5"), std::invalid_argument);
 }
 
-TEST(NDTSensorModel3DTests, LoadFromHDF5NonExistingFile) {
+TEST(NdtSensorModel3DTests, LoadFromHDF5NonExistingFile) {
   ASSERT_THROW(io::load_from_hdf5<sparse_grid_3d_t>("bad_file.hdf5"), std::invalid_argument);
 }
 
-TEST(NDTSensorModel3DTests, LoadFromHDF5HappyPath) {
+TEST(NdtSensorModel3DTests, LoadFromHDF5HappyPath) {
   const auto ndt_map_representation = io::load_from_hdf5<sparse_grid_3d_t>("./test_data/sample_3d_ndt_map.hdf5");
   ASSERT_EQ(ndt_map_representation.size(), 398);
 }
diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp
index c9d44c692..8c467d08c 100644
--- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp
+++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp
@@ -19,28 +19,17 @@
 #include <optional>
 #include <utility>
 
+#include <tf2_ros/message_filter.h>
+
+#include <std_srvs/srv/empty.hpp>
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcpp"
 #include <message_filters/subscriber.h>
 #pragma GCC diagnostic pop
 
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-#include <bondcpp/bond.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <std_srvs/srv/empty.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-
-#include <beluga/beluga.hpp>
+#include <beluga_amcl/ros2_common.hpp>
 #include <beluga_ros/amcl.hpp>
-#include "beluga_amcl/ros2_common.hpp"
 
 /**
  * \file
diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp
index 5e7deae4b..8022e4f13 100644
--- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp
+++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp
@@ -15,48 +15,27 @@
 #ifndef BELUGA_AMCL_NDT_AMCL_NODE_HPP
 #define BELUGA_AMCL_NDT_AMCL_NODE_HPP
 
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <beluga/beluga.hpp>
-#include <beluga/motion/differential_drive_model.hpp>
-#include <beluga/motion/omnidirectional_drive_model.hpp>
-#include <beluga/motion/stationary_model.hpp>
-#include <beluga/primitives.hpp>
-#include <beluga/sensor/data/ndt_cell.hpp>
-#include <beluga/sensor/data/sparse_value_grid.hpp>
-#include <beluga/sensor/ndt_sensor_model.hpp>
-
-#include <execution>
 #include <functional>
 #include <optional>
-#include <rclcpp/callback_group.hpp>
-#include <rclcpp/subscription_options.hpp>
-#include <sophus/se2.hpp>
 #include <tuple>
-#include <variant>
 
-#include <Eigen/src/Core/Matrix.h>
-#include <H5Cpp.h>
+#include <tf2_ros/message_filter.h>
 
-#include <beluga/algorithm/amcl_core.hpp>
-#include <bondcpp/bond.hpp>
-#include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <std_srvs/srv/empty.hpp>
-
-#include <beluga_ros/laser_scan.hpp>
-#include "beluga_amcl/ros2_common.hpp"
+#include <sophus/types.hpp>
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcpp"
 #include <message_filters/subscriber.h>
 #pragma GCC diagnostic pop
 
+#include <beluga/algorithm/amcl_core.hpp>
+#include <beluga/motion/differential_drive_model.hpp>
+#include <beluga/sensor/data/sparse_value_grid.hpp>
+#include <beluga/sensor/ndt_sensor_model.hpp>
+#include <beluga_amcl/ros2_common.hpp>
+#include <beluga_ros/laser_scan.hpp>
+
 /**
  * \file
  * \brief ROS 2 integration of the 2D NDT-AMCL algorithm.
@@ -65,39 +44,14 @@
 namespace beluga_amcl {
 
 /// Underlying map representation for the NDT sensor model.
-using NDTMapRepresentation =
-    beluga::SparseValueGrid2<std::unordered_map<Eigen::Vector2i, beluga::NDTCell2d, beluga::detail::CellHasher<2>>>;
-
-/// Type of a particle-dependent random state generator.
-using RandomStateGenerator = std::function<std::function<beluga::NDTSensorModel<NDTMapRepresentation>::state_type()>(
-    const beluga::TupleVector<std::tuple<beluga::NDTSensorModel<NDTMapRepresentation>::state_type, beluga::Weight>>)>;
+using NdtMap =
+    beluga::SparseValueGrid2<std::unordered_map<Eigen::Vector2i, beluga::NdtCell2d, beluga::detail::CellHasher<2>>>;
 
-/// Partial specialization of the core AMCL pipeline for convinience.
-template <class MotionModel, class ExecutionPolicy>
+/// Instantiatiation of the core AMCL pipeline with the NDT sensor model.
 using NdtAmcl = beluga::Amcl<
-    MotionModel,
-    beluga::NDTSensorModel<NDTMapRepresentation>,
-    RandomStateGenerator,
-    beluga::Weight,
-    std::tuple<typename beluga::NDTSensorModel<NDTMapRepresentation>::state_type, beluga::Weight>,
-    ExecutionPolicy>;
-
-/// All combinations of supported NDT AMCL variants.
-using NdtAmclVariant = std::variant<
-    NdtAmcl<beluga::StationaryModel, std::execution::parallel_policy>,            //
-    NdtAmcl<beluga::StationaryModel, std::execution::sequenced_policy>,           //
-    NdtAmcl<beluga::DifferentialDriveModel2d, std::execution::parallel_policy>,   //
-    NdtAmcl<beluga::DifferentialDriveModel2d, std::execution::sequenced_policy>,  //
-    NdtAmcl<beluga::OmnidirectionalDriveModel, std::execution::parallel_policy>,  //
-    NdtAmcl<beluga::OmnidirectionalDriveModel, std::execution::sequenced_policy>  //
-    >;
-
-/// Supported motion models.
-using MotionModelVariant =
-    std::variant<beluga::DifferentialDriveModel2d, beluga::StationaryModel, beluga::OmnidirectionalDriveModel>;
-
-/// Supported execution policies.
-using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
+    beluga::DifferentialDriveModel2d,  //
+    beluga::NdtSensorModel<NdtMap>,    //
+    std::tuple<Sophus::SE2d, beluga::Weight>>;
 
 /// 2D NDT AMCL as a ROS 2 composable lifecycle node.
 class NdtAmclNode : public BaseAMCLNode {
@@ -119,13 +73,13 @@ class NdtAmclNode : public BaseAMCLNode {
   auto get_initial_estimate() const -> std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>>;
 
   /// Get motion model as per current parametrization.
-  auto get_motion_model() const -> MotionModelVariant;
+  auto get_motion_model() const -> beluga::DifferentialDriveModel2d;
 
   /// Get sensor model as per current parametrization.
-  beluga::NDTSensorModel<NDTMapRepresentation> get_sensor_model() const;
+  auto get_sensor_model() const -> beluga::NdtSensorModel<NdtMap>;
 
   /// Instantiate particle filter given an initial occupancy grid map and the current parametrization.
-  auto make_particle_filter() const -> std::unique_ptr<NdtAmclVariant>;
+  auto make_particle_filter() const -> std::unique_ptr<NdtAmcl>;
 
   /// Callback for periodic particle cloud updates.
   void do_periodic_timer_callback() override;
@@ -157,7 +111,7 @@ class NdtAmclNode : public BaseAMCLNode {
   message_filters::Connection laser_scan_connection_;
 
   /// Particle filter instance.
-  std::unique_ptr<NdtAmclVariant> particle_filter_ = nullptr;
+  std::unique_ptr<NdtAmcl> particle_filter_ = nullptr;
   /// Last known pose estimate, if any.
   std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_ = std::nullopt;
   /// Last known map to odom correction estimate, if any.
diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp
index f5edb2ad3..d2dd53d3f 100644
--- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp
+++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp
@@ -15,36 +15,27 @@
 #ifndef BELUGA_AMCL_NDT_AMCL_NODE_3D_HPP
 #define BELUGA_AMCL_NDT_AMCL_NODE_3D_HPP
 
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <beluga/algorithm/amcl_core.hpp>
-#include <beluga/beluga.hpp>
-
-#include <execution>
 #include <functional>
 #include <optional>
-#include <sophus/types.hpp>
 #include <tuple>
-#include <variant>
 
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
-#include <std_srvs/srv/empty.hpp>
+#include <tf2_ros/message_filter.h>
 
-#include <beluga_ros/laser_scan.hpp>
-#include "beluga_amcl/ros2_common.hpp"
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <sophus/types.hpp>
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wcpp"
 #include <message_filters/subscriber.h>
 #pragma GCC diagnostic pop
 
+#include <beluga/algorithm/amcl_core.hpp>
+#include <beluga/motion/differential_drive_model.hpp>
+#include <beluga/sensor/data/sparse_value_grid.hpp>
+#include <beluga/sensor/ndt_sensor_model.hpp>
+#include <beluga_amcl/ros2_common.hpp>
+#include <beluga_ros/laser_scan.hpp>
+
 /**
  * \file
  * \brief ROS 2 integration of the 3D NDT-AMCL algorithm.
@@ -53,32 +44,14 @@
 namespace beluga_amcl {
 
 /// Underlying map representation for the NDT sensor model.
-using NDTMapRepresentation =
-    beluga::SparseValueGrid3<std::unordered_map<Eigen::Vector3i, beluga::NDTCell3d, beluga::detail::CellHasher<3>>>;
+using NdtMap =
+    beluga::SparseValueGrid3<std::unordered_map<Eigen::Vector3i, beluga::NdtCell3d, beluga::detail::CellHasher<3>>>;
 
-/// State type for the particles.
-using StateType = typename beluga::NDTSensorModel<NDTMapRepresentation>::state_type;
-
-/// Type of a particle-dependent random state generator.
-using RandomStateGenerator = std::function<StateType()>;
-
-/// Partial specialization of the core AMCL pipeline for convinience.
-template <class MotionModel, class ExecutionPolicy>
+/// Instantiatiation of the core AMCL pipeline with the NDT sensor model.
 using NdtAmcl = beluga::Amcl<
-    MotionModel,
-    beluga::NDTSensorModel<NDTMapRepresentation>,
-    RandomStateGenerator,
-    beluga::Weight,
-    std::tuple<StateType, beluga::Weight>,
-    ExecutionPolicy>;
-
-/// All combinations of supported 3D NDT AMCL variants.
-using NdtAmclVariant = std::variant<
-    NdtAmcl<beluga::DifferentialDriveModel3d, std::execution::parallel_policy>,  //
-    NdtAmcl<beluga::DifferentialDriveModel3d, std::execution::sequenced_policy>>;
-
-/// Supported motion models.
-using MotionModelVariant = std::variant<beluga::DifferentialDriveModel3d>;
+    beluga::DifferentialDriveModel3d,  //
+    beluga::NdtSensorModel<NdtMap>,    //
+    std::tuple<Sophus::SE3d, beluga::Weight>>;
 
 /// 3D NDT AMCL as a ROS 2 composable lifecycle node.
 class NdtAmclNode3D : public BaseAMCLNode {
@@ -100,13 +73,13 @@ class NdtAmclNode3D : public BaseAMCLNode {
   auto get_initial_estimate() const -> std::optional<std::pair<Sophus::SE3d, Sophus::Matrix6d>>;
 
   /// Get motion model as per current parametrization.
-  auto get_motion_model() const -> MotionModelVariant;
+  auto get_motion_model() const -> beluga::DifferentialDriveModel3d;
 
   /// Get sensor model as per current parametrization.
-  beluga::NDTSensorModel<NDTMapRepresentation> get_sensor_model() const;
+  auto get_sensor_model() const -> beluga::NdtSensorModel<NdtMap>;
 
   /// Instantiate particle filter given an initial occupancy grid map and the current parametrization.
-  auto make_particle_filter() const -> std::unique_ptr<NdtAmclVariant>;
+  auto make_particle_filter() const -> std::unique_ptr<NdtAmcl>;
 
   /// Callback for periodic particle cloud updates.
   void do_periodic_timer_callback() override;
@@ -138,7 +111,7 @@ class NdtAmclNode3D : public BaseAMCLNode {
   message_filters::Connection laser_scan_connection_;
 
   /// Particle filter instance.
-  std::unique_ptr<NdtAmclVariant> particle_filter_;
+  std::unique_ptr<NdtAmcl> particle_filter_;
   /// Last known pose estimate, if any.
   std::optional<std::pair<Sophus::SE3d, Sophus::Matrix6d>> last_known_estimate_;
   /// Last known map to odom correction estimate, if any.
diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp
index 9e7ad3754..d56f5eb01 100644
--- a/beluga_amcl/src/amcl_node.cpp
+++ b/beluga_amcl/src/amcl_node.cpp
@@ -13,61 +13,20 @@
 // limitations under the License.
 
 #include <chrono>
-#include <cstddef>
-#include <execution>
 #include <functional>
-#include <limits>
 #include <memory>
 #include <optional>
-#include <ratio>
 #include <stdexcept>
 #include <string>
-#include <string_view>
-#include <tuple>
 #include <utility>
-
-#include <tf2/convert.h>
-#include <tf2/exceptions.h>
-#include <tf2/time.h>
-
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/buffer_interface.h>
-#include <tf2_ros/create_timer_ros.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <Eigen/Core>
-#include <sophus/se2.hpp>
-
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wcpp"
-#include <message_filters/subscriber.h>
-#pragma GCC diagnostic pop
+#include <vector>
 
 #include <rclcpp/version.h>
-#include <bondcpp/bond.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <lifecycle_msgs/msg/state.hpp>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <std_srvs/srv/empty.hpp>
-
-#include <beluga/motion/differential_drive_model.hpp>
-#include <beluga/motion/omnidirectional_drive_model.hpp>
-#include <beluga/motion/stationary_model.hpp>
-#include <beluga/sensor/beam_model.hpp>
-#include <beluga/sensor/likelihood_field_model.hpp>
-#include <beluga_ros/amcl.hpp>
-#include <beluga_ros/messages.hpp>
+
+#include <beluga_amcl/amcl_node.hpp>
 #include <beluga_ros/particle_cloud.hpp>
 #include <beluga_ros/tf2_sophus.hpp>
-#include "beluga_amcl/amcl_node.hpp"
-#include "beluga_amcl/ros2_common.hpp"
 
 namespace beluga_amcl {
 
diff --git a/beluga_amcl/src/ndt_amcl_node.cpp b/beluga_amcl/src/ndt_amcl_node.cpp
index 85e7a2c89..f3b52950f 100644
--- a/beluga_amcl/src/ndt_amcl_node.cpp
+++ b/beluga_amcl/src/ndt_amcl_node.cpp
@@ -13,64 +13,18 @@
 // limitations under the License.
 
 #include <chrono>
-#include <cstddef>
-#include <execution>
 #include <functional>
 #include <memory>
 #include <optional>
-#include <rclcpp/duration.hpp>
 #include <stdexcept>
 #include <string>
-#include <string_view>
 #include <utility>
-#include <variant>
 #include <vector>
 
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
-
-#include <Eigen/Core>
-#include <sophus/se2.hpp>
-#include <sophus/types.hpp>
-
-#include <tf2/convert.h>
-#include <tf2/exceptions.h>
-#include <tf2/time.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/buffer_interface.h>
-#include <tf2_ros/create_timer_ros.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wcpp"
-#include <message_filters/subscriber.h>
-#pragma GCC diagnostic pop
-
-#include <bondcpp/bond.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <lifecycle_msgs/msg/state.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-
-#include <beluga/algorithm/amcl_core.hpp>
-#include <beluga/algorithm/estimation.hpp>
-#include <beluga/algorithm/spatial_hash.hpp>
-#include <beluga/motion/differential_drive_model.hpp>
-#include <beluga/motion/omnidirectional_drive_model.hpp>
-#include <beluga/motion/stationary_model.hpp>
-#include <beluga/random/multivariate_normal_distribution.hpp>
-#include <beluga/sensor/ndt_sensor_model.hpp>
-#include <beluga/views/particles.hpp>
-#include <beluga_ros/messages.hpp>
 #include <beluga_ros/particle_cloud.hpp>
 #include <beluga_ros/tf2_sophus.hpp>
-#include "beluga_amcl/ndt_amcl_node.hpp"
-#include "beluga_amcl/ros2_common.hpp"
+
+#include <beluga_amcl/ndt_amcl_node.hpp>
 
 namespace beluga_amcl {
 
@@ -178,81 +132,52 @@ auto NdtAmclNode::get_initial_estimate() const -> std::optional<std::pair<Sophus
   return std::make_pair(pose, covariance);
 }
 
-auto NdtAmclNode::get_motion_model() const -> MotionModelVariant {
+auto NdtAmclNode::get_motion_model() const -> beluga::DifferentialDriveModel2d {
   const auto name = get_parameter("robot_model_type").as_string();
-  if (name == kDifferentialModelName) {
+  if (name == kDifferentialModelName || name == kNav2DifferentialModelName) {
     auto params = beluga::DifferentialDriveModelParam{};
     params.rotation_noise_from_rotation = get_parameter("alpha1").as_double();
     params.rotation_noise_from_translation = get_parameter("alpha2").as_double();
     params.translation_noise_from_translation = get_parameter("alpha3").as_double();
     params.translation_noise_from_rotation = get_parameter("alpha4").as_double();
-    return beluga::DifferentialDriveModel{params};
-  }
-  if (name == kOmnidirectionalModelName) {
-    auto params = beluga::OmnidirectionalDriveModelParam{};
-    params.rotation_noise_from_rotation = get_parameter("alpha1").as_double();
-    params.rotation_noise_from_translation = get_parameter("alpha2").as_double();
-    params.translation_noise_from_translation = get_parameter("alpha3").as_double();
-    params.translation_noise_from_rotation = get_parameter("alpha4").as_double();
-    params.strafe_noise_from_translation = get_parameter("alpha5").as_double();
-    return beluga::OmnidirectionalDriveModel{params};
+    return beluga::DifferentialDriveModel2d{params};
   }
-  if (name == kStationaryModelName) {
-    return beluga::StationaryModel{};
-  }
-
   throw std::invalid_argument(std::string("Invalid motion model: ") + name);
 }
 
-beluga::NDTSensorModel<NDTMapRepresentation> NdtAmclNode::get_sensor_model() const {
-  auto params = beluga::NDTModelParam2d{};
+auto NdtAmclNode::get_sensor_model() const -> beluga::NdtSensorModel<NdtMap> {
+  auto params = beluga::NdtModelParam2d{};
   params.minimum_likelihood = get_parameter("minimum_likelihood").as_double();
   params.d1 = get_parameter("d1").as_double();
   params.d2 = get_parameter("d2").as_double();
   const auto map_path = get_parameter("map_path").as_string();
   RCLCPP_INFO(get_logger(), "Loading map from %s.", map_path.c_str());
 
-  return beluga::NDTSensorModel<NDTMapRepresentation>{
-      params, beluga::io::load_from_hdf5<NDTMapRepresentation>(get_parameter("map_path").as_string())};
+  return beluga::NdtSensorModel<NdtMap>{
+      params, beluga::io::load_from_hdf5<NdtMap>(get_parameter("map_path").as_string())};
 }
 
-auto NdtAmclNode::make_particle_filter() const -> std::unique_ptr<NdtAmclVariant> {
-  auto amcl = std::visit(
-      [this](auto motion_model, auto execution_policy) -> NdtAmclVariant {
-        auto params = beluga::AmclParams{};
-        params.update_min_d = get_parameter("update_min_d").as_double();
-        params.update_min_a = get_parameter("update_min_a").as_double();
-        params.resample_interval = static_cast<std::size_t>(get_parameter("resample_interval").as_int());
-        params.selective_resampling = get_parameter("selective_resampling").as_bool();
-        params.min_particles = static_cast<std::size_t>(get_parameter("min_particles").as_int());
-        params.max_particles = static_cast<std::size_t>(get_parameter("max_particles").as_int());
-        params.alpha_slow = get_parameter("recovery_alpha_slow").as_double();
-        params.alpha_fast = get_parameter("recovery_alpha_fast").as_double();
-        params.kld_epsilon = get_parameter("pf_err").as_double();
-        params.kld_z = get_parameter("pf_z").as_double();
-
-        auto hasher = beluga::spatial_hash<Sophus::SE2d>(
-            get_parameter("spatial_resolution_x").as_double(), get_parameter("spatial_resolution_y").as_double(),
-            get_parameter("spatial_resolution_theta").as_double());
-
-        RandomStateGenerator random_state_maker = [](const auto& particles) {
-          static thread_local auto generator = std::mt19937{std::random_device()()};
-          const auto estimate = beluga::estimate(beluga::views::states(particles), beluga::views::weights(particles));
-          return [estimate]() {
-            return beluga::MultivariateNormalDistribution{estimate.first, estimate.second}(generator);
-          };
-        };
-
-        return beluga::Amcl(
-            std::move(motion_model),
-            get_sensor_model(),             //
-            std::move(random_state_maker),  //
-            std::move(hasher),              //
-            params,                         //
-            execution_policy);
-      },
-      get_motion_model(), get_execution_policy());
-  return std::make_unique<NdtAmclVariant>(std::move(amcl));
+auto NdtAmclNode::make_particle_filter() const -> std::unique_ptr<beluga_amcl::NdtAmcl> {
+  auto params = beluga::AmclParams{};
+  params.update_min_d = get_parameter("update_min_d").as_double();
+  params.update_min_a = get_parameter("update_min_a").as_double();
+  params.resample_interval = static_cast<std::size_t>(get_parameter("resample_interval").as_int());
+  params.selective_resampling = get_parameter("selective_resampling").as_bool();
+  params.min_particles = static_cast<std::size_t>(get_parameter("min_particles").as_int());
+  params.max_particles = static_cast<std::size_t>(get_parameter("max_particles").as_int());
+  params.kld_epsilon = get_parameter("pf_err").as_double();
+  params.kld_z = get_parameter("pf_z").as_double();
+
+  auto hasher = beluga::spatial_hash<Sophus::SE2d>(
+      get_parameter("spatial_resolution_x").as_double(),  //
+      get_parameter("spatial_resolution_y").as_double(),  //
+      get_parameter("spatial_resolution_theta").as_double());
+
+  return std::make_unique<beluga_amcl::NdtAmcl>(
+      get_motion_model(),
+      get_sensor_model(),  //
+      std::move(hasher),   //
+      params);
 }
 
 void NdtAmclNode::do_periodic_timer_callback() {
@@ -263,14 +188,11 @@ void NdtAmclNode::do_periodic_timer_callback() {
   if (particle_cloud_pub_->get_subscription_count() == 0) {
     return;
   }
-  std::visit(
-      [this](const auto& particle_filter) {
-        auto message = beluga_ros::msg::PoseArray{};
-        beluga_ros::assign_particle_cloud(particle_filter.particles(), message);
-        beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
-        particle_cloud_pub_->publish(message);
-      },
-      *particle_filter_);
+
+  auto message = beluga_ros::msg::PoseArray{};
+  beluga_ros::assign_particle_cloud(particle_filter_->particles(), message);
+  beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
+  particle_cloud_pub_->publish(message);
 }
 
 // TODO(alon): Wouldn't it be better in the callback of each message to simply receive
@@ -314,22 +236,21 @@ void NdtAmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr las
 
   const auto update_start_time = std::chrono::high_resolution_clock::now();
 
-  // TODO(nahuel, Ramiro): Remove this once we update the measurement type.
+  // TODO(nahuel): Remove this once we update the measurement type.
   auto scan = beluga_ros::LaserScan{
-      laser_scan, laser_pose_in_base, static_cast<std::size_t>(get_parameter("max_beams").as_int()),
-      get_parameter("laser_min_range").as_double(), get_parameter("laser_max_range").as_double()};
+      laser_scan,                                                     //
+      laser_pose_in_base,                                             //
+      static_cast<std::size_t>(get_parameter("max_beams").as_int()),  //
+      get_parameter("laser_min_range").as_double(),                   //
+      get_parameter("laser_max_range").as_double()};
+
   auto measurement = scan.points_in_cartesian_coordinates() |  //
                      ranges::views::transform([&scan](const auto& p) {
                        return Eigen::Vector2d((scan.origin() * Sophus::Vector3d{p.x(), p.y(), 0}).head<2>());
                      }) |
                      ranges::to<std::vector>;
-  const auto new_estimate = std::visit(
-      [base_pose_in_odom, measurement = std::move(measurement)](auto& particle_filter) {
-        return particle_filter.update(
-            base_pose_in_odom,  //
-            std::move(measurement));
-      },
-      *particle_filter_);
+
+  const auto new_estimate = particle_filter_->update(base_pose_in_odom, std::move(measurement));
 
   const auto update_stop_time = std::chrono::high_resolution_clock::now();
   const auto update_duration = update_stop_time - update_start_time;
@@ -339,8 +260,7 @@ void NdtAmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr las
     last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
     last_known_estimate_ = new_estimate;
 
-    const auto num_particles =
-        std::visit([](const auto& particle_filter) { return particle_filter.particles().size(); }, *particle_filter_);
+    const auto num_particles = particle_filter_->particles().size();
 
     RCLCPP_INFO(
         get_logger(), "Particle filter update iteration stats: %ld particles %ld points - %.3fms", num_particles,
@@ -400,9 +320,7 @@ bool NdtAmclNode::initialize_from_estimate(const std::pair<Sophus::SE2d, Eigen::
   }
 
   try {
-    std::visit(
-        [estimate](auto& particle_filter) { particle_filter.initialize(estimate.first, estimate.second); },
-        *particle_filter_);
+    particle_filter_->initialize(estimate.first, estimate.second);
   } catch (const std::runtime_error& error) {
     RCLCPP_ERROR(get_logger(), "Could not initialize particles: %s", error.what());
     return false;
@@ -410,16 +328,17 @@ bool NdtAmclNode::initialize_from_estimate(const std::pair<Sophus::SE2d, Eigen::
 
   enable_tf_broadcast_ = true;
 
-  const auto num_particles =
-      std::visit([](const auto& particle_filter) { return particle_filter.particles().size(); }, *particle_filter_);
-
   const auto& pose = estimate.first;
   RCLCPP_INFO(
       get_logger(), "Particle filter initialized with %ld particles about initial pose x=%g, y=%g, yaw=%g",
-      num_particles, pose.translation().x(), pose.translation().y(), pose.so2().log());
+      particle_filter_->particles().size(),  //
+      pose.translation().x(),                //
+      pose.translation().y(),                //
+      pose.so2().log());
 
   return true;
 }
+
 }  // namespace beluga_amcl
 
 #include <rclcpp_components/register_node_macro.hpp>
diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp
index 7b8ac2b3c..957a8bc71 100644
--- a/beluga_amcl/src/ndt_amcl_node_3d.cpp
+++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp
@@ -12,71 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <sensor_msgs/msg/detail/point_cloud2__struct.h>
-#include <sensor_msgs/msg/point_cloud2.h>
-#include <beluga/algorithm/unscented_transform.hpp>
 #include <chrono>
-#include <cstddef>
-#include <execution>
 #include <functional>
 #include <memory>
 #include <optional>
-#include <range/v3/view/zip.hpp>
-#include <rclcpp/duration.hpp>
-#include <sensor_msgs/point_cloud2_iterator.hpp>
-#include <sophus/se3.hpp>
 #include <stdexcept>
 #include <string>
-#include <string_view>
 #include <utility>
-#include <variant>
 #include <vector>
 
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
-
-#include <Eigen/Core>
-#include <sophus/types.hpp>
-
-#include <Eigen/src/Core/Matrix.h>
-#include <tf2/convert.h>
-#include <tf2/exceptions.h>
-#include <tf2/time.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/buffer_interface.h>
-#include <tf2_ros/create_timer_ros.h>
-#include <tf2_ros/message_filter.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wcpp"
-#include <message_filters/subscriber.h>
-#pragma GCC diagnostic pop
-
-#include <bondcpp/bond.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_lifecycle/lifecycle_node.hpp>
-
-#include <geometry_msgs/msg/pose_array.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <lifecycle_msgs/msg/state.hpp>
-#include <sensor_msgs/msg/laser_scan.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
-
-#include <beluga/algorithm/amcl_core.hpp>
-#include <beluga/algorithm/estimation.hpp>
-#include <beluga/algorithm/spatial_hash.hpp>
-#include <beluga/motion/differential_drive_model.hpp>
-
-#include <beluga/random/multivariate_normal_distribution.hpp>
-#include <beluga/sensor/ndt_sensor_model.hpp>
-#include <beluga/views/particles.hpp>
-#include <beluga_ros/messages.hpp>
 #include <beluga_ros/particle_cloud.hpp>
 #include <beluga_ros/tf2_sophus.hpp>
-#include "beluga_amcl/ndt_amcl_node_3d.hpp"
-#include "beluga_amcl/ros2_common.hpp"
+
+#include <beluga_amcl/ndt_amcl_node_3d.hpp>
 
 namespace beluga_amcl {
 
@@ -224,62 +172,51 @@ auto NdtAmclNode3D::get_initial_estimate() const -> std::optional<std::pair<Soph
   return std::make_pair(pose, covariance);
 }
 
-auto NdtAmclNode3D::get_motion_model() const -> MotionModelVariant {
+auto NdtAmclNode3D::get_motion_model() const -> beluga::DifferentialDriveModel3d {
   const auto name = get_parameter("robot_model_type").as_string();
-  if (name == kDifferentialModelName || name == kDifferentialModelName) {
+  if (name == kDifferentialModelName || name == kNav2DifferentialModelName) {
     auto params = beluga::DifferentialDriveModelParam{};
     params.rotation_noise_from_rotation = get_parameter("alpha1").as_double();
     params.rotation_noise_from_translation = get_parameter("alpha2").as_double();
     params.translation_noise_from_translation = get_parameter("alpha3").as_double();
     params.translation_noise_from_rotation = get_parameter("alpha4").as_double();
-    return beluga::DifferentialDriveModel<Sophus::SE3d>{params};
+    return beluga::DifferentialDriveModel3d{params};
   }
   throw std::invalid_argument(std::string("Invalid motion model: ") + name);
 }
 
-beluga::NDTSensorModel<NDTMapRepresentation> NdtAmclNode3D::get_sensor_model() const {
-  auto params = beluga::NDTModelParam3d{};
+auto NdtAmclNode3D::get_sensor_model() const -> beluga::NdtSensorModel<NdtMap> {
+  auto params = beluga::NdtModelParam3d{};
   params.minimum_likelihood = get_parameter("minimum_likelihood").as_double();
   params.d1 = get_parameter("d1").as_double();
   params.d2 = get_parameter("d2").as_double();
   const auto map_path = get_parameter("map_path").as_string();
   RCLCPP_INFO(get_logger(), "Loading map from %s.", map_path.c_str());
 
-  return beluga::NDTSensorModel<NDTMapRepresentation>{
-      params, beluga::io::load_from_hdf5<NDTMapRepresentation>(get_parameter("map_path").as_string())};
+  return beluga::NdtSensorModel<NdtMap>{
+      params, beluga::io::load_from_hdf5<NdtMap>(get_parameter("map_path").as_string())};
 }
-auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr<NdtAmclVariant> {
-  auto amcl = std::visit(
-      [this](auto motion_model, auto execution_policy) -> NdtAmclVariant {
-        auto params = beluga::AmclParams{};
-        params.update_min_d = get_parameter("update_min_d").as_double();
-        params.update_min_a = get_parameter("update_min_a").as_double();
-        params.resample_interval = static_cast<std::size_t>(get_parameter("resample_interval").as_int());
-        params.selective_resampling = get_parameter("selective_resampling").as_bool();
-        params.min_particles = static_cast<std::size_t>(get_parameter("min_particles").as_int());
-        params.max_particles = static_cast<std::size_t>(get_parameter("max_particles").as_int());
-        params.alpha_slow = get_parameter("recovery_alpha_slow").as_double();
-        params.alpha_fast = get_parameter("recovery_alpha_fast").as_double();
-        params.kld_epsilon = get_parameter("pf_err").as_double();
-        params.kld_z = get_parameter("pf_z").as_double();
-
-        auto hasher = beluga::spatial_hash<Sophus::SE3d>(
-            get_parameter("spatial_resolution_x").as_double(), get_parameter("spatial_resolution_theta").as_double());
-
-        // We don't support randomly sampling from NDT maps, so we enforce a non-adaptive filter.
-        assert(params.min_particles == params.max_particles);
-        RandomStateGenerator random_state_maker = []() { return Sophus::SE3d{}; };
-
-        return beluga::Amcl(
-            std::move(motion_model),
-            get_sensor_model(),             //
-            std::move(random_state_maker),  //
-            std::move(hasher),              //
-            params,                         //
-            execution_policy);
-      },
-      get_motion_model(), get_execution_policy());
-  return std::make_unique<NdtAmclVariant>(std::move(amcl));
+
+auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr<beluga_amcl::NdtAmcl> {
+  auto params = beluga::AmclParams{};
+  params.update_min_d = get_parameter("update_min_d").as_double();
+  params.update_min_a = get_parameter("update_min_a").as_double();
+  params.resample_interval = static_cast<std::size_t>(get_parameter("resample_interval").as_int());
+  params.selective_resampling = get_parameter("selective_resampling").as_bool();
+  params.min_particles = static_cast<std::size_t>(get_parameter("min_particles").as_int());
+  params.max_particles = static_cast<std::size_t>(get_parameter("max_particles").as_int());
+  params.kld_epsilon = get_parameter("pf_err").as_double();
+  params.kld_z = get_parameter("pf_z").as_double();
+
+  auto hasher = beluga::spatial_hash<Sophus::SE3d>(
+      get_parameter("spatial_resolution_x").as_double(),  //
+      get_parameter("spatial_resolution_theta").as_double());
+
+  return std::make_unique<beluga_amcl::NdtAmcl>(
+      get_motion_model(),
+      get_sensor_model(),  //
+      std::move(hasher),   //
+      params);
 }
 
 void NdtAmclNode3D::do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) {
@@ -299,14 +236,11 @@ void NdtAmclNode3D::do_periodic_timer_callback() {
   if (particle_cloud_pub_->get_subscription_count() == 0) {
     return;
   }
-  std::visit(
-      [this](const auto& particle_filter) {
-        auto message = beluga_ros::msg::PoseArray{};
-        beluga_ros::assign_particle_cloud(particle_filter.particles(), message);
-        beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
-        particle_cloud_pub_->publish(message);
-      },
-      *particle_filter_);
+
+  auto message = beluga_ros::msg::PoseArray{};
+  beluga_ros::assign_particle_cloud(particle_filter_->particles(), message);
+  beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message);
+  particle_cloud_pub_->publish(message);
 }
 
 // TODO(alon): Wouldn't it be better in the callback of each message to simply receive
@@ -357,13 +291,7 @@ void NdtAmclNode3D::laser_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr
   };
 
   RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Processing %ld points.", measurement.size());
-  const auto new_estimate = std::visit(
-      [base_pose_in_odom, measurement = measurement](auto& particle_filter) {
-        return particle_filter.update(
-            base_pose_in_odom,  //
-            std::move(measurement));
-      },
-      *particle_filter_);
+  const auto new_estimate = particle_filter_->update(base_pose_in_odom, std::move(measurement));
 
   const auto update_stop_time = std::chrono::high_resolution_clock::now();
   const auto update_duration = update_stop_time - update_start_time;
@@ -373,11 +301,9 @@ void NdtAmclNode3D::laser_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr
     last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
     last_known_estimate_ = new_estimate;
 
-    const auto num_particles =
-        std::visit([](const auto& particle_filter) { return particle_filter.particles().size(); }, *particle_filter_);
-
     RCLCPP_INFO(
-        get_logger(), "Particle filter update iteration stats: %ld particles - %.3fms", num_particles,
+        get_logger(), "Particle filter update iteration stats: %ld particles - %.3fms",  //
+        particle_filter_->particles().size(),                                            //
         std::chrono::duration<double, std::milli>(update_duration).count());
   }
 
@@ -428,9 +354,7 @@ bool NdtAmclNode3D::initialize_from_estimate(const std::pair<Sophus::SE3d, Sophu
   }
 
   try {
-    std::visit(
-        [estimate](auto& particle_filter) { particle_filter.initialize(estimate.first, estimate.second); },
-        *particle_filter_);
+    particle_filter_->initialize(estimate.first, estimate.second);
   } catch (const std::runtime_error& error) {
     RCLCPP_ERROR(get_logger(), "Could not initialize particles: %s", error.what());
     return false;
@@ -438,13 +362,13 @@ bool NdtAmclNode3D::initialize_from_estimate(const std::pair<Sophus::SE3d, Sophu
 
   enable_tf_broadcast_ = true;
 
-  const auto num_particles =
-      std::visit([](const auto& particle_filter) { return particle_filter.particles().size(); }, *particle_filter_);
-
   const auto& pose = estimate.first;
   RCLCPP_INFO(
-      get_logger(), "Particle filter initialized with %ld particles about initial pose x=%g, y=%g, z=%g", num_particles,
-      pose.translation().x(), pose.translation().y(), pose.translation().z());
+      get_logger(), "Particle filter initialized with %ld particles about initial pose x=%g, y=%g, z=%g",
+      particle_filter_->particles().size(),  //
+      pose.translation().x(),                //
+      pose.translation().y(),                //
+      pose.translation().z());
 
   return true;
 }
diff --git a/beluga_amcl/test/test_ndt_amcl_3d_node.cpp b/beluga_amcl/test/test_ndt_amcl_3d_node.cpp
index f2401ee5b..5445b3b19 100644
--- a/beluga_amcl/test/test_ndt_amcl_3d_node.cpp
+++ b/beluga_amcl/test/test_ndt_amcl_3d_node.cpp
@@ -206,7 +206,7 @@ TEST_P(TestInitializationWithModel, ParticleCount) {
 
   ASSERT_TRUE(wait_for_initialization());
 
-  std::visit([](const auto& pf) { ASSERT_EQ(pf.particles().size(), 10UL); }, *ndt_amcl_node_->particle_filter());
+  ASSERT_EQ(ndt_amcl_node_->particle_filter()->particles().size(), 10UL);
 }
 
 class TestNode : public BaseNodeFixture<::testing::Test> {};
@@ -371,26 +371,6 @@ TEST_F(TestNode, InvalidMotionModel) {
   ndt_amcl_node_->activate();
   ASSERT_FALSE(wait_for_initialization());
 }
-TEST_F(TestNode, InvalidExecutionPolicy) {
-  ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "non_existing_policy"});
-  ndt_amcl_node_->configure();
-  ndt_amcl_node_->activate();
-  ASSERT_FALSE(wait_for_initialization());
-}
-
-TEST_F(TestNode, SequentialExecutionPolicy) {
-  ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "seq"});
-  ndt_amcl_node_->configure();
-  ndt_amcl_node_->activate();
-  ASSERT_TRUE(wait_for_initialization());
-}
-
-TEST_F(TestNode, ParallelExecutionPolicy) {
-  ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "par"});
-  ndt_amcl_node_->configure();
-  ndt_amcl_node_->activate();
-  ASSERT_TRUE(wait_for_initialization());
-}
 
 TEST_F(TestNode, InitialPoseBeforeInitialize) {
   ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false});
diff --git a/beluga_amcl/test/test_ndt_amcl_node.cpp b/beluga_amcl/test/test_ndt_amcl_node.cpp
index 871c48cfb..f346856aa 100644
--- a/beluga_amcl/test/test_ndt_amcl_node.cpp
+++ b/beluga_amcl/test/test_ndt_amcl_node.cpp
@@ -184,10 +184,7 @@ TEST_F(TestLifecycle, AutoStart) {
 
 class TestInitializationWithModel : public BaseNodeFixture<::testing::TestWithParam<const char*>> {};
 
-INSTANTIATE_TEST_SUITE_P(
-    Models,
-    TestInitializationWithModel,
-    testing::Values("differential_drive", "omnidirectional_drive", "stationary"));
+INSTANTIATE_TEST_SUITE_P(Models, TestInitializationWithModel, testing::Values("differential_drive"));
 
 TEST_P(TestInitializationWithModel, ParticleCount) {
   const auto* const motion_model = GetParam();
@@ -201,13 +198,8 @@ TEST_P(TestInitializationWithModel, ParticleCount) {
   ndt_amcl_node_->activate();
 
   ASSERT_TRUE(wait_for_initialization());
-
-  std::visit(
-      [](const auto& pf) {
-        ASSERT_GE(pf.particles().size(), 10UL);
-        ASSERT_LE(pf.particles().size(), 30UL);
-      },
-      *ndt_amcl_node_->particle_filter());
+  ASSERT_GE(ndt_amcl_node_->particle_filter()->particles().size(), 10UL);
+  ASSERT_LE(ndt_amcl_node_->particle_filter()->particles().size(), 30UL);
 }
 
 class TestNode : public BaseNodeFixture<::testing::Test> {};
@@ -365,26 +357,6 @@ TEST_F(TestNode, InvalidMotionModel) {
   ndt_amcl_node_->activate();
   ASSERT_FALSE(wait_for_initialization());
 }
-TEST_F(TestNode, InvalidExecutionPolicy) {
-  ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "non_existing_policy"});
-  ndt_amcl_node_->configure();
-  ndt_amcl_node_->activate();
-  ASSERT_FALSE(wait_for_initialization());
-}
-
-TEST_F(TestNode, SequentialExecutionPolicy) {
-  ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "seq"});
-  ndt_amcl_node_->configure();
-  ndt_amcl_node_->activate();
-  ASSERT_TRUE(wait_for_initialization());
-}
-
-TEST_F(TestNode, ParallelExecutionPolicy) {
-  ndt_amcl_node_->set_parameter(rclcpp::Parameter{"execution_policy", "par"});
-  ndt_amcl_node_->configure();
-  ndt_amcl_node_->activate();
-  ASSERT_TRUE(wait_for_initialization());
-}
 
 TEST_F(TestNode, InitialPoseBeforeInitialize) {
   ndt_amcl_node_->set_parameter(rclcpp::Parameter{"set_initial_pose", false});
diff --git a/beluga_amcl/test/test_utils/node_testing.hpp b/beluga_amcl/test/test_utils/node_testing.hpp
index 2724dd892..24c3166d0 100644
--- a/beluga_amcl/test/test_utils/node_testing.hpp
+++ b/beluga_amcl/test/test_utils/node_testing.hpp
@@ -149,6 +149,13 @@ class TesterNode : public rclcpp::Node {
     auto scan = sensor_msgs::msg::LaserScan{};
     scan.header.stamp = timestamp;
     scan.header.frame_id = "laser";
+    scan.angle_min = -1.57F;
+    scan.angle_max = 1.57F;
+    scan.angle_increment = 1.57F;
+    scan.time_increment = 0.0F;
+
+    scan.ranges.resize(1);
+    scan.intensities.resize(1);
 
     auto transform_base = geometry_msgs::msg::TransformStamped{};
     transform_base.header.stamp = timestamp;
@@ -175,8 +182,12 @@ class TesterNode : public rclcpp::Node {
     sensor_msgs::PointCloud2Modifier modifier(scan);
 
     modifier.setPointCloud2Fields(
-        3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1,
-        sensor_msgs::msg::PointField::FLOAT32);
+        3,                                              //
+        "x", 1, sensor_msgs::msg::PointField::FLOAT32,  //
+        "y", 1, sensor_msgs::msg::PointField::FLOAT32,  //
+        "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+
+    modifier.resize(1);
 
     auto transform_base = geometry_msgs::msg::TransformStamped{};
     transform_base.header.stamp = timestamp;
diff --git a/beluga_example/params/default_ndt.ros2.yaml b/beluga_example/params/default_ndt.ros2.yaml
index 19e5934c2..2ce3a45ab 100644
--- a/beluga_example/params/default_ndt.ros2.yaml
+++ b/beluga_example/params/default_ndt.ros2.yaml
@@ -35,14 +35,6 @@ ndt_amcl:
     # Upper standard normal quantile for the probability that the error in the
     # estimated distribution is less than pf_err.
     pf_z: 3.0
-    # Fast exponential filter constant, used to filter the average particles weights.
-    # Random particles are added if the fast filter result is larger than the slow filter result
-    # allowing the particle filter to recover from a bad approximation.
-    recovery_alpha_fast: 0.1
-    # Slow exponential filter constant, used to filter the average particles weights.
-    # Random particles are added if the fast filter result is larger than the slow filter result
-    # allowing the particle filter to recover from a bad approximation.
-    recovery_alpha_slow: 0.001
     # Resample will happen after the amount of updates specified here happen.
     resample_interval: 1
     # Minimum angle difference from last resample for resampling to happen again.
@@ -57,9 +49,6 @@ ndt_amcl:
     tf_broadcast: true
     # Transform tolerance allowed.
     transform_tolerance: 1.0
-    # Execution policy used to apply the motion update and importance weight steps.
-    # Valid options: "seq", "par".
-    execution_policy: seq
     # Whether to set initial pose based on parameters.
     # When enabled, particles will be initialized with the specified pose coordinates and covariance.
     set_initial_pose: true
diff --git a/beluga_example/params/default_ndt_3d.ros2.yaml b/beluga_example/params/default_ndt_3d.ros2.yaml
index b406277fc..aa2a60bb6 100644
--- a/beluga_example/params/default_ndt_3d.ros2.yaml
+++ b/beluga_example/params/default_ndt_3d.ros2.yaml
@@ -35,14 +35,6 @@ ndt_amcl:
     # Upper standard normal quantile for the probability that the error in the
     # estimated distribution is less than pf_err.
     pf_z: 3.0
-    # Fast exponential filter constant, used to filter the average particles weights.
-    # Random particles are added if the fast filter result is larger than the slow filter result
-    # allowing the particle filter to recover from a bad approximation.
-    recovery_alpha_fast: 0.1
-    # Slow exponential filter constant, used to filter the average particles weights.
-    # Random particles are added if the fast filter result is larger than the slow filter result
-    # allowing the particle filter to recover from a bad approximation.
-    recovery_alpha_slow: 0.001
     # Resample will happen after the amount of updates specified here happen.
     resample_interval: 1
     # Minimum angle difference from last resample for resampling to happen again.
@@ -57,9 +49,6 @@ ndt_amcl:
     tf_broadcast: true
     # Transform tolerance allowed.
     transform_tolerance: 1.0
-    # Execution policy used to apply the motion update and importance weight steps.
-    # Valid options: "seq", "par".
-    execution_policy: par
     # Whether to set initial pose based on parameters.
     # When enabled, particles will be initialized with the specified pose coordinates and covariance.
     set_initial_pose: true
diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp
index 4f52f9afc..45240e0e9 100644
--- a/beluga_ros/src/amcl.cpp
+++ b/beluga_ros/src/amcl.cpp
@@ -82,7 +82,7 @@ auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_sc
     auto random_state = ranges::compose(beluga::make_from_state<particle_type>, std::ref(map_distribution_));
 
     if (random_state_probability > 0.0) {
-      random_probability_estimator_.reset();
+      random_probability_estimator_.reset();  // LCOV_EXCL_LINE
     }
 
     particles_ |= beluga::views::sample |

From e536e504b43122b14f05fbc46441ea2fa611fea1 Mon Sep 17 00:00:00 2001
From: Nahuel Espinosa <nespinosa@ekumenlabs.com>
Date: Tue, 29 Oct 2024 10:01:17 -0300
Subject: [PATCH 2/2] Remove support for Nav2 differential model name

Signed-off-by: Nahuel Espinosa <nespinosa@ekumenlabs.com>
---
 beluga_amcl/src/ndt_amcl_node.cpp           | 2 +-
 beluga_amcl/src/ndt_amcl_node_3d.cpp        | 2 +-
 beluga_example/params/default_ndt.ros2.yaml | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/beluga_amcl/src/ndt_amcl_node.cpp b/beluga_amcl/src/ndt_amcl_node.cpp
index f3b52950f..f5608306a 100644
--- a/beluga_amcl/src/ndt_amcl_node.cpp
+++ b/beluga_amcl/src/ndt_amcl_node.cpp
@@ -134,7 +134,7 @@ auto NdtAmclNode::get_initial_estimate() const -> std::optional<std::pair<Sophus
 
 auto NdtAmclNode::get_motion_model() const -> beluga::DifferentialDriveModel2d {
   const auto name = get_parameter("robot_model_type").as_string();
-  if (name == kDifferentialModelName || name == kNav2DifferentialModelName) {
+  if (name == kDifferentialModelName) {
     auto params = beluga::DifferentialDriveModelParam{};
     params.rotation_noise_from_rotation = get_parameter("alpha1").as_double();
     params.rotation_noise_from_translation = get_parameter("alpha2").as_double();
diff --git a/beluga_amcl/src/ndt_amcl_node_3d.cpp b/beluga_amcl/src/ndt_amcl_node_3d.cpp
index 957a8bc71..e53938bfb 100644
--- a/beluga_amcl/src/ndt_amcl_node_3d.cpp
+++ b/beluga_amcl/src/ndt_amcl_node_3d.cpp
@@ -174,7 +174,7 @@ auto NdtAmclNode3D::get_initial_estimate() const -> std::optional<std::pair<Soph
 
 auto NdtAmclNode3D::get_motion_model() const -> beluga::DifferentialDriveModel3d {
   const auto name = get_parameter("robot_model_type").as_string();
-  if (name == kDifferentialModelName || name == kNav2DifferentialModelName) {
+  if (name == kDifferentialModelName) {
     auto params = beluga::DifferentialDriveModelParam{};
     params.rotation_noise_from_rotation = get_parameter("alpha1").as_double();
     params.rotation_noise_from_translation = get_parameter("alpha2").as_double();
diff --git a/beluga_example/params/default_ndt.ros2.yaml b/beluga_example/params/default_ndt.ros2.yaml
index 2ce3a45ab..d77aa9681 100644
--- a/beluga_example/params/default_ndt.ros2.yaml
+++ b/beluga_example/params/default_ndt.ros2.yaml
@@ -1,7 +1,7 @@
 ndt_amcl:
   ros__parameters:
     # Odometry motion model type.
-    robot_model_type: nav2_amcl::DifferentialMotionModel
+    robot_model_type: differential_drive
     # Expected process noise in odometry’s rotation estimate from rotation.
     alpha1: 0.002
     # Expected process noise in odometry’s rotation estimate from translation.