#include <gridslamprocessor.h>
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 (std::ostream &infoStr) | |
GridSlamProcessor () | |
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, OrientedPoint, laserPose, protected, public, public) | |
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, lasamplestep, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, llsamplestep, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, lasamplerange, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, llsamplerange, 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) | |
MEMBER_PARAM_SET_GET (m_matcher, double, optLinearDelta, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, optAngularDelta, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, int, kernelSize, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, likelihoodSigma, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, gaussianSigma, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, usableRange, protected, public, public) | |
MEMBER_PARAM_SET_GET (m_matcher, double, laserMaxRange, 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, stt, protected, public, public) | |
STRUCT_PARAM_SET_GET (m_motionModel, double, str, protected, public, public) | |
STRUCT_PARAM_SET_GET (m_motionModel, double, srt, protected, public, public) | |
STRUCT_PARAM_SET_GET (m_motionModel, double, srr, 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, ymax, protected, public) | |
PARAM_GET (double, xmax, protected, public) | |
PARAM_GET (double, ymin, protected, public) | |
PARAM_GET (double, xmin, protected, public) | |
PARAM_GET (double, neff, protected, public) | |
PARAM_SET_GET (double, obsSigmaGain, protected, public, public) | |
PARAM_SET_GET (double, angularThresholdDistance, protected, public, public) | |
PARAM_SET_GET (double, linearThresholdDistance, protected, public, public) | |
PARAM_SET_GET (double, maxMove, protected, public, public) | |
PARAM_SET_GET (double, critScore, 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) |
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 32 of file gridslamprocessor.h.
typedef std::vector<Particle> GMapping::GridSlamProcessor::ParticleVector |
Definition at line 109 of file gridslamprocessor.h.
typedef std::deque<GridSlamProcessor::TNode*> GMapping::GridSlamProcessor::TNodeDeque |
Definition at line 68 of file gridslamprocessor.h.
typedef std::vector<GridSlamProcessor::TNode*> GMapping::GridSlamProcessor::TNodeVector |
Definition at line 67 of file gridslamprocessor.h.
GMapping::GridSlamProcessor::GridSlamProcessor | ( | ) |
Constructs a GridSlamProcessor, initialized with the default parameters
GMapping::GridSlamProcessor::GridSlamProcessor | ( | std::ostream & | infoStr | ) |
Constructs a GridSlamProcessor, whose output is routed to a stream.
infoStr,: | the output stream |
virtual GMapping::GridSlamProcessor::~GridSlamProcessor | ( | ) | [virtual] |
Deleted the gridslamprocessor
GMapping::GridSlamProcessor::GridSlamProcessor | ( | const GridSlamProcessor & | gsp | ) | [protected] |
Copy constructor
GridSlamProcessor* GMapping::GridSlamProcessor::clone | ( | ) | const |
int GMapping::GridSlamProcessor::getBestParticleIndex | ( | ) | const |
const std::vector<unsigned int>& GMapping::GridSlamProcessor::getIndexes | ( | ) | const [inline] |
Definition at line 157 of file gridslamprocessor.h.
const ParticleVector& GMapping::GridSlamProcessor::getParticles | ( | ) | const [inline] |
Definition at line 155 of file gridslamprocessor.h.
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.
std::ostream& GMapping::GridSlamProcessor::infoStream | ( | ) |
the stream used for writing the info/debug messages
void GMapping::GridSlamProcessor::init | ( | unsigned int | size, | |
double | xmin, | |||
double | ymin, | |||
double | xmax, | |||
double | ymax, | |||
double | delta, | |||
OrientedPoint | initialPose = OrientedPoint(0, 0, 0) | |||
) |
void GMapping::GridSlamProcessor::integrateScanSequence | ( | TNode * | node | ) |
GMapping::GridSlamProcessor::MEMBER_PARAM_SET_GET | ( | m_matcher | , | |
OrientedPoint | , | |||
laserPose | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
pose of the laser wrt the robot [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 | , | |
bool | , | |||
generateMap | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
generate an accupancy grid map [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 | , | |
double | , | |||
llsamplestep | , | |||
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 | , | |||
llsamplerange | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
translational sampling range for the likelihood [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 | , | |
unsigned | int, | |||
optRecursiveIterations | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
The number of iterations of the scanmatcher [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 | , | |
double | , | |||
optAngularDelta | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
The optimization step in rotation [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 | , | |||
likelihoodSigma | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
The sigma of a beam used for likelihood computation [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 | , | |||
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 | , | |||
laserMaxRange | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
the maxrange of the laser to consider
void GMapping::GridSlamProcessor::normalize | ( | ) | [inline, private] |
normalizes the particle weights
Definition at line 43 of file gridslamprocessor.h.
virtual void GMapping::GridSlamProcessor::onOdometryUpdate | ( | ) | [virtual] |
virtual void GMapping::GridSlamProcessor::onResampleUpdate | ( | ) | [virtual] |
virtual void GMapping::GridSlamProcessor::onScanmatchUpdate | ( | ) | [virtual] |
std::ofstream& GMapping::GridSlamProcessor::outputStream | ( | ) |
the stream used for writing the output of the algorithm
GMapping::GridSlamProcessor::PARAM_GET | ( | double | , | |
delta | , | |||
protected | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_GET | ( | double | , | |
ymax | , | |||
protected | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_GET | ( | double | , | |
xmax | , | |||
protected | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_GET | ( | double | , | |
ymin | , | |||
protected | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_GET | ( | double | , | |
xmin | , | |||
protected | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_GET | ( | double | , | |
neff | , | |||
protected | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
obsSigmaGain | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
angularThresholdDistance | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
linearThresholdDistance | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
maxMove | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
critScore | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
regScore | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
resampleThreshold | , | |||
protected | , | |||
public | , | |||
public | ||||
) | [protected] |
this sets the neff based resampling threshold
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , | |
minimumScore | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
minimum score for considering the outcome of the scanmatching good
bool GMapping::GridSlamProcessor::processScan | ( | const RangeReading & | reading, | |
int | adaptParticles = 0 | |||
) |
void GMapping::GridSlamProcessor::processTruePos | ( | const OdometryReading & | odometry | ) |
double GMapping::GridSlamProcessor::propagateWeights | ( | ) | [private] |
bool GMapping::GridSlamProcessor::resample | ( | const double * | plainReading, | |
int | adaptParticles, | |||
const RangeReading * | rr = 0 | |||
) | [inline, private] |
Definition at line 71 of file gridslamprocessor.h.
void GMapping::GridSlamProcessor::resetTree | ( | ) | [private] |
void GMapping::GridSlamProcessor::scanMatch | ( | const double * | plainReading | ) | [inline, private] |
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 | |||
) |
void GMapping::GridSlamProcessor::setMotionModelParameters | ( | double | srr, | |
double | srt, | |||
double | str, | |||
double | stt | |||
) |
void GMapping::GridSlamProcessor::setSensorMap | ( | const SensorMap & | smap | ) |
void GMapping::GridSlamProcessor::setUpdateDistances | ( | double | linear, | |
double | angular, | |||
double | resampleThreshold | |||
) |
void GMapping::GridSlamProcessor::setUpdatePeriod | ( | double | p | ) | [inline] |
Definition at line 134 of file gridslamprocessor.h.
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]
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 | , | |||
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 | , | |||
srr | , | |||
protected | , | |||
public | , | |||
public | ||||
) |
odometry error in translation as a function of translation (rho/rho) [motionmodel]
void GMapping::GridSlamProcessor::updateTreeWeights | ( | bool | weightsAlreadyNormalized = false |
) | [private] |
double GMapping::GridSlamProcessor::last_update_time_ [protected] |
Definition at line 235 of file gridslamprocessor.h.
double GMapping::GridSlamProcessor::m_angularDistance [protected] |
Definition at line 260 of file gridslamprocessor.h.
unsigned int GMapping::GridSlamProcessor::m_beams [protected] |
the laser beams
Definition at line 234 of file gridslamprocessor.h.
int GMapping::GridSlamProcessor::m_count [protected] |
Definition at line 256 of file gridslamprocessor.h.
std::vector<unsigned int> GMapping::GridSlamProcessor::m_indexes [protected] |
the particle indexes after resampling (internally used)
Definition at line 244 of file gridslamprocessor.h.
std::ostream& GMapping::GridSlamProcessor::m_infoStream [protected] |
Definition at line 291 of file gridslamprocessor.h.
Definition at line 257 of file gridslamprocessor.h.
double GMapping::GridSlamProcessor::m_linearDistance [protected] |
Definition at line 260 of file gridslamprocessor.h.
the scanmatcher algorithm
Definition at line 149 of file gridslamprocessor.h.
the motion model
Definition at line 250 of file gridslamprocessor.h.
OrientedPoint GMapping::GridSlamProcessor::m_odoPose [protected] |
Definition at line 258 of file gridslamprocessor.h.
std::ofstream GMapping::GridSlamProcessor::m_outputStream [protected] |
Definition at line 288 of file gridslamprocessor.h.
the particles
Definition at line 241 of file gridslamprocessor.h.
OrientedPoint GMapping::GridSlamProcessor::m_pose [protected] |
Definition at line 259 of file gridslamprocessor.h.
int GMapping::GridSlamProcessor::m_readingCount [protected] |
Definition at line 256 of file gridslamprocessor.h.
std::vector<double> GMapping::GridSlamProcessor::m_weights [protected] |
the particle weights (internally used)
Definition at line 247 of file gridslamprocessor.h.
double GMapping::GridSlamProcessor::period_ [protected] |
Definition at line 236 of file gridslamprocessor.h.