Public Types | Public Member Functions | Private Attributes | List of all members
beluga_ros::Amcl Class Reference

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

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::OccupancyGridmap_distribution_
 
motion_model_variant motion_model_
 
AmclParams params_
 
beluga::TupleVector< particle_typeparticles_
 
beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_
 
beluga::any_policy< decltype(particles_)> resample_policy_
 
sensor_model_variant sensor_model_
 
beluga::spatial_hash< Sophus::SE2dspatial_hasher_
 
beluga::any_policy< Sophus::SE2dupdate_policy_
 

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

Motion model variant type for runtime selection support.

Definition at line 97 of file amcl.hpp.

◆ particle_type

Weighted SE(2) state particle type.

Definition at line 91 of file amcl.hpp.

◆ sensor_model_variant

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
mapOccupancy grid map.
motion_modelVariant of motion model.
sensor_modelVariant of sensor model.
paramsParameters for AMCL implementation.
execution_policyVariant 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_errorIf 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_odomBase pose in the odometry frame.
laser_scanLaser 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_

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:


beluga_ros
Author(s):
autogenerated on Tue Jul 16 2024 03:00:02