Class PFLocalizationCore

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public mrpt::system::COutputLogger

Class Documentation

class PFLocalizationCore : public mrpt::system::COutputLogger

The core C++ non-ROS part of the particle filter localization algorithm. It features a 2D SE(2) and 3D SE(3) particle filter, all in one, depending on parameters.

Main API

void init_from_yaml(const mrpt::containers::yaml &pf_params, const mrpt::containers::yaml &relocalization_pipeline)

Load all params from a YAML source. This method loads all required params and put the system from UNINITIALIZED into TO_BE_INITIALIZED.

void on_observation(const mrpt::obs::CObservation::Ptr &obs)

Must be called for each new observation that arrives from the robot: odometry, 2D or 3D lidar, GPS, etc.

void step()

The main API call: executes one PF step, taking into account all the parameters and observations gathered so far, updates the optional GUI, etc.

Must be called at a timely fashion. Upon return, the PF state can be grabbed via the public methods below.

void reset()

Reset the object to the initial state as if created from scratch

bool set_map_from_simple_map(const std::string &map_config_ini_file, const std::string &simplemap_file)

Defines the map to use from a pair of files: an MRPT metric map definition INI file, and a .simplemap file with sensor observations.

Returns:

true on success, false on any error.

void set_map_from_metric_map(const mrpt::maps::CMultiMetricMap::Ptr &metricMap, const std::optional<mp2p_icp::metric_map_t::Georeferencing> &georeferencing = std::nullopt, const std::vector<std::string> &layerNames = {})

Defines the map to use from a multimetric map, which may contain gridmaps, pointclouds, etc.

void set_map_from_metric_map(const mp2p_icp::metric_map_t &mm)

Defines the map to use from a metric_map_t map, with optional georeferencing.

void relocalize_here(const mrpt::poses::CPose3DPDFGaussian &pose)
bool input_queue_has_odometry()
std::optional<mrpt::Clock::time_point> input_queue_last_stamp()
void set_fake_odometry_increment(const mrpt::poses::CPose3D &incrPose)
inline State getState() const
inline const Parameters getParams()

Returns a copy (it is intentional) of the parameters at this moment

mrpt::poses::CPose3DPDFParticles::Ptr getLastPoseEstimation() const

Returns the last filter estimate, or empty ptr if never run yet. Multi thread safe.

Public Types

enum class State : uint8_t

The state of the particle filter. The “loop()” method will look at this to know what to do.

Values:

enumerator UNINITIALIZED

The filter has been neither initialized nor parameters/map loaded.

enumerator TO_BE_INITIALIZED

Next iteration must create samples at the initial pose PDF. All parameters and map(s) are correctly loaded.

enumerator RUNNING

Running as usual, the robot can move and particles will update.

Public Functions

PFLocalizationCore()
virtual ~PFLocalizationCore() = default

Protected Functions

inline mrpt::obs::CObservationGPS::Ptr get_last_gnss_obs() const

Protected Attributes

Parameters params_
struct InternalState

Public Functions

InternalState()

Public Members

State fsm_state = State::UNINITIALIZED
mrpt::maps::CMultiMetricMap::Ptr metric_map

Empty=uninitialized.

std::optional<mp2p_icp::metric_map_t::Georeferencing> georeferencing
mrpt::bayes::CParticleFilter pf

interface for particle filters

mrpt::bayes::CParticleFilter::TParticleFilterStats pf_stats
std::optional<mrpt::slam::CMonteCarloLocalization2D> pdf2d

The filter:

std::optional<mrpt::slam::CMonteCarloLocalization3D> pdf3d
mrpt::Clock::time_point time_last_update

Timestamp of the last update (default=INVALID)

mrpt::obs::CObservationOdometry::Ptr last_odom
std::vector<mrpt::obs::CObservation::Ptr> pendingObs

Observations in the queue since the last run. This field is protected by an independent mutex.

mrpt::poses::CPose3DPDFParticles::Ptr lastResult

The last state of the filter, for sending as a copy to the user API

std::optional<mrpt::poses::CPose3D> nextFakeOdometryIncrPose
mrpt::pimpl<Relocalization> pendingRelocalization
struct Parameters

Parameters the filter will use to initialize or to run. The ROS node will overwrite here when reading params from YAML file, CLI, or service-based changes.

Public Functions

Parameters()
~Parameters() = default
void load_from(const mrpt::containers::yaml &params)

This method loads all parameters from the YAML, except the metric_map (handled in parent class):

Public Members

std::optional<mrpt::poses::CPose3DPDFGaussian> initial_pose

initial pose used to intialize the filter. Empty=not set yet.

std::set<std::string> metric_map_use_only_these_layers
mrpt::maps::CMultiMetricMap::Ptr metric_map

Empty=uninitialized.

std::optional<mp2p_icp::metric_map_t::Georeferencing> georeferencing
std::vector<std::string> metric_map_layer_names
bool gui_enable = true

Shows a custom MRPT GUI with the PF and map state Can be changed at any moment.

bool use_se3_pf = false

If false (default), will use 2D (SE(2)) particle filter. Otherwise, the 3D mode (SE(3)) is enabled. Can be changed while state = UNINITIALIZED.

bool gui_camera_follow_robot = true

If gui_enable==true, makes the camera to follow the mean of the particles. Can be changed at any moment.

mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_2d

For SE(2) mode: Uncertainty motion model for regular odometry-based motion. Can be changed at any moment.

mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_no_odom_2d

For SE(2) mode: Uncertainty motion model to use when NO odometry has been received. Can be changed at any moment.

mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motion_model_3d

For SE(3) mode: Uncertainty motion model for regular odometry-based motion. Can be changed at any moment.

mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motion_model_no_odom_3d

For SE(3) mode: Uncertainty motion model to use when NO odometry has been received. Can be changed at any moment.

mrpt::bayes::CParticleFilter::TParticleFilterOptions pf_options

All the PF parameters: algorithm, number of samples, dynamic samples, etc. Can be changed while state = UNINITIALIZED.

mrpt::slam::TKLDParams kld_options

Dynamic sampling-related parameters. Can be changed at any moment.

std::optional<mrpt::maps::CPointsMap::TLikelihoodOptions> override_likelihood_point_maps
std::optional<mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions> override_likelihood_gridmaps
unsigned int initial_particles_per_m2 = 10

Number of particles/m² to use upon initialization. Can be changed while state = UNINITIALIZED.

bool initialize_from_gnss = false

If true, the particles will be initialized according to the first incomming GNSS observation, once the map has been also received.

Note

This requires a georeferencied metric_map_t.

uint32_t samples_drawn_from_gnss = 20

If >0, new tentative particles will be generated from GNSS data, to help re-localizing if using georeferenced maps:

double gnss_samples_num_sigmas = 6.0

If samples_drawn_from_gnss is enabled, the number of standard deviations (“sigmas”) to use as the area in which to draw random samples around the GNSS prediction:

double relocalize_num_sigmas = 3.0

The number of standard deviations (“sigmas”) to use as the area in which to draw random samples around the input initialization pose (when NOT using GNSS as input)

unsigned int relocalization_initial_divisions_xy = 4
unsigned int relocalization_initial_divisions_phi = 4
unsigned int relocalization_min_sample_copies_per_candidate = 4
double relocalization_resolution_xy = 0.50
double relocalization_resolution_phi = 0.20
double relocalization_minimum_icp_quality = 0.50
double relocalization_icp_sigma = 5.0
mp2p_icp::ICP::Ptr relocalization_icp
mp2p_icp::Parameters relocalization_icp_params
mp2p_icp_filters::FilterPipeline relocalization_obs_filter
mp2p_icp::ParameterSource paramSource