Template Class Amcl

Class Documentation

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 Amcl

Implementation of the Adaptive Monte Carlo Localization (AMCL) algorithm.

Template Parameters:

  • MotionModel – Class representing a motion model. Must satisfy Beluga named requirements: MotionModel.

  • SensorModel – Class representing a sensor model. Must satisfy Beluga named requirements: SensorModel.

  • 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.

  • WeightT – Type to represent a weight of a particle.

  • ParticleType – Full particle type, containing state, weight and possibly other information .

  • ExecutionPolicy – Execution policy for particles processing.

Public Functions

inline 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)

Construct a AMCL instance.

Parameters:

  • motion_model – Motion model instance.

  • sensor_model – Sensor model Instance.

  • random_state_generator – A callable able to produce random states, optionally based on the current particles state.

  • spatial_hasher – A spatial hasher instance capable of computing a hash out of a particle state.

  • params – Parameters for AMCL implementation.

  • execution_policy – Policy to use when processing particles.

inline const auto &particles() const

Returns a reference to the current set of particles.

template<class Distribution>
inline void initialize(Distribution distribution)

Initialize particles using a custom distribution.

template<class CovarianceT>
inline void initialize(state_type pose, CovarianceT covariance)

Initialize particles with a given pose and covariance.

Template Parameters:

CovarianceT – type representing a covariance, compliant with state_type.

Throws:

std::runtime_error – If the provided covariance is invalid.

inline void update_map(map_type map)

Update the map used for localization.

inline auto update(state_type control_action, measurement_type measurement) -> std::optional<estimation_type>

Update particles based on motion and sensor information.

This method performs a particle filter update step using motion and sensor data. It evaluates whether an update is necessary based on the configured update policy and the force_update flag. If an update is required, or if it was forced via force_update(), the motion model and sensor model updates are applied to the particles, and the particle weights are adjusted accordingly. Also, according to the configured resampling policy, the particles are resampled to maintain diversity and prevent degeneracy.

Parameters:

  • control_action – Control action.

  • measurement – Measurement data.

Returns:

An optional pair containing the estimated pose and covariance after the update, or std::nullopt if no update was performed.

inline void force_update()

Force a manual update of the particles on the next iteration of the filter.