Class Amcl

Class Documentation

class Amcl

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

Public Types

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

Weighted SE(2) state particle type.

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

Motion model variant type for runtime selection support.

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

Sensor model variant type for runtime selection support.

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

Execution policy variant type for runtime selection support.

Public Functions

Amcl(beluga_ros::OccupancyGrid map, motion_model_variant motion_model, sensor_model_variant sensor_model, const AmclParams &params, execution_policy_variant execution_policy)

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.

inline const auto &particles() const

Returns a reference to the current set of particles.

inline const auto &likelihood_field() const

Returns a reference to the current likelihood field.

inline auto likelihood_field_origin() const

Returns the current likelihood field origin transform.

inline bool has_likelihood_field() const

Check if the sensor model bears a likelihood field.

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

Initialize particles using a custom distribution.

inline void initialize(Sophus::SE2d pose, Sophus::Matrix3d covariance)

Initialize particles with a given pose and covariance.

Throws:

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

inline void initialize_from_map()

Initialize particles using the default map distribution.

void update_map(beluga_ros::OccupancyGrid map)

Update the map used for localization.

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

Update particles using laser scan data.

This method transforms laser scan data from polar to cartesian coordinates in the robot base frame. Then forwards to the generic update() method which performs the particle filter update step based on motion and sensor information. See the detailed description of the general update() method below.

See also

update(Sophus::SE2d, std::vector<std::pair<double, double>>&&)

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.

auto update(Sophus::SE2d base_pose_in_odom, beluga_ros::SparsePointCloud3f point_cloud) -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>

Update particles using point cloud data.

This method transforms and projects point cloud data onto the z = 0 plane of the robot base frame. Then forwards the data to the generic update() method that performs the particle filter update step using motion and sensor information.

See also

update(Sophus::SE2d, std::vector<std::pair<double, double>>&&)

Parameters:

  • base_pose_in_odom – Base pose in the odometry frame.

  • point_cloud – Point cloud measurement in the sensor frame; points are projected onto the z=0 plane.

Returns:

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

auto update(Sophus::SE2d base_pose_in_odom, std::vector<std::pair<double, double>> &&measurement) -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>

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.

  • measurement – A vector of 2D points representing the sensor measurement in the base frame.

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.