/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/amcl.hpp Source File#
Beluga ROS
|
amcl.hpp
Go to the documentation of this file.
116 using execution_policy_variant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
183 beluga::MultivariateUniformDistribution<Sophus::SE2d, beluga_ros::OccupancyGrid> map_distribution_;
Definition: amcl.hpp:99
void initialize_from_map()
Initialize particles using the default map distribution.
Definition: amcl.hpp:155
std::variant< beluga::DifferentialDriveModel2d, beluga::OmnidirectionalDriveModel, beluga::StationaryModel > motion_model_variant
Motion model variant type for runtime selection support.
Definition: amcl.hpp:108
std::variant< std::execution::sequenced_policy, std::execution::parallel_policy > execution_policy_variant
Execution policy variant type for runtime selection support.
Definition: amcl.hpp:116
void update_map(beluga_ros::OccupancyGrid map)
Update the map used for localization.
const auto & particles() const
Returns a reference to the current set of particles.
Definition: amcl.hpp:134
std::tuple< Sophus::SE2d, beluga::Weight > particle_type
Weighted SE(2) state particle type.
Definition: amcl.hpp:102
std::variant< beluga::LikelihoodFieldModel< beluga_ros::OccupancyGrid >, beluga::BeamSensorModel< beluga_ros::OccupancyGrid > > sensor_model_variant
Sensor model variant type for runtime selection support.
Definition: amcl.hpp:113
void initialize(Sophus::SE2d pose, Sophus::Matrix3d covariance)
Initialize particles with a given pose and covariance.
Definition: amcl.hpp:150
void initialize(Distribution distribution)
Initialize particles using a custom distribution.
Definition: amcl.hpp:138
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.
Amcl(beluga_ros::OccupancyGrid map, motion_model_variant motion_model, sensor_model_variant sensor_model, const AmclParams ¶ms, execution_policy_variant execution_policy)
Constructor.
void force_update()
Force a manual update of the particles on the next iteration of the filter.
Definition: amcl.hpp:177
Thin wrapper type for 2D sensor_msgs/LaserScan messages.
Definition: laser_scan.hpp:34
Thin wrapper type for 2D nav_msgs/OccupancyGrid messages.
Definition: occupancy_grid.hpp:47
Implementation of sensor_msgs/LaserScan wrapper type.
constexpr detail::covariance_fn covariance
DifferentialDriveModel< Sophus::SE2d > DifferentialDriveModel2d
Implementation of nav_msgs/OccupancyGrid wrapper type.
Struct containing parameters for the Adaptive Monte Carlo Localization (AMCL) implementation.
Definition: amcl.hpp:47
std::size_t max_particles
Maximum allowed number of particles.
Definition: amcl.hpp:68
std::size_t resample_interval
Number of filter updates required before resampling.
Definition: amcl.hpp:55
double kld_epsilon
Maximum particle filter population error between the true distribution and the estimated distribution...
Definition: amcl.hpp:81
double kld_z
Upper standard normal quantile for , where is the probability that the error in the estimated distri...
Definition: amcl.hpp:85
double alpha_fast
Exponential decay rate for the fast average weight filter, used in deciding when to recover from a ba...
Definition: amcl.hpp:76
double update_min_a
Rotational movement required from last resample for resampling to happen again.
Definition: amcl.hpp:52
double alpha_slow
Exponential decay rate for the slow average weight filter, used in deciding when to recover from a ba...
Definition: amcl.hpp:72
double spatial_resolution_theta
Spatial resolution around the z-axis to create buckets for KLD resampling.
Definition: amcl.hpp:94
double spatial_resolution_x
Spatial resolution along the x-axis to create buckets for KLD resampling.
Definition: amcl.hpp:88
std::size_t min_particles
Minimum allowed number of particles.
Definition: amcl.hpp:65
double spatial_resolution_y
Spatial resolution along the y-axis to create buckets for KLD resampling.
Definition: amcl.hpp:91
bool selective_resampling
Whether to enable selective resampling to help avoid loss of diversity in the particle population....
Definition: amcl.hpp:62
double update_min_d
Translational movement required from last resample for resampling to happen again.
Definition: amcl.hpp:49
Generated by 1.9.1