#include <amcl.hpp>

Public Types
using	execution_policy_variant = std::variant< std::execution::sequenced_policy, std::execution::parallel_policy >
	Execution policy variant type for runtime selection support. More...

using	motion_model_variant = std::variant< beluga::DifferentialDriveModel2d, beluga::OmnidirectionalDriveModel, beluga::StationaryModel >
	Motion model variant type for runtime selection support. More...

using	particle_type = std::tuple< Sophus::SE2d, beluga::Weight >
	Weighted SE(2) state particle type. More...

using	sensor_model_variant = std::variant< beluga::LikelihoodFieldModel< beluga_ros::OccupancyGrid >, beluga::BeamSensorModel< beluga_ros::OccupancyGrid > >
	Sensor model variant type for runtime selection support. More...

Public Member Functions
	Amcl (beluga_ros::OccupancyGrid map, motion_model_variant motion_model, sensor_model_variant sensor_model, const AmclParams &params=AmclParams(), execution_policy_variant execution_policy=std::execution::seq)
	Constructor. 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...

void	initialize (Sophus::SE2d pose, Sophus::Matrix3d covariance)
	Initialize particles with a given pose and covariance. More...

void	initialize_from_map ()
	Initialize particles using the default map distribution. More...

const auto &	particles () const
	Returns a reference to the current set of particles. More...

auto	update (Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_scan) -> std::optional< std::pair< Sophus::SE2d, Sophus::Matrix3d >>
	Update particles based on motion and sensor information. More...

void	update_map (beluga_ros::OccupancyGrid map)
	Update the map used for localization. More...

Detailed Description

Implementation of the 2D Adaptive Monte Carlo Localization (AMCL) algorithm. Generic two-dimensional implementation of the Adaptive Monte Carlo Localization (AMCL) algorithm in 2D.

Definition at line 88 of file amcl.hpp.

Member Typedef Documentation

◆ execution_policy_variant

using beluga_ros::Amcl::execution_policy_variant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>

Execution policy variant type for runtime selection support.

Definition at line 105 of file amcl.hpp.

◆ motion_model_variant

using beluga_ros::Amcl::motion_model_variant = std::variant< beluga::DifferentialDriveModel2d, beluga::OmnidirectionalDriveModel, beluga::StationaryModel>

Motion model variant type for runtime selection support.

Definition at line 97 of file amcl.hpp.

◆ particle_type

using beluga_ros::Amcl::particle_type = std::tuple<Sophus::SE2d, beluga::Weight>

Weighted SE(2) state particle type.

Definition at line 91 of file amcl.hpp.

◆ sensor_model_variant

using beluga_ros::Amcl::sensor_model_variant = std::variant< beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>, beluga::BeamSensorModel<beluga_ros::OccupancyGrid> >

Sensor model variant type for runtime selection support.

Definition at line 102 of file amcl.hpp.

Constructor & Destructor Documentation

◆ Amcl()

beluga_ros::Amcl::Amcl	(	beluga_ros::OccupancyGrid	map,
		motion_model_variant	motion_model,
		sensor_model_variant	sensor_model,
		const AmclParams &	params = `AmclParams()`,
		execution_policy_variant	execution_policy = `std::execution::seq`
	)

inline

Constructor.

Parameters

map	Occupancy grid map.
motion_model	Variant of motion model.
sensor_model	Variant of sensor model.
params	Parameters for AMCL implementation.
execution_policy	Variant of execution policy.

Definition at line 115 of file amcl.hpp.

Member Function Documentation

◆ force_update()

void beluga_ros::Amcl::force_update ( )

inline

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

Definition at line 230 of file amcl.hpp.

◆ initialize() [1/2]

template<class Distribution >

void beluga_ros::Amcl::initialize ( Distribution distribution )

inline

Initialize particles using a custom distribution.

Definition at line 140 of file amcl.hpp.

◆ initialize() [2/2]

void beluga_ros::Amcl::initialize	(	Sophus::SE2d	pose,
		Sophus::Matrix3d	covariance
	)

inline

Initialize particles with a given pose and covariance.

Exceptions

std::runtime_error If the provided covariance is invalid.

Definition at line 152 of file amcl.hpp.

◆ initialize_from_map()

void beluga_ros::Amcl::initialize_from_map ( )

inline

Initialize particles using the default map distribution.

Definition at line 157 of file amcl.hpp.

◆ particles()

const auto& beluga_ros::Amcl::particles ( ) const

inline

Returns a reference to the current set of particles.

Definition at line 136 of file amcl.hpp.

◆ update()

auto beluga_ros::Amcl::update	(	Sophus::SE2d	base_pose_in_odom,
		beluga_ros::LaserScan	laser_scan
	)		-> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>

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, 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

base_pose_in_odom	Base pose in the odometry frame.
laser_scan	Laser scan 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 178 of file amcl.hpp.

◆ update_map()

void beluga_ros::Amcl::update_map ( beluga_ros::OccupancyGrid map )

inline

Update the map used for localization.

Definition at line 160 of file amcl.hpp.

Member Data Documentation

◆ control_action_window_

beluga::RollingWindow<Sophus::SE2d, 2> beluga_ros::Amcl::control_action_window_

private

Definition at line 246 of file amcl.hpp.

◆ execution_policy_

execution_policy_variant beluga_ros::Amcl::execution_policy_

private

Definition at line 239 of file amcl.hpp.

◆ force_update_

bool beluga_ros::Amcl::force_update_ {true}

private

Definition at line 248 of file amcl.hpp.

◆ map_distribution_

beluga::MultivariateUniformDistribution<Sophus::SE2d, beluga_ros::OccupancyGrid> beluga_ros::Amcl::map_distribution_

private

Definition at line 236 of file amcl.hpp.

◆ motion_model_

motion_model_variant beluga_ros::Amcl::motion_model_

private

Definition at line 237 of file amcl.hpp.

◆ params_

AmclParams beluga_ros::Amcl::params_

private

Definition at line 235 of file amcl.hpp.

◆ particles_

beluga::TupleVector<particle_type> beluga_ros::Amcl::particles_

private

Definition at line 233 of file amcl.hpp.

◆ random_probability_estimator_

beluga::ThrunRecoveryProbabilityEstimator beluga_ros::Amcl::random_probability_estimator_

private

Definition at line 242 of file amcl.hpp.

◆ resample_policy_

beluga::any_policy<decltype(particles_)> beluga_ros::Amcl::resample_policy_

private

Definition at line 244 of file amcl.hpp.

◆ sensor_model_

sensor_model_variant beluga_ros::Amcl::sensor_model_

private

Definition at line 238 of file amcl.hpp.

◆ spatial_hasher_

beluga::spatial_hash<Sophus::SE2d> beluga_ros::Amcl::spatial_hasher_

private

Definition at line 241 of file amcl.hpp.

◆ update_policy_

beluga::any_policy<Sophus::SE2d> beluga_ros::Amcl::update_policy_

private

Definition at line 243 of file amcl.hpp.

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

amcl.hpp

Private Attributes
beluga::RollingWindow< Sophus::SE2d, 2 >	control_action_window_

execution_policy_variant	execution_policy_

bool	force_update_ {true}

beluga::MultivariateUniformDistribution< Sophus::SE2d, beluga_ros::OccupancyGrid >	map_distribution_

motion_model_variant	motion_model_

AmclParams	params_

beluga::TupleVector< particle_type >	particles_

beluga::ThrunRecoveryProbabilityEstimator	random_probability_estimator_

beluga::any_policy< decltype(particles_)>	resample_policy_

sensor_model_variant	sensor_model_

beluga::spatial_hash< Sophus::SE2d >	spatial_hasher_

beluga::any_policy< Sophus::SE2d >	update_policy_

Public Types

Public Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ execution_policy_variant

◆ motion_model_variant

◆ particle_type

◆ sensor_model_variant

Constructor & Destructor Documentation

◆ Amcl()

Member Function Documentation

◆ force_update()

◆ initialize() [1/2]

◆ initialize() [2/2]

◆ initialize_from_map()

◆ particles()

◆ update()

◆ update_map()

Member Data Documentation

◆ control_action_window_

◆ execution_policy_

◆ force_update_

◆ map_distribution_

◆ motion_model_

◆ params_

◆ particles_

◆ random_probability_estimator_

◆ resample_policy_

◆ sensor_model_

◆ spatial_hasher_

◆ update_policy_