#include <gridslamprocessor.h>

Inheritance diagram for GMapping::GridSlamProcessor:

Classes
struct	Particle

struct	TNode

Public Types
typedef std::vector< Particle >	ParticleVector

typedef std::deque< GridSlamProcessor::TNode * >	TNodeDeque

typedef std::vector< GridSlamProcessor::TNode * >	TNodeVector

Public Member Functions
GridSlamProcessor *	clone () const

int	getBestParticleIndex () const

const std::vector< unsigned int > &	getIndexes () const

const ParticleVector &	getParticles () const

TNodeVector	getTrajectories () const

	GridSlamProcessor ()

	GridSlamProcessor (std::ostream &infoStr)

std::ostream &	infoStream ()

void	init (unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose=OrientedPoint(0, 0, 0))

void	integrateScanSequence (TNode *node)

	MEMBER_PARAM_SET_GET (m_matcher, double, laserMaxRange, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, usableRange, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, gaussianSigma, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, likelihoodSigma, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, int, kernelSize, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, optAngularDelta, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, optLinearDelta, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, unsigned int, optRecursiveIterations, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, unsigned int, likelihoodSkip, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, llsamplerange, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, lasamplerange, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, llsamplestep, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, double, lasamplestep, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, bool, generateMap, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, bool, enlargeStep, protected, public, public)

	MEMBER_PARAM_SET_GET (m_matcher, OrientedPoint, laserPose, protected, public, public)

virtual void	onOdometryUpdate ()

virtual void	onResampleUpdate ()

virtual void	onScanmatchUpdate ()

std::ofstream &	outputStream ()

	PARAM_SET_GET (double, minimumScore, protected, public, public)

bool	processScan (const RangeReading &reading, int adaptParticles=0)

void	processTruePos (const OdometryReading &odometry)

void	setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0)

void	setMotionModelParameters (double srr, double srt, double str, double stt)

void	setSensorMap (const SensorMap &smap)

void	setUpdateDistances (double linear, double angular, double resampleThreshold)

void	setUpdatePeriod (double p)

	STRUCT_PARAM_SET_GET (m_motionModel, double, srr, protected, public, public)

	STRUCT_PARAM_SET_GET (m_motionModel, double, srt, protected, public, public)

	STRUCT_PARAM_SET_GET (m_motionModel, double, str, protected, public, public)

	STRUCT_PARAM_SET_GET (m_motionModel, double, stt, protected, public, public)

virtual	~GridSlamProcessor ()

Public Attributes
ScanMatcher	m_matcher

Protected Member Functions
	GridSlamProcessor (const GridSlamProcessor &gsp)

	PARAM_GET (double, neff, protected, public)

	PARAM_GET (double, xmin, protected, public)

	PARAM_GET (double, ymin, protected, public)

	PARAM_GET (double, xmax, protected, public)

	PARAM_GET (double, ymax, protected, public)

	PARAM_GET (double, delta, protected, public)

	PARAM_SET_GET (double, resampleThreshold, protected, public, public)

	PARAM_SET_GET (double, regScore, protected, public, public)

	PARAM_SET_GET (double, critScore, protected, public, public)

	PARAM_SET_GET (double, maxMove, protected, public, public)

	PARAM_SET_GET (double, linearThresholdDistance, protected, public, public)

	PARAM_SET_GET (double, angularThresholdDistance, protected, public, public)

	PARAM_SET_GET (double, obsSigmaGain, protected, public, public)

Protected Attributes
double	last_update_time_

double	m_angularDistance

unsigned int	m_beams

int	m_count

std::vector< unsigned int >	m_indexes

std::ostream &	m_infoStream

OrientedPoint	m_lastPartPose

double	m_linearDistance

MotionModel	m_motionModel

OrientedPoint	m_odoPose

std::ofstream	m_outputStream

ParticleVector	m_particles

OrientedPoint	m_pose

int	m_readingCount

std::vector< double >	m_weights

double	period_

Private Member Functions
void	normalize ()

double	propagateWeights ()

bool	resample (const double plainReading, int adaptParticles, const RangeReading rr=0)

void	resetTree ()

void	scanMatch (const double *plainReading)

void	updateTreeWeights (bool weightsAlreadyNormalized=false)

Detailed Description

This class defines the basic GridFastSLAM algorithm. It implements a rao blackwellized particle filter. Each particle has its own map and robot pose.
This implementation works as follows: each time a new pair odometry/laser reading is received, the particle's robot pose is updated according to the motion model. This pose is subsequently used for initalizing a scan matching algorithm. The scanmatcher performs a local optimization for each particle. It is initialized with the pose drawn from the motion model, and the pose is corrected according to the each particle map.
In order to avoid unnecessary computation the filter state is updated only when the robot moves more than a given threshold.

Definition at line 34 of file gridslamprocessor.h.

Member Typedef Documentation

typedef std::vector<Particle> GMapping::GridSlamProcessor::ParticleVector

Definition at line 126 of file gridslamprocessor.h.

typedef std::deque<GridSlamProcessor::TNode*> GMapping::GridSlamProcessor::TNodeDeque

Definition at line 85 of file gridslamprocessor.h.

typedef std::vector<GridSlamProcessor::TNode*> GMapping::GridSlamProcessor::TNodeVector

Definition at line 84 of file gridslamprocessor.h.

Constructor & Destructor Documentation

GMapping::GridSlamProcessor::GridSlamProcessor ( )

Constructs a GridSlamProcessor, initialized with the default parameters

Definition at line 20 of file gridslamprocessor.cpp.

GMapping::GridSlamProcessor::GridSlamProcessor ( std::ostream & infoStr )

Constructs a GridSlamProcessor, whose output is routed to a stream.

Parameters

infoStr the output stream

Definition at line 90 of file gridslamprocessor.cpp.

GMapping::GridSlamProcessor::~GridSlamProcessor ( )

virtual

Deleted the gridslamprocessor

Definition at line 151 of file gridslamprocessor.cpp.

GMapping::GridSlamProcessor::GridSlamProcessor ( const GridSlamProcessor & gsp )

protected

Copy constructor

Definition at line 28 of file gridslamprocessor.cpp.

Member Function Documentation

GridSlamProcessor * GMapping::GridSlamProcessor::clone ( ) const

Returns: a deep copy of the grid slam processor with all the internal structures.

Definition at line 98 of file gridslamprocessor.cpp.

int GMapping::GridSlamProcessor::getBestParticleIndex ( ) const

Definition at line 498 of file gridslamprocessor.cpp.

const std::vector<unsigned int>& GMapping::GridSlamProcessor::getIndexes ( ) const

inline

Definition at line 174 of file gridslamprocessor.h.

const ParticleVector& GMapping::GridSlamProcessor::getParticles ( ) const

inline

Returns: the particles

Definition at line 172 of file gridslamprocessor.h.

GridSlamProcessor::TNodeVector GMapping::GridSlamProcessor::getTrajectories ( ) const

This method copies the state of the filter in a tree. The tree is represented through reversed pointers (each node has a pointer to its parent). The leafs are stored in a vector, whose size is the same as the number of particles.

Returns: the leafs of the tree

Definition at line 40 of file gridslamprocessor_tree.cpp.

std::ostream & GMapping::GridSlamProcessor::infoStream ( )

the stream used for writing the info/debug messages

Definition at line 493 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::init	(	unsigned int	size,
		double	xmin,
		double	ymin,
		double	xmax,
		double	ymax,
		double	delta,
		OrientedPoint	initialPose = `OrientedPoint(0,0,0)`
	)

Definition at line 268 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::integrateScanSequence ( GridSlamProcessor::TNode * node )

Definition at line 121 of file gridslamprocessor_tree.cpp.

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		laserMaxRange	,
		protected	,
		public	,
		public
	)

the maxrange of the laser to consider

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		usableRange	,
		protected	,
		public	,
		public
	)

the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		gaussianSigma	,
		protected	,
		public	,
		public
	)

The sigma used by the greedy endpoint matching. [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		likelihoodSigma	,
		protected	,
		public	,
		public
	)

The sigma of a beam used for likelihood computation [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		int	,
		kernelSize	,
		protected	,
		public	,
		public
	)

The kernel in which to look for a correspondence[scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		optAngularDelta	,
		protected	,
		public	,
		public
	)

The optimization step in rotation [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		optLinearDelta	,
		protected	,
		public	,
		public
	)

The optimization step in translation [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		unsigned	int,
		optRecursiveIterations	,
		protected	,
		public	,
		public
	)

The number of iterations of the scanmatcher [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		unsigned	int,
		likelihoodSkip	,
		protected	,
		public	,
		public
	)

the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		llsamplerange	,
		protected	,
		public	,
		public
	)

translational sampling range for the likelihood [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		lasamplerange	,
		protected	,
		public	,
		public
	)

angular sampling range for the likelihood [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		llsamplestep	,
		protected	,
		public	,
		public
	)

translational sampling range for the likelihood [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		double	,
		lasamplestep	,
		protected	,
		public	,
		public
	)

angular sampling step for the likelihood [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		bool	,
		generateMap	,
		protected	,
		public	,
		public
	)

generate an accupancy grid map [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		bool	,
		enlargeStep	,
		protected	,
		public	,
		public
	)

enlarge the map when the robot goes out of the boundaries [scanmatcher]

GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET	(	m_matcher	,
		OrientedPoint	,
		laserPose	,
		protected	,
		public	,
		public
	)

pose of the laser wrt the robot [scanmatcher]

void GMapping::GridSlamProcessor::normalize ( )

inlineprivate

normalizes the particle weights

Definition at line 43 of file gridslamprocessor.h.

void GMapping::GridSlamProcessor::onOdometryUpdate ( )

virtual

Reimplemented in GridSlamProcessorThread.

Definition at line 511 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::onResampleUpdate ( )

virtual

Reimplemented in GridSlamProcessorThread.

Definition at line 510 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::onScanmatchUpdate ( )

virtual

Reimplemented in GridSlamProcessorThread.

Definition at line 509 of file gridslamprocessor.cpp.

std::ofstream & GMapping::GridSlamProcessor::outputStream ( )

the stream used for writing the output of the algorithm

Definition at line 489 of file gridslamprocessor.cpp.

GMapping::GridSlamProcessor::PARAM_GET	(	double	,
		neff	,
		protected	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_GET	(	double	,
		xmin	,
		protected	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_GET	(	double	,
		ymin	,
		protected	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_GET	(	double	,
		xmax	,
		protected	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_GET	(	double	,
		ymax	,
		protected	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_GET	(	double	,
		delta	,
		protected	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		minimumScore	,
		protected	,
		public	,
		public
	)

minimum score for considering the outcome of the scanmatching good

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		resampleThreshold	,
		protected	,
		public	,
		public
	)

protected

this sets the neff based resampling threshold

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		regScore	,
		protected	,
		public	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		critScore	,
		protected	,
		public	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		maxMove	,
		protected	,
		public	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		linearThresholdDistance	,
		protected	,
		public	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		angularThresholdDistance	,
		protected	,
		public	,
		public
	)

protected

GMapping::GridSlamProcessor::PARAM_SET_GET	(	double	,
		obsSigmaGain	,
		protected	,
		public	,
		public
	)

protected

bool GMapping::GridSlamProcessor::processScan	(	const RangeReading &	reading,
		int	adaptParticles = `0`
	)

retireve the position from the reading, and compute the odometry

Definition at line 316 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::processTruePos ( const OdometryReading & odometry )

Definition at line 306 of file gridslamprocessor.cpp.

double GMapping::GridSlamProcessor::propagateWeights ( )

private

Definition at line 233 of file gridslamprocessor_tree.cpp.

bool GMapping::GridSlamProcessor::resample	(	const double *	plainReading,
		int	adaptParticles,
		const RangeReading *	rr = `0`
	)

inlineprivate

Definition at line 71 of file gridslamprocessor.h.

void GMapping::GridSlamProcessor::resetTree ( )

private

Definition at line 207 of file gridslamprocessor_tree.cpp.

void GMapping::GridSlamProcessor::scanMatch ( const double * plainReading )

inlineprivate

scanmatches all the particles

Just scan match every single particle. If the scan matching fails, the particle gets a default likelihood.

Definition at line 10 of file gridslamprocessor.h.

void GMapping::GridSlamProcessor::setMatchingParameters	(	double	urange,
		double	range,
		double	sigma,
		int	kernsize,
		double	lopt,
		double	aopt,
		int	iterations,
		double	likelihoodSigma = `1`,
		double	likelihoodGain = `1`,
		unsigned int	likelihoodSkip = `0`
	)

Definition at line 195 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::setMotionModelParameters	(	double	srr,
		double	srt,
		double	str,
		double	stt
	)

Definition at line 212 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::setSensorMap ( const SensorMap & smap )

Definition at line 242 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::setUpdateDistances	(	double	linear,
		double	angular,
		double	resampleThreshold
	)

Definition at line 224 of file gridslamprocessor.cpp.

void GMapping::GridSlamProcessor::setUpdatePeriod ( double p )

inline

Definition at line 151 of file gridslamprocessor.h.

GMapping::GridSlamProcessor::STRUCT_PARAM_SET_GET	(	m_motionModel	,
		double	,
		srr	,
		protected	,
		public	,
		public
	)

odometry error in translation as a function of translation (rho/rho) [motionmodel]

GMapping::GridSlamProcessor::STRUCT_PARAM_SET_GET	(	m_motionModel	,
		double	,
		srt	,
		protected	,
		public	,
		public
	)

odometry error in translation as a function of rotation (rho/theta) [motionmodel]

GMapping::GridSlamProcessor::STRUCT_PARAM_SET_GET	(	m_motionModel	,
		double	,
		str	,
		protected	,
		public	,
		public
	)

odometry error in rotation as a function of translation (theta/rho) [motionmodel]

GMapping::GridSlamProcessor::STRUCT_PARAM_SET_GET	(	m_motionModel	,
		double	,
		stt	,
		protected	,
		public	,
		public
	)

odometry error in rotation as a function of rotation (theta/theta) [motionmodel]

void GMapping::GridSlamProcessor::updateTreeWeights ( bool weightsAlreadyNormalized = false )

private

Definition at line 198 of file gridslamprocessor_tree.cpp.

Member Data Documentation

double GMapping::GridSlamProcessor::last_update_time_

protected

Definition at line 252 of file gridslamprocessor.h.

double GMapping::GridSlamProcessor::m_angularDistance

protected

Definition at line 275 of file gridslamprocessor.h.

unsigned int GMapping::GridSlamProcessor::m_beams

protected

the laser beams

Definition at line 251 of file gridslamprocessor.h.

int GMapping::GridSlamProcessor::m_count

protected

Definition at line 271 of file gridslamprocessor.h.

std::vector<unsigned int> GMapping::GridSlamProcessor::m_indexes

protected

the particle indexes after resampling (internally used)

Definition at line 259 of file gridslamprocessor.h.

std::ostream& GMapping::GridSlamProcessor::m_infoStream

protected

Definition at line 306 of file gridslamprocessor.h.

OrientedPoint GMapping::GridSlamProcessor::m_lastPartPose

protected

Definition at line 272 of file gridslamprocessor.h.

double GMapping::GridSlamProcessor::m_linearDistance

protected

Definition at line 275 of file gridslamprocessor.h.

ScanMatcher GMapping::GridSlamProcessor::m_matcher

the scanmatcher algorithm

Definition at line 166 of file gridslamprocessor.h.

MotionModel GMapping::GridSlamProcessor::m_motionModel

protected

the motion model

Definition at line 265 of file gridslamprocessor.h.

OrientedPoint GMapping::GridSlamProcessor::m_odoPose

protected

Definition at line 273 of file gridslamprocessor.h.

std::ofstream GMapping::GridSlamProcessor::m_outputStream

protected

Definition at line 303 of file gridslamprocessor.h.

ParticleVector GMapping::GridSlamProcessor::m_particles

protected

the particles

Definition at line 256 of file gridslamprocessor.h.

OrientedPoint GMapping::GridSlamProcessor::m_pose

protected

Definition at line 274 of file gridslamprocessor.h.

int GMapping::GridSlamProcessor::m_readingCount

protected

Definition at line 271 of file gridslamprocessor.h.

std::vector<double> GMapping::GridSlamProcessor::m_weights

protected

the particle weights (internally used)

Definition at line 262 of file gridslamprocessor.h.

double GMapping::GridSlamProcessor::period_

protected

Definition at line 253 of file gridslamprocessor.h.

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

Classes

Public Types

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Private Member Functions

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation