Classes | Public Types | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | List of all members
GMapping::GridSlamProcessor Class Reference

#include <gridslamprocessor.h>

Inheritance diagram for GMapping::GridSlamProcessor:
Inheritance graph
[legend]

Classes

struct  Particle
 
struct  TNode
 

Public Types

typedef std::vector< ParticleParticleVector
 
typedef std::deque< GridSlamProcessor::TNode * > TNodeDeque
 
typedef std::vector< GridSlamProcessor::TNode * > TNodeVector
 

Public Member Functions

GridSlamProcessorclone () const
 
int getBestParticleIndex () const
 
const std::vector< unsigned int > & getIndexes () const
 
const ParticleVectorgetParticles () 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, bool, enlargeStep, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, bool, generateMap, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, gaussianSigma, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, lasamplerange, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, lasamplestep, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, laserMaxRange, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, likelihoodSigma, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, llsamplerange, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, double, llsamplestep, 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, double, usableRange, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, int, kernelSize, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, OrientedPoint, laserPose, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, unsigned int, likelihoodSkip, protected, public, public)
 
 MEMBER_PARAM_SET_GET (m_matcher, unsigned int, optRecursiveIterations, 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, delta, protected, public)
 
 PARAM_GET (double, neff, protected, public)
 
 PARAM_GET (double, xmax, protected, public)
 
 PARAM_GET (double, xmin, protected, public)
 
 PARAM_GET (double, ymax, protected, public)
 
 PARAM_GET (double, ymin, protected, public)
 
 PARAM_SET_GET (double, angularThresholdDistance, protected, public, public)
 
 PARAM_SET_GET (double, critScore, protected, public, public)
 
 PARAM_SET_GET (double, linearThresholdDistance, protected, public, public)
 
 PARAM_SET_GET (double, maxMove, protected, public, public)
 
 PARAM_SET_GET (double, obsSigmaGain, protected, public, public)
 
 PARAM_SET_GET (double, regScore, protected, public, public)
 
 PARAM_SET_GET (double, resampleThreshold, 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 35 of file gridslamprocessor.h.

Member Typedef Documentation

◆ ParticleVector

Definition at line 127 of file gridslamprocessor.h.

◆ TNodeDeque

Definition at line 86 of file gridslamprocessor.h.

◆ TNodeVector

Definition at line 85 of file gridslamprocessor.h.

Constructor & Destructor Documentation

◆ GridSlamProcessor() [1/3]

GMapping::GridSlamProcessor::GridSlamProcessor ( )

Constructs a GridSlamProcessor, initialized with the default parameters

Definition at line 20 of file gridslamprocessor.cpp.

◆ GridSlamProcessor() [2/3]

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

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

Parameters
infoStrthe output stream

Definition at line 90 of file gridslamprocessor.cpp.

◆ ~GridSlamProcessor()

GMapping::GridSlamProcessor::~GridSlamProcessor ( )
virtual

Deleted the gridslamprocessor

Definition at line 151 of file gridslamprocessor.cpp.

◆ GridSlamProcessor() [3/3]

GMapping::GridSlamProcessor::GridSlamProcessor ( const GridSlamProcessor gsp)
protected

Copy constructor

Definition at line 28 of file gridslamprocessor.cpp.

Member Function Documentation

◆ clone()

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.

◆ getBestParticleIndex()

int GMapping::GridSlamProcessor::getBestParticleIndex ( ) const

Definition at line 498 of file gridslamprocessor.cpp.

◆ getIndexes()

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

Definition at line 175 of file gridslamprocessor.h.

◆ getParticles()

const ParticleVector& GMapping::GridSlamProcessor::getParticles ( ) const
inline
Returns
the particles

Definition at line 173 of file gridslamprocessor.h.

◆ getTrajectories()

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.

◆ infoStream()

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

the stream used for writing the info/debug messages

Definition at line 493 of file gridslamprocessor.cpp.

◆ init()

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.

◆ integrateScanSequence()

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

Definition at line 121 of file gridslamprocessor_tree.cpp.

◆ MEMBER_PARAM_SET_GET() [1/16]

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]

◆ MEMBER_PARAM_SET_GET() [2/16]

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

generate an accupancy grid map [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [3/16]

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

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

◆ MEMBER_PARAM_SET_GET() [4/16]

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

angular sampling range for the likelihood [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [5/16]

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

angular sampling step for the likelihood [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [6/16]

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

the maxrange of the laser to consider

◆ MEMBER_PARAM_SET_GET() [7/16]

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

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

◆ MEMBER_PARAM_SET_GET() [8/16]

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

translational sampling range for the likelihood [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [9/16]

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

translational sampling range for the likelihood [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [10/16]

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

The optimization step in rotation [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [11/16]

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

The optimization step in translation [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [12/16]

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]

◆ MEMBER_PARAM_SET_GET() [13/16]

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

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

◆ MEMBER_PARAM_SET_GET() [14/16]

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

pose of the laser wrt the robot [scanmatcher]

◆ MEMBER_PARAM_SET_GET() [15/16]

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]

◆ MEMBER_PARAM_SET_GET() [16/16]

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

The number of iterations of the scanmatcher [scanmatcher]

◆ normalize()

void GMapping::GridSlamProcessor::normalize ( )
inlineprivate

normalizes the particle weights

◆ onOdometryUpdate()

void GMapping::GridSlamProcessor::onOdometryUpdate ( )
virtual

Reimplemented in GridSlamProcessorThread.

Definition at line 511 of file gridslamprocessor.cpp.

◆ onResampleUpdate()

void GMapping::GridSlamProcessor::onResampleUpdate ( )
virtual

Reimplemented in GridSlamProcessorThread.

Definition at line 510 of file gridslamprocessor.cpp.

◆ onScanmatchUpdate()

void GMapping::GridSlamProcessor::onScanmatchUpdate ( )
virtual

Reimplemented in GridSlamProcessorThread.

Definition at line 509 of file gridslamprocessor.cpp.

◆ outputStream()

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

the stream used for writing the output of the algorithm

Definition at line 489 of file gridslamprocessor.cpp.

◆ PARAM_GET() [1/6]

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

◆ PARAM_GET() [2/6]

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

◆ PARAM_GET() [3/6]

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

◆ PARAM_GET() [4/6]

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

◆ PARAM_GET() [5/6]

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

◆ PARAM_GET() [6/6]

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

◆ PARAM_SET_GET() [1/8]

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

◆ PARAM_SET_GET() [2/8]

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

◆ PARAM_SET_GET() [3/8]

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

◆ PARAM_SET_GET() [4/8]

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

◆ PARAM_SET_GET() [5/8]

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

minimum score for considering the outcome of the scanmatching good

◆ PARAM_SET_GET() [6/8]

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

◆ PARAM_SET_GET() [7/8]

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

◆ PARAM_SET_GET() [8/8]

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

this sets the neff based resampling threshold

◆ processScan()

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.

◆ processTruePos()

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

Definition at line 306 of file gridslamprocessor.cpp.

◆ propagateWeights()

double GMapping::GridSlamProcessor::propagateWeights ( )
private

Definition at line 233 of file gridslamprocessor_tree.cpp.

◆ resample()

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

◆ resetTree()

void GMapping::GridSlamProcessor::resetTree ( )
private

Definition at line 207 of file gridslamprocessor_tree.cpp.

◆ scanMatch()

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

scanmatches all the particles

◆ setMatchingParameters()

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.

◆ setMotionModelParameters()

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

Definition at line 212 of file gridslamprocessor.cpp.

◆ setSensorMap()

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

Definition at line 242 of file gridslamprocessor.cpp.

◆ setUpdateDistances()

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

Definition at line 224 of file gridslamprocessor.cpp.

◆ setUpdatePeriod()

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

Definition at line 152 of file gridslamprocessor.h.

◆ STRUCT_PARAM_SET_GET() [1/4]

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]

◆ STRUCT_PARAM_SET_GET() [2/4]

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]

◆ STRUCT_PARAM_SET_GET() [3/4]

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]

◆ STRUCT_PARAM_SET_GET() [4/4]

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]

◆ updateTreeWeights()

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

Definition at line 198 of file gridslamprocessor_tree.cpp.

Member Data Documentation

◆ last_update_time_

double GMapping::GridSlamProcessor::last_update_time_
protected

Definition at line 253 of file gridslamprocessor.h.

◆ m_angularDistance

double GMapping::GridSlamProcessor::m_angularDistance
protected

Definition at line 276 of file gridslamprocessor.h.

◆ m_beams

unsigned int GMapping::GridSlamProcessor::m_beams
protected

the laser beams

Definition at line 252 of file gridslamprocessor.h.

◆ m_count

int GMapping::GridSlamProcessor::m_count
protected

Definition at line 272 of file gridslamprocessor.h.

◆ m_indexes

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

the particle indexes after resampling (internally used)

Definition at line 260 of file gridslamprocessor.h.

◆ m_infoStream

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

Definition at line 307 of file gridslamprocessor.h.

◆ m_lastPartPose

OrientedPoint GMapping::GridSlamProcessor::m_lastPartPose
protected

Definition at line 273 of file gridslamprocessor.h.

◆ m_linearDistance

double GMapping::GridSlamProcessor::m_linearDistance
protected

Definition at line 276 of file gridslamprocessor.h.

◆ m_matcher

ScanMatcher GMapping::GridSlamProcessor::m_matcher

the scanmatcher algorithm

Definition at line 167 of file gridslamprocessor.h.

◆ m_motionModel

MotionModel GMapping::GridSlamProcessor::m_motionModel
protected

the motion model

Definition at line 266 of file gridslamprocessor.h.

◆ m_odoPose

OrientedPoint GMapping::GridSlamProcessor::m_odoPose
protected

Definition at line 274 of file gridslamprocessor.h.

◆ m_outputStream

std::ofstream GMapping::GridSlamProcessor::m_outputStream
protected

Definition at line 304 of file gridslamprocessor.h.

◆ m_particles

ParticleVector GMapping::GridSlamProcessor::m_particles
protected

the particles

Definition at line 257 of file gridslamprocessor.h.

◆ m_pose

OrientedPoint GMapping::GridSlamProcessor::m_pose
protected

Definition at line 275 of file gridslamprocessor.h.

◆ m_readingCount

int GMapping::GridSlamProcessor::m_readingCount
protected

Definition at line 272 of file gridslamprocessor.h.

◆ m_weights

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

the particle weights (internally used)

Definition at line 263 of file gridslamprocessor.h.

◆ period_

double GMapping::GridSlamProcessor::period_
protected

Definition at line 254 of file gridslamprocessor.h.


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


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51