Encapsulation of particle used in FastSLAM algorithm. More...
#include <Robots/LoBot/slam/LoParticle.H>
Classes | |
| struct | Viz |
Public Member Functions | |
| Particle (float initial_weight, const std::vector< int > &initial_scan) | |
| Particle (float initial_weight, const std::vector< int > &initial_scan, const boost::shared_ptr< OccGrid > &known_map) | |
| Particle (const Particle &) | |
| Copy. | |
| Particle & | operator= (const Particle &) |
| Assignment. | |
| void | apply_motion_model (const Odometry &control_input, const std::vector< int > &sensor_data) |
| void | apply_sensor_model (const std::vector< int > &range_readings) |
| void | update_map (const std::vector< int > &range_readings) |
| void | normalize (float normalizer) |
| This method normalizes the particle weight. | |
| void | randomize () |
| Viz | viz () const |
| float | weight () const |
| Accessors. | |
| Pose | pose () const |
| const OccGrid & | map () const |
Encapsulation of particle used in FastSLAM algorithm.
Robolocust uses FastSLAM to build an occupancy grid map of its surroundings and record its trajectory (which is used to compare the performance of different LGMD-based avoidance algorithms). This class encapsulates the notion of a particle as required by the FastSLAM algorithm.
Definition at line 79 of file LoParticle.H.
| lobot::Particle::Particle | ( | float | initial_weight, | |
| const std::vector< int > & | initial_scan | |||
| ) |
When a particle is created, the client should supply an initial weight as well as an initial LRF scan (for the scan matching). The particle will start off assuming a pose of (0,0,0) and an occupancy map which has equal probabilities for all locations.
Definition at line 75 of file LoParticle.C.
| lobot::Particle::Particle | ( | float | initial_weight, | |
| const std::vector< int > & | initial_scan, | |||
| const boost::shared_ptr< OccGrid > & | known_map | |||
| ) |
When FastSLAM is configured to perform localization only, i.e., no mapping, the user will specify a map to use and FastSLAM will use only its Monte Carlo Localization component. This constructor takes the same parameters as the normal SLAM mode constructor. In addition, it takes an OccGrid to serve as the common map to be shared by all the particles. The map has to be loaded by this class's client.
Definition at line 85 of file LoParticle.C.
| lobot::Particle::Particle | ( | const Particle & | that | ) |
Copy.
Definition at line 95 of file LoParticle.C.
| void lobot::Particle::apply_motion_model | ( | const Odometry & | control_input, | |
| const std::vector< int > & | sensor_data | |||
| ) |
This method "perturbs" the particle so as to produce a new state hypothesis based on the current control input. The current sensor data is also used because the Robolocust implementation of FastSLAM uses laser range scan matching to correct the raw odometry obtained from the robot's lower layers.
In Bayesian/particle filter parlance, this method implements the prediction step, producing a new state according to the likelihood function P(xt|xt-1,ut).
Definition at line 228 of file LoParticle.C.
References B, lobot::SlamParams::map_extents(), lobot::SlamParams::match_scans(), R, lobot::Transformation::w(), lobot::Pose::x(), and lobot::SlamParams::x_error().
| void lobot::Particle::apply_sensor_model | ( | const std::vector< int > & | range_readings | ) |
This method reweights the particle once it has been "moved" to a new state. The weighting is based on the current sensor observation.
In Bayesian/particle filter parlance, this method implements the correction step, taking the latest observation into account to correct the state predicted by the motion model according to the likelihood function P(xt|zt).
Definition at line 645 of file LoParticle.C.
| void lobot::Particle::normalize | ( | float | normalizer | ) | [inline] |
This method normalizes the particle weight.
Definition at line 184 of file LoParticle.H.
Assignment.
Definition at line 105 of file LoParticle.C.
| void lobot::Particle::randomize | ( | ) |
This function can be used to randomize this particle's pose estimate. It is useful when we want to introduce random particles to help deal with mislocalizations.
Definition at line 120 of file LoParticle.C.
References B, lobot::SlamParams::map_extents(), R, and lobot::Pose::x().
Referenced by lobot::FastSLAM::update().
| void lobot::Particle::update_map | ( | const std::vector< int > & | range_readings | ) |
This method updates the occupancy map based on the latest state hypothesis and laser range finder data. That is, it "evolves" the latest occupancy probabilities according to the likelihood function P(mt|xt,mt-1).
Definition at line 860 of file LoParticle.C.
| Viz lobot::Particle::viz | ( | ) | const [inline] |
Returns the particle's current pose and weight for visualization purposes.
Definition at line 208 of file LoParticle.H.
Referenced by lobot::FastSLAM::viz().
| float lobot::Particle::weight | ( | ) | const [inline] |
Accessors.
Definition at line 188 of file LoParticle.H.
1.6.3