Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy > Class Template Reference

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

#include <amcl_core.hpp>

Public Member Functions

 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. More...
 
void force_update ()
 Force a manual update of the particles on the next iteration of the filter. More...
 
template<class Distribution >
void initialize (Distribution distribution)
 Initialize particles using a custom distribution. More...
 
template<class CovarianceT >
void initialize (state_type pose, CovarianceT covariance)
 Initialize particles with a given pose and covariance. More...
 
const auto & particles () const
 Returns a reference to the current set of particles. More...
 
auto update (state_type control_action, measurement_type measurement) -> std::optional< estimation_type >
 Update particles based on motion and sensor information. More...
 
void update_map (map_type map)
 Update the map used for localization. More...
 

Private Types

using estimation_type = std::invoke_result_t< decltype(beluga::estimate< std::vector< state_type > >), std::vector< state_type > >
 
using map_type = typename SensorModel::map_type
 
using measurement_type = typename SensorModel::measurement_type
 
using particle_type = ParticleType
 
using random_state_generator_type = RandomStateGenerator
 
using spatial_hasher_type = spatial_hash< state_type >
 
using state_type = typename SensorModel::state_type
 

Private Member Functions

decltype(auto) get_random_state_generator () const
 Gets a callable that will produce a random state. More...
 

Private Attributes

beluga::RollingWindow< state_type, 2 > control_action_window_
 
ExecutionPolicy execution_policy_
 
bool force_update_ {true}
 
MotionModel motion_model_
 
AmclParams params_
 
beluga::TupleVector< particle_type > particles_
 
beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_
 
random_state_generator_type random_state_generator_
 
beluga::any_policy< decltype(particles_)> resample_policy_
 
SensorModel sensor_model_
 
spatial_hasher_type spatial_hasher_
 
beluga::any_policy< state_type > update_policy_
 

Detailed Description

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 beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >

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

Template Parameters
MotionModelClass representing a motion model. Must satisfy Beluga named requirements: MotionModel.
SensorModelClass representing a sensor model. Must satisfy Beluga named requirements: SensorModel.
RandomStateGeneratorA 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’.
WeightTType to represent a weight of a particle.
ParticleTypeFull particle type, containing state, weight and possibly other information .
ExecutionPolicyExecution policy for particles processing.

Definition at line 80 of file amcl_core.hpp.

Member Typedef Documentation

◆ estimation_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::estimation_type = std::invoke_result_t<decltype(beluga::estimate<std::vector<state_type> >), std::vector<state_type> >
private

Definition at line 92 of file amcl_core.hpp.

◆ map_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::map_type = typename SensorModel::map_type
private

Definition at line 88 of file amcl_core.hpp.

◆ measurement_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::measurement_type = typename SensorModel::measurement_type
private

Definition at line 86 of file amcl_core.hpp.

◆ particle_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::particle_type = ParticleType
private

Definition at line 85 of file amcl_core.hpp.

◆ random_state_generator_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::random_state_generator_type = RandomStateGenerator
private

Definition at line 90 of file amcl_core.hpp.

◆ spatial_hasher_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::spatial_hasher_type = spatial_hash<state_type>
private

Definition at line 89 of file amcl_core.hpp.

◆ state_type

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>
using beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::state_type = typename SensorModel::state_type
private

Definition at line 87 of file amcl_core.hpp.

Constructor & Destructor Documentation

◆ Amcl()

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>
beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::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 
)
inline

Construct a AMCL instance.

Parameters
motion_modelMotion model instance.
sensor_modelSensor model Instance.
random_state_generatorA callable able to produce random states, optionally based on the current particles state.
spatial_hasherA spatial hasher instance capable of computing a hash out of a particle state.
paramsParameters for AMCL implementation.
execution_policyPolicy to use when processing particles.

Definition at line 105 of file amcl_core.hpp.

Member Function Documentation

◆ force_update()

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>
void beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::force_update ( )
inline

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

Definition at line 204 of file amcl_core.hpp.

◆ get_random_state_generator()

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>
decltype(auto) beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::get_random_state_generator ( ) const
inlineprivate

Gets a callable that will produce a random state.

Definition at line 208 of file amcl_core.hpp.

◆ initialize() [1/2]

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>
template<class Distribution >
void beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::initialize ( Distribution  distribution)
inline

Initialize particles using a custom distribution.

Definition at line 131 of file amcl_core.hpp.

◆ initialize() [2/2]

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>
template<class CovarianceT >
void beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::initialize ( state_type  pose,
CovarianceT  covariance 
)
inline

Initialize particles with a given pose and covariance.

Template Parameters
CovarianceTtype representing a covariance, compliant with state_type.
Exceptions
std::runtime_errorIf the provided covariance is invalid.

Definition at line 145 of file amcl_core.hpp.

◆ particles()

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>
const auto& beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::particles ( ) const
inline

Returns a reference to the current set of particles.

Definition at line 127 of file amcl_core.hpp.

◆ update()

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>
auto beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::update ( state_type  control_action,
measurement_type  measurement 
) -> std::optional<estimation_type>
inline

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_actionControl action.
measurementMeasurement data.
Returns
An optional pair containing the estimated pose and covariance after the update, or std::nullopt if no update was performed.

Definition at line 165 of file amcl_core.hpp.

◆ update_map()

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>
void beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::update_map ( map_type  map)
inline

Update the map used for localization.

Definition at line 150 of file amcl_core.hpp.

Member Data Documentation

◆ control_action_window_

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>
beluga::RollingWindow<state_type, 2> beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::control_action_window_
private

Definition at line 230 of file amcl_core.hpp.

◆ execution_policy_

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>
ExecutionPolicy beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::execution_policy_
private

Definition at line 221 of file amcl_core.hpp.

◆ force_update_

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>
bool beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::force_update_ {true}
private

Definition at line 232 of file amcl_core.hpp.

◆ motion_model_

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>
MotionModel beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::motion_model_
private

Definition at line 219 of file amcl_core.hpp.

◆ params_

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>
AmclParams beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::params_
private

Definition at line 217 of file amcl_core.hpp.

◆ particles_

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>
beluga::TupleVector<particle_type> beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::particles_
private

Definition at line 215 of file amcl_core.hpp.

◆ random_probability_estimator_

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>
beluga::ThrunRecoveryProbabilityEstimator beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::random_probability_estimator_
private

Definition at line 224 of file amcl_core.hpp.

◆ random_state_generator_

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>
random_state_generator_type beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::random_state_generator_
private

Definition at line 228 of file amcl_core.hpp.

◆ resample_policy_

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>
beluga::any_policy<decltype(particles_)> beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::resample_policy_
private

Definition at line 226 of file amcl_core.hpp.

◆ sensor_model_

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>
SensorModel beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::sensor_model_
private

Definition at line 220 of file amcl_core.hpp.

◆ spatial_hasher_

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>
spatial_hasher_type beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::spatial_hasher_
private

Definition at line 223 of file amcl_core.hpp.

◆ update_policy_

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>
beluga::any_policy<state_type> beluga::Amcl< MotionModel, SensorModel, RandomStateGenerator, WeightT, ParticleType, ExecutionPolicy >::update_policy_
private

Definition at line 225 of file amcl_core.hpp.

The documentation for this class was generated from the following file:

beluga
Author(s):
autogenerated on Tue Jul 16 2024 02:59:53