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 #include #include #include 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 #include -#include - -#include -#include +#include #include -#include "beluga/policies/on_motion.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include 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&> 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, - class ExecutionPolicy = std::execution::sequenced_policy> + class Particle = std::tuple> class Amcl { - static_assert( - std::is_same_v or - std::is_same_v); - - 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; - using random_state_generator_type = RandomStateGenerator; using estimation_type = std::invoke_result_t>; 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(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 - void initialize(state_type pose, CovarianceT covariance) { - initialize(beluga::MultivariateNormalDistribution{pose, covariance}); + template + 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, 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) { - return random_state_generator_; - } else { - return random_state_generator_(particles_); - } - } beluga::TupleVector 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 update_policy_; beluga::any_policy resample_policy_; - random_state_generator_type random_state_generator_; - beluga::RollingWindow 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 -struct NDTCell { +struct NdtCell { static_assert(std::is_floating_point_v, "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 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& tf, const NDTCell& ndt_cell) { + friend NdtCell operator*(const Sophus::SE2& 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& tf, const NDTCell& ndt_cell) { + friend NdtCell operator*(const Sophus::SE3& 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 #include +#include #include #include -#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 -inline NDTCell fit_points(const std::vector>& points) { +inline NdtCell fit_points(const std::vector>& points) { static constexpr double kMinVariance = 1e-5; Eigen::Map> points_view( reinterpret_cast(points.data()), NDim, static_cast(points.size())); @@ -71,14 +71,14 @@ inline NDTCell fit_points(const std::vector{mean, cov}; + return NdtCell{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 -inline std::vector> to_cells( +inline std::vector> to_cells( const std::vector>& points, const double resolution) { static constexpr int kMinPointsPerCell = 5; @@ -86,7 +86,7 @@ inline std::vector> to_cells( const Eigen::Map> points_view( reinterpret_cast(points.data()), NDim, static_cast(points.size())); - std::vector> ret; + std::vector> ret; ret.reserve(static_cast(points_view.cols()) / kMinPointsPerCell); std::unordered_map, std::vector>, CellHasher> 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 -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 -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; + using param_type = NdtModelParam; - /// 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 -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 +NdtMap load_from_hdf5(const std::filesystem::path& path_to_hdf5_file) { static_assert( - std::is_same_v or - std::is_same_v); + std::is_same_v or + std::is_same_v); static_assert( - std::is_same_v or - std::is_same_v); + std::is_same_v or + std::is_same_v); - 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(i))] = - NDTCell{means_matrix.col(static_cast(i)), covariances.at(i)}; + NdtCell{means_matrix.col(static_cast(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 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 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::pi() / 2}, Eigen::Vector2d::Zero()}; Eigen::Matrix 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>>; -using sparse_grid_3d_t = SparseValueGrid3>>; +using sparse_grid_2d_t = SparseValueGrid2>>; +using sparse_grid_3d_t = SparseValueGrid3>>; } // 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 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 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()] = 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("./test_data/turtlebot3_world.hdf5"); ASSERT_EQ(ndt_map_representation.size(), 30UL); } -TEST(NDTSensorModel2DTests, LoadFromHDF5NonExistingFile) { +TEST(NdtSensorModel2DTests, LoadFromHDF5NonExistingFile) { ASSERT_THROW(io::load_from_hdf5("bad_file.hdf5"), std::invalid_argument); } -TEST(NDTSensorModel3DTests, LoadFromHDF5NonExistingFile) { +TEST(NdtSensorModel3DTests, LoadFromHDF5NonExistingFile) { ASSERT_THROW(io::load_from_hdf5("bad_file.hdf5"), std::invalid_argument); } -TEST(NDTSensorModel3DTests, LoadFromHDF5HappyPath) { +TEST(NdtSensorModel3DTests, LoadFromHDF5HappyPath) { const auto ndt_map_representation = io::load_from_hdf5("./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 #include +#include + +#include + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcpp" #include #pragma GCC diagnostic pop -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include +#include #include -#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 -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include #include #include -#include -#include -#include #include -#include -#include -#include +#include -#include -#include -#include #include -#include -#include -#include - -#include -#include "beluga_amcl/ros2_common.hpp" +#include #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcpp" #include #pragma GCC diagnostic pop +#include +#include +#include +#include +#include +#include + /** * \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>>; - -/// Type of a particle-dependent random state generator. -using RandomStateGenerator = std::function::state_type()>( - const beluga::TupleVector::state_type, beluga::Weight>>)>; +using NdtMap = + beluga::SparseValueGrid2>>; -/// Partial specialization of the core AMCL pipeline for convinience. -template +/// Instantiatiation of the core AMCL pipeline with the NDT sensor model. using NdtAmcl = beluga::Amcl< - MotionModel, - beluga::NDTSensorModel, - RandomStateGenerator, - beluga::Weight, - std::tuple::state_type, beluga::Weight>, - ExecutionPolicy>; - -/// All combinations of supported NDT AMCL variants. -using NdtAmclVariant = std::variant< - NdtAmcl, // - NdtAmcl, // - NdtAmcl, // - NdtAmcl, // - NdtAmcl, // - NdtAmcl // - >; - -/// Supported motion models. -using MotionModelVariant = - std::variant; - -/// Supported execution policies. -using ExecutionPolicyVariant = std::variant; + beluga::DifferentialDriveModel2d, // + beluga::NdtSensorModel, // + std::tuple>; /// 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>; /// 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 get_sensor_model() const; + auto get_sensor_model() const -> beluga::NdtSensorModel; /// Instantiate particle filter given an initial occupancy grid map and the current parametrization. - auto make_particle_filter() const -> std::unique_ptr; + auto make_particle_filter() const -> std::unique_ptr; /// 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 particle_filter_ = nullptr; + std::unique_ptr particle_filter_ = nullptr; /// Last known pose estimate, if any. std::optional> 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 -#include -#include -#include - -#include -#include - -#include #include #include -#include #include -#include -#include -#include -#include -#include -#include -#include +#include -#include -#include "beluga_amcl/ros2_common.hpp" +#include +#include #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcpp" #include #pragma GCC diagnostic pop +#include +#include +#include +#include +#include +#include + /** * \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>>; +using NdtMap = + beluga::SparseValueGrid3>>; -/// State type for the particles. -using StateType = typename beluga::NDTSensorModel::state_type; - -/// Type of a particle-dependent random state generator. -using RandomStateGenerator = std::function; - -/// Partial specialization of the core AMCL pipeline for convinience. -template +/// Instantiatiation of the core AMCL pipeline with the NDT sensor model. using NdtAmcl = beluga::Amcl< - MotionModel, - beluga::NDTSensorModel, - RandomStateGenerator, - beluga::Weight, - std::tuple, - ExecutionPolicy>; - -/// All combinations of supported 3D NDT AMCL variants. -using NdtAmclVariant = std::variant< - NdtAmcl, // - NdtAmcl>; - -/// Supported motion models. -using MotionModelVariant = std::variant; + beluga::DifferentialDriveModel3d, // + beluga::NdtSensorModel, // + std::tuple>; /// 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>; /// 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 get_sensor_model() const; + auto get_sensor_model() const -> beluga::NdtSensorModel; /// Instantiate particle filter given an initial occupancy grid map and the current parametrization. - auto make_particle_filter() const -> std::unique_ptr; + auto make_particle_filter() const -> std::unique_ptr; /// 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 particle_filter_; + std::unique_ptr particle_filter_; /// Last known pose estimate, if any. std::optional> 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 -#include -#include #include -#include #include #include -#include #include #include -#include -#include #include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcpp" -#include -#pragma GCC diagnostic pop +#include #include -#include #include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include + +#include #include #include -#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 -#include -#include #include #include #include -#include #include #include -#include #include -#include #include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcpp" -#include -#pragma GCC diagnostic pop - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include -#include "beluga_amcl/ndt_amcl_node.hpp" -#include "beluga_amcl/ros2_common.hpp" + +#include namespace beluga_amcl { @@ -178,81 +132,52 @@ auto NdtAmclNode::get_initial_estimate() const -> std::optional 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 NdtAmclNode::get_sensor_model() const { - auto params = beluga::NDTModelParam2d{}; +auto NdtAmclNode::get_sensor_model() const -> beluga::NdtSensorModel { + 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{ - params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; + return beluga::NdtSensorModel{ + params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; } -auto NdtAmclNode::make_particle_filter() const -> std::unique_ptr { - 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(get_parameter("resample_interval").as_int()); - params.selective_resampling = get_parameter("selective_resampling").as_bool(); - params.min_particles = static_cast(get_parameter("min_particles").as_int()); - params.max_particles = static_cast(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( - 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(std::move(amcl)); +auto NdtAmclNode::make_particle_filter() const -> std::unique_ptr { + 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(get_parameter("resample_interval").as_int()); + params.selective_resampling = get_parameter("selective_resampling").as_bool(); + params.min_particles = static_cast(get_parameter("min_particles").as_int()); + params.max_particles = static_cast(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( + 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( + 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(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(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; - 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::pairinitialize(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::pairparticles().size(), // + pose.translation().x(), // + pose.translation().y(), // + pose.so2().log()); return true; } + } // namespace beluga_amcl #include 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 -#include -#include #include -#include -#include #include #include #include -#include -#include -#include -#include #include #include -#include #include -#include #include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcpp" -#include -#pragma GCC diagnostic pop - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include #include #include -#include "beluga_amcl/ndt_amcl_node_3d.hpp" -#include "beluga_amcl/ros2_common.hpp" + +#include namespace beluga_amcl { @@ -224,62 +172,51 @@ auto NdtAmclNode3D::get_initial_estimate() const -> std::optional 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{params}; + return beluga::DifferentialDriveModel3d{params}; } throw std::invalid_argument(std::string("Invalid motion model: ") + name); } -beluga::NDTSensorModel NdtAmclNode3D::get_sensor_model() const { - auto params = beluga::NDTModelParam3d{}; +auto NdtAmclNode3D::get_sensor_model() const -> beluga::NdtSensorModel { + 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{ - params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; + return beluga::NdtSensorModel{ + params, beluga::io::load_from_hdf5(get_parameter("map_path").as_string())}; } -auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr { - 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(get_parameter("resample_interval").as_int()); - params.selective_resampling = get_parameter("selective_resampling").as_bool(); - params.min_particles = static_cast(get_parameter("min_particles").as_int()); - params.max_particles = static_cast(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( - 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(std::move(amcl)); + +auto NdtAmclNode3D::make_particle_filter() const -> std::unique_ptr { + 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(get_parameter("resample_interval").as_int()); + params.selective_resampling = get_parameter("selective_resampling").as_bool(); + params.min_particles = static_cast(get_parameter("min_particles").as_int()); + params.max_particles = static_cast(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( + get_parameter("spatial_resolution_x").as_double(), // + get_parameter("spatial_resolution_theta").as_double()); + + return std::make_unique( + 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(update_duration).count()); } @@ -428,9 +354,7 @@ bool NdtAmclNode3D::initialize_from_estimate(const std::pairinitialize(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::pairparticles().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> {}; -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_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