CGR Localization
 All Classes Namespaces Files Functions Variables Macros Pages
Classes | Public Types | Public Member Functions | Protected Attributes | List of all members
VectorLocalization2D Class Reference

#include <vectorparticlefilter.h>

Collaboration diagram for VectorLocalization2D:
Collaboration graph
[legend]

Classes

struct  EvalValues
 
class  LidarParams
 
struct  MotionModelParams
 
class  PointCloudParams
 

Public Types

enum  Resample { NaiveResampling, LowVarianceResampling, SensorResettingResampling }
 

Public Member Functions

 VectorLocalization2D (const char *_mapsFolder)
 
 VectorLocalization2D (int _numParticles)
 
void setParams (MotionModelParams _predictParams, LidarParams _lidarUpdateParams)
 Sets Particle Filter LIDAR parameters.
 
void loadAtlas ()
 Loads All the floor maps listed in atlas.txt.
 
void initialize (int _numParticles, const char *mapName, vector2f loc, float angle, float locationUncertainty=0.0, float angleUncertainty=0.0)
 Initialise arrays, and sets initial location to.
 
void predict (float dx, float dy, float dtheta, const VectorLocalization2D::MotionModelParams &motionParams)
 Predict step of the particle filter. Samples from the motion model.
 
void refineLidar (const VectorLocalization2D::LidarParams &lidarParams)
 Refine proposal distribution based on LIDAR observations.
 
void refinePointCloud (const vector< vector2f > &pointCloud, const vector< vector2f > &pointNormals, const VectorLocalization2D::PointCloudParams &pointCloudParams)
 Refine proposal distribution based on Point Cloud observations.
 
void updateLidar (const VectorLocalization2D::LidarParams &lidarParams, const VectorLocalization2D::MotionModelParams &motionParams)
 Update distribution based on LIDAR observations.
 
void updatePointCloud (vector< vector2f > &pointCloud, vector< vector2f > &pointNormals, const VectorLocalization2D::MotionModelParams &motionParams, const VectorLocalization2D::PointCloudParams &pointCloudParams)
 Update distribution based on Point Cloud observations.
 
void resample (Resample type=LowVarianceResampling)
 Resample distribution.
 
void predictParticle (Particle2D &p, float dx, float dy, float dtheta, const VectorLocalization2D::MotionModelParams &motionParams)
 Predict particle motion by sampling from the motion model.
 
void refineLocationLidar (vector2f &loc, float &angle, float &initialWeight, float &finalWeight, const VectorLocalization2D::LidarParams &lidarParams, const std::vector< Vector2f > &laserPoints)
 Refine a single location hypothesis based on a LIDAR observation.
 
void refineLocationPointCloud (vector2f &loc, float &angle, float &initialWeight, float &finalWeight, const vector< vector2f > &pointCloud, const vector< vector2f > &pointNormals, const VectorLocalization2D::PointCloudParams &pointCloudParams)
 Refine a single location hypothesis based on a Point Cloud observation.
 
void computeParticleWeights (vector2f deltaLoc, float deltaAngle, vector2f minLocStdDev, float minAngleStdDev, const VectorLocalization2D::MotionModelParams &motionParams)
 
Vector2f attractorFunction (line2f l, Vector2f p, float attractorRange, float margin=0)
 Attractor function used for refining location hypotheses.
 
Vector2f observationFunction (line2f l, Vector2f p)
 Observation function for a single ray.
 
void getPointCloudGradient (vector2f loc, float angle, vector2f &locGrad, float &angleGrad, const std::vector< vector2f > &pointCloud, const std::vector< vector2f > &pointNormals, float &logWeight, const VectorLocalization2D::PointCloudParams &pointCloudParams, const std::vector< int > &lineCorrespondences, const std::vector< line2f > &lines)
 Gradient based on pointCloud observation.
 
void getLidarGradient (vector2f loc, float angle, vector2f &locGrad, float &angleGrad, float &logWeight, VectorLocalization2D::LidarParams lidarParams, const vector< Vector2f > &laserPoints, const vector< int > &lineCorrespondences, const vector< line2f > &lines)
 Gradient based on LIDAR observation.
 
float observationWeightLidar (vector2f loc, float angle, const VectorLocalization2D::LidarParams &lidarParams, const std::vector< Vector2f > &laserPoints)
 Observation likelihood based on LIDAR obhservation.
 
float observationWeightPointCloud (vector2f loc, float angle, vector< vector2f > &pointCloud, vector< vector2f > &pointNormals, const PointCloudParams &pointCloudParams)
 Observation likelihood based on point cloud obhservation.
 
float motionModelWeight (vector2f loc, float angle, const VectorLocalization2D::MotionModelParams &motionParams)
 Probability of specified pose corresponding to motion model.
 
void setLocation (vector2f loc, float angle, const char *map, float locationUncertainty, float angleUncertainty)
 Set pose with specified uncertainty.
 
void setLocation (vector2f loc, float angle, float locationUncertainty, float angleUncertainty)
 Set pose and map with specified uncertainty.
 
void setMap (const char *map)
 Switch to a different map.
 
void lowVarianceResample ()
 Resample particles using low variance resampling.
 
void naiveResample ()
 Resample particles using naive resampling.
 
void computeLocation (vector2f &loc, float &angle)
 Compute the maximum likelihood location based on particle spread.
 
const char * getCurrentMapName ()
 Returns the current map name.
 
Particle2D createParticle (VectorMap *map, vector2f loc, float angle, float locationUncertainty, float angleUncertainty)
 Creates a particle with the specified properties.
 
void saveRunLog (FILE *f)
 Write to file run statistics about particle distribution.
 
void saveProfilingStats (FILE *f)
 Write to file riun-time profiling information.
 
void drawDisplay (vector< float > &lines_p1x, vector< float > &lines_p1y, vector< float > &lines_p2x, vector< float > &lines_p2y, vector< uint32_t > &lines_color, vector< float > &points_x, vector< float > &points_y, vector< uint32_t > &points_color, vector< float > &circles_x, vector< float > &circles_y, vector< uint32_t > &circles_color, float scale=1.0)
 Compile lists of drawing primitives that can be visualized for debugging purposes.
 
void getEvalValues (EvalValues &_laserEval, EvalValues &_pointCloudEval)
 Return evaluation values.
 
void getUncertainty (float &_angleUnc, float &_locUnc)
 Return angle and location uncertainties.
 
void reducePointCloud (const vector< vector2f > &pointCloud, const vector< vector2f > &pointNormals, vector< vector2f > &reducedPointCloud, vector< vector2f > &reducedPointNormals)
 Removes duplicate points with the same observation angle and range.
 
void getParticles (vector< Particle2D > &_particles)
 Returns current particles.
 

Protected Attributes

VectorMap * currentMap
 
vector2f currentLocation
 
float currentAngle
 
vector2f currentLocStdDev
 
float currentAngleStdDev
 
vector< Particle2D > particles
 
vector< Particle2D > particlesRefined
 
int numParticles
 
vector< float > samplingDensity
 
vector2f lastDistanceMoved
 
float lastAngleTurned
 
string mapsFolder
 
vector< VectorMap > maps
 
vector< float > stage0Weights
 
vector< float > stageRWeights
 
vector< Vector2f > gradients
 
vector< Vector2f > points
 
vector< Vector2f > gradients2
 
vector< Vector2f > points2
 
vector< line2f > debugLines
 
vector< int > lineCorrespondences
 
vector2f locCorrectionP0
 
vector2f locCorrectionP1
 
int numUnrefinedParticlesSampled
 
int numRefinedParticlesSampled
 
float refinedImportanceWeights
 
float unrefinedImportanceWeights
 
double refineTime
 
double updateTime
 
EvalValues pointCloudEval
 
EvalValues laserEval
 

Detailed Description

Particle filter for vector localization

Definition at line 58 of file vectorparticlefilter.h.

The documentation for this class was generated from the following files: