DualCoding: ParticleFilter Class Reference

Create an instance of ParticleFilter, and set its parameters by modifying member variables such as num_particles and num_generations. Then call its localize() method to determine the robot's location. Subsequent calls will start with the current particle distribution unless you call uniformly_distribute() to randomize things.

Definition at line 25 of file ParticleFilter.h.


Public Member Functions
	ParticleFilter (ShapeSpace &LocalShS, ShapeSpace &WorldShS)
void	reinitialize ()
	Reset particle filter and restore default settings.
void	setWorldBounds (const Shape< PolygonData > &poly)
	Constrain new particles to world boundaries defined by a polygon.
int	localize ()
	Invoke the particle filter: determine best match of local view to world map. Returns index of best particle, or -1 if failure.
void	uniformlyDistribute ()
	Uniformly distribute particles throughout the region spanned by the world landmarks.
void	moveBy (float const xdelta, float const ydelta, AngPi const tdelta)
	Update the current particles when the robot moves.
void	makeParticles ()
	Create two Particle vectors of length numParticles, stored in curParticles and newParticles.
void	loadLms ()
	Load the local and world landmarks from their shape spaces, and create or resize particles if necessary.
void	resizeParticles ()
	Set the size of each Particle's addLocal vector to nlocal, and resize some internal vectors.
void	randomizeNewParticle (int const i)
	Set new Particle i to random dx, dy, and theta values. Called by uniformlyDistribute().
void	getInitialGuess ()
	Check particle scores, and randomize and repeat if no particle is close to a solution.
void	computeParticleScores ()
	Calculate the score for each Particle p, leaving it in p.prob.
void	setParticleView (int const i)
	Set up particleView vectors based on the parameters of the i-th Particle.
void	computeLocalMatch (int const i, int const j)
	Match each local landmark against the world landmarks.
void	resample ()
	Generate newParticles from curParticles, then swap the two vectors and compute scores.
void	spawnInto (int const i, int const j)
void	determineAdditions (int const i)
Static Public Member Functions
static float	distanceFromLine (coordinate_t x, coordinate_t y, PfLine &wline)
static float	normpdf (float const distsq)
Public Attributes
ShapeSpace &	localShS
	Local shape space.
ShapeSpace &	worldShS
	World shape space.
int	numParticles
	Number of particles to use.
int	numGenerations
	Maximum number of generations of resampling.
int	numTries
	Number of times to restart if no good initial guess.
float	noiseFactorXY
	Scale of noise for translation parameters x,y.
float	noiseFactorT
	Scale of noise for rotation parameter theta.
Shape< PolygonData >	worldBounds
	If valid shape, particles must lie inside it.
int	nlocal
	Current number of local landmarks.
int	nworld
vector< Particle > *	curParticles
	Pointer to vector holding the current set of particles.
int	bestIndex
	Index of highest scoring particle from most recent resampling.
vector< Particle > *	newParticles
	Pointer to vector where new particles are generated by resampling.
vector< PfRoot * > *	localLms
	Pointer to current vector of local landmarks.
vector< PfRoot * > *	worldLms
	Pointer to current vector of world landmarks.
vector< float >	particleViewX
	x coords of local landmarks according to the currently-selected particle
vector< float >	particleViewY
	y coords of local landmarks according to the currently-selected particle
vector< float >	particleViewX2
	x coords of other point of local line
vector< float >	particleViewY2
	y coords of other point of local line
vector< float >	localScores
	Match scores for local landmarks according to the currently-selected particle.
vector< int >	localMatches
	Index of best matching world landmark for each local landmark according to the currently-selected particle.

float	xmin
	Bound box of world shapes.
float	xmax
	Bound box of world shapes.
float	xrange
	Bound box of world shapes.
float	ymin
	Bound box of world shapes.
float	ymax
	Bound box of world shapes.
float	yrange
	Bound box of world shapes.
Static Public Attributes
static const int	NUM_PARTICLES = 2000
static const int	NUM_GENERATIONS = 15
static const int	NUM_TRIES = 5
static const float	INITIAL_XY_NOISE = 100
static const float	INITIAL_THETA_NOISE = 30 * M_PI/180
static const float	NOISE_REDUCTION_XY = 0.85
static const float	NOISE_REDUCTION_T = 0.9
static const float	INFINITE_DISTANCE = 10000000
static const float	STDEV = 50
static const float	ADDITION_PENALTY = 0.1
static const float	PERCENT_RANDOM = 0.1
	Percent of random particles in each resampling.
Private Member Functions
ParticleFilter &	operator= (const ParticleFilter &)
	don't call this
	ParticleFilter (const ParticleFilter &)
	don't call this

Constructor & Destructor Documentation

ParticleFilter	(	ShapeSpace &	LocalShS,
		ShapeSpace &	WorldShS
	)			`[inline]`

Definition at line 69 of file ParticleFilter.h.

ParticleFilter ( const ParticleFilter & ) [private]

don't call this

Member Function Documentation

void computeLocalMatch	(	int const	i,
		int const	j
	)

Match each local landmark against the world landmarks.

Definition at line 218 of file ParticleFilter.cc.

Referenced by ParticleFilter::computeParticleScores().

void computeParticleScores ( )

Calculate the score for each Particle p, leaving it in p.prob.

Definition at line 176 of file ParticleFilter.cc.

Referenced by ParticleFilter::getInitialGuess(), ParticleFilter::moveBy(), and ParticleFilter::resample().

void determineAdditions ( int const i )

Definition at line 351 of file ParticleFilter.cc.

Referenced by ParticleFilter::computeParticleScores().

float distanceFromLine	(	coordinate_t	x,
		coordinate_t	y,
		PfLine &	wline
	)			`[static]`

Definition at line 297 of file ParticleFilter.cc.

Referenced by ParticleFilter::computeLocalMatch().

void getInitialGuess ( )

Check particle scores, and randomize and repeat if no particle is close to a solution.

Definition at line 165 of file ParticleFilter.cc.

Referenced by ParticleFilter::localize().

void loadLms ( )

Load the local and world landmarks from their shape spaces, and create or resize particles if necessary.

Definition at line 62 of file ParticleFilter.cc.

Referenced by ParticleFilter::localize(), and ParticleFilter::uniformlyDistribute().

int localize ( )

Invoke the particle filter: determine best match of local view to world map. Returns index of best particle, or -1 if failure.

Definition at line 132 of file ParticleFilter.cc.

void makeParticles ( )

Create two Particle vectors of length numParticles, stored in curParticles and newParticles.

This function is called once, automatically, by localize() to initialize the Particle vectors. It should only be called explicitly when the value of numParticles has changed.

Definition at line 42 of file ParticleFilter.cc.

Referenced by ParticleFilter::loadLms().

void moveBy	(	float const	xdelta,
		float const	ydelta,
		AngPi const	tdelta
	)

Update the current particles when the robot moves.

Definition at line 154 of file ParticleFilter.cc.

static float normpdf ( float const distsq ) [inline, static]

Definition at line 146 of file ParticleFilter.h.

ParticleFilter& operator= ( const ParticleFilter & ) [private]

don't call this

void randomizeNewParticle ( int const i )

Set new Particle i to random dx, dy, and theta values. Called by uniformlyDistribute().

Definition at line 120 of file ParticleFilter.cc.

Referenced by ParticleFilter::resample(), and ParticleFilter::uniformlyDistribute().

void reinitialize ( )

Reset particle filter and restore default settings.

Definition at line 25 of file ParticleFilter.cc.

Referenced by VRmixin::~VRmixin().

void resample ( )

Generate newParticles from curParticles, then swap the two vectors and compute scores.

Definition at line 307 of file ParticleFilter.cc.

Referenced by ParticleFilter::localize().

void resizeParticles ( )

Set the size of each Particle's addLocal vector to nlocal, and resize some internal vectors.

Definition at line 49 of file ParticleFilter.cc.

Referenced by ParticleFilter::loadLms().

void setParticleView ( int const i )

Set up particleView vectors based on the parameters of the i-th Particle.

Definition at line 199 of file ParticleFilter.cc.

Referenced by ParticleFilter::computeParticleScores().

void setWorldBounds ( const Shape< PolygonData > & poly ) [inline]

Constrain new particles to world boundaries defined by a polygon.

Definition at line 94 of file ParticleFilter.h.

void spawnInto	(	int const	i,
		int const	j
	)

Definition at line 333 of file ParticleFilter.cc.

Referenced by ParticleFilter::resample().

void uniformlyDistribute ( )

Uniformly distribute particles throughout the region spanned by the world landmarks.

Definition at line 109 of file ParticleFilter.cc.

Referenced by ParticleFilter::getInitialGuess(), and ParticleFilter::loadLms().

Member Data Documentation

const float ADDITION_PENALTY = 0.1 [static]

Definition at line 36 of file ParticleFilter.h.

Referenced by ParticleFilter::determineAdditions().

int bestIndex

Index of highest scoring particle from most recent resampling.

Definition at line 55 of file ParticleFilter.h.

Referenced by ParticleFilter::computeParticleScores(), ParticleFilter::getInitialGuess(), ParticleFilter::localize(), ParticleFilter::reinitialize(), and ParticleFilter::resample().

vector<Particle>* curParticles

Pointer to vector holding the current set of particles.

Definition at line 54 of file ParticleFilter.h.

Referenced by ParticleFilter::getInitialGuess(), ParticleFilter::loadLms(), ParticleFilter::makeParticles(), ParticleFilter::moveBy(), ParticleFilter::reinitialize(), ParticleFilter::resample(), ParticleFilter::resizeParticles(), and ParticleFilter::uniformlyDistribute().

const float INFINITE_DISTANCE = 10000000 [static]

Definition at line 34 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch().

const float INITIAL_THETA_NOISE = 30 * M_PI/180 [static]

Definition at line 31 of file ParticleFilter.h.

Referenced by ParticleFilter::localize(), and ParticleFilter::reinitialize().

const float INITIAL_XY_NOISE = 100 [static]

Definition at line 30 of file ParticleFilter.h.

Referenced by ParticleFilter::localize(), and ParticleFilter::reinitialize().

vector<PfRoot*>* localLms

Pointer to current vector of local landmarks.

Definition at line 59 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), ParticleFilter::loadLms(), and ParticleFilter::setParticleView().

vector<int> localMatches

Index of best matching world landmark for each local landmark according to the currently-selected particle.

Definition at line 66 of file ParticleFilter.h.

Referenced by ParticleFilter::determineAdditions(), ParticleFilter::reinitialize(), and ParticleFilter::resizeParticles().

vector<float> localScores

Match scores for local landmarks according to the currently-selected particle.

Definition at line 65 of file ParticleFilter.h.

Referenced by ParticleFilter::computeParticleScores(), ParticleFilter::determineAdditions(), ParticleFilter::reinitialize(), and ParticleFilter::resizeParticles().

ShapeSpace& localShS

Local shape space.

Definition at line 39 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms().

vector<Particle>* newParticles

Pointer to vector where new particles are generated by resampling.

Definition at line 58 of file ParticleFilter.h.

Referenced by ParticleFilter::makeParticles(), ParticleFilter::reinitialize(), ParticleFilter::resample(), and ParticleFilter::uniformlyDistribute().

int nlocal

Current number of local landmarks.

Definition at line 47 of file ParticleFilter.h.

Referenced by ParticleFilter::computeParticleScores(), ParticleFilter::determineAdditions(), ParticleFilter::getInitialGuess(), ParticleFilter::loadLms(), ParticleFilter::localize(), ParticleFilter::resizeParticles(), and ParticleFilter::setParticleView().

const float NOISE_REDUCTION_T = 0.9 [static]

Definition at line 33 of file ParticleFilter.h.

Referenced by ParticleFilter::localize().

const float NOISE_REDUCTION_XY = 0.85 [static]

Definition at line 32 of file ParticleFilter.h.

Referenced by ParticleFilter::localize().

float noiseFactorT

Scale of noise for rotation parameter theta.

Definition at line 45 of file ParticleFilter.h.

Referenced by ParticleFilter::localize(), ParticleFilter::reinitialize(), and ParticleFilter::spawnInto().

float noiseFactorXY

Scale of noise for translation parameters x,y.

Definition at line 44 of file ParticleFilter.h.

Referenced by ParticleFilter::localize(), ParticleFilter::reinitialize(), and ParticleFilter::spawnInto().

const int NUM_GENERATIONS = 15 [static]

Definition at line 28 of file ParticleFilter.h.

Referenced by ParticleFilter::reinitialize().

const int NUM_PARTICLES = 2000 [static]

Definition at line 27 of file ParticleFilter.h.

Referenced by ParticleFilter::reinitialize().

const int NUM_TRIES = 5 [static]

Definition at line 29 of file ParticleFilter.h.

Referenced by ParticleFilter::reinitialize().

int numGenerations

Maximum number of generations of resampling.

Definition at line 42 of file ParticleFilter.h.

Referenced by ParticleFilter::localize(), and ParticleFilter::reinitialize().

int numParticles

Number of particles to use.

Definition at line 41 of file ParticleFilter.h.

Referenced by ParticleFilter::computeParticleScores(), ParticleFilter::makeParticles(), ParticleFilter::moveBy(), ParticleFilter::reinitialize(), ParticleFilter::resample(), ParticleFilter::resizeParticles(), and ParticleFilter::uniformlyDistribute().

int numTries

Number of times to restart if no good initial guess.

Definition at line 43 of file ParticleFilter.h.

Referenced by ParticleFilter::getInitialGuess(), and ParticleFilter::reinitialize().

int nworld

Current number of world landmarks

Definition at line 48 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), ParticleFilter::loadLms(), and ParticleFilter::localize().

vector<float> particleViewX

x coords of local landmarks according to the currently-selected particle

Definition at line 61 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), ParticleFilter::resizeParticles(), and ParticleFilter::setParticleView().

vector<float> particleViewX2

x coords of other point of local line

Definition at line 63 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), ParticleFilter::resizeParticles(), and ParticleFilter::setParticleView().

vector<float> particleViewY

y coords of local landmarks according to the currently-selected particle

Definition at line 62 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), ParticleFilter::resizeParticles(), and ParticleFilter::setParticleView().

vector<float> particleViewY2

y coords of other point of local line

Definition at line 64 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), ParticleFilter::resizeParticles(), and ParticleFilter::setParticleView().

const float PERCENT_RANDOM = 0.1 [static]

Percent of random particles in each resampling.

Definition at line 37 of file ParticleFilter.h.

Referenced by ParticleFilter::resample().

const float STDEV = 50 [static]

Definition at line 35 of file ParticleFilter.h.

Referenced by ParticleFilter::normpdf().

Shape<PolygonData> worldBounds

If valid shape, particles must lie inside it.

Definition at line 46 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms(), ParticleFilter::randomizeNewParticle(), ParticleFilter::reinitialize(), ParticleFilter::setWorldBounds(), and ParticleFilter::spawnInto().

vector<PfRoot*>* worldLms

Pointer to current vector of world landmarks.

Definition at line 60 of file ParticleFilter.h.

Referenced by ParticleFilter::computeLocalMatch(), and ParticleFilter::loadLms().

ShapeSpace& worldShS

World shape space.

Definition at line 40 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms().

float xmax

Bound box of world shapes.

Definition at line 51 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms().

float xmin

Bound box of world shapes.

Definition at line 51 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms(), and ParticleFilter::randomizeNewParticle().

float xrange

Bound box of world shapes.

Definition at line 51 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms(), and ParticleFilter::randomizeNewParticle().

float ymax

Bound box of world shapes.

Definition at line 52 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms().

float ymin

Bound box of world shapes.

Definition at line 52 of file ParticleFilter.h.

Referenced by ParticleFilter::loadLms(), and ParticleFilter::randomizeNewParticle().

ParticleFilter Class Reference

Detailed Description

Public Member Functions

Static Public Member Functions

Public Attributes

Static Public Attributes

Private Member Functions

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation