#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 () | |
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) |
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.
typedef std::vector<Particle> GMapping::GridSlamProcessor::ParticleVector |
Definition at line 127 of file gridslamprocessor.h.
typedef std::deque<GridSlamProcessor::TNode*> GMapping::GridSlamProcessor::TNodeDeque |
Definition at line 86 of file gridslamprocessor.h.
typedef std::vector<GridSlamProcessor::TNode*> GMapping::GridSlamProcessor::TNodeVector |
Definition at line 85 of file gridslamprocessor.h.
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.
infoStr | the output stream |
Definition at line 90 of file gridslamprocessor.cpp.
|
virtual |
Deleted the gridslamprocessor
Definition at line 151 of file gridslamprocessor.cpp.
|
protected |
Copy constructor
Definition at line 28 of file gridslamprocessor.cpp.
GridSlamProcessor * GMapping::GridSlamProcessor::clone | ( | ) | const |
Definition at line 98 of file gridslamprocessor.cpp.
int GMapping::GridSlamProcessor::getBestParticleIndex | ( | ) | const |
Definition at line 498 of file gridslamprocessor.cpp.
|
inline |
Definition at line 175 of file gridslamprocessor.h.
|
inline |
Definition at line 173 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.
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]
|
inlineprivate |
normalizes the particle weights
|
virtual |
Reimplemented in GridSlamProcessorThread.
Definition at line 511 of file gridslamprocessor.cpp.
|
virtual |
Reimplemented in GridSlamProcessorThread.
Definition at line 510 of file gridslamprocessor.cpp.
|
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.
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
GMapping::GridSlamProcessor::PARAM_SET_GET | ( | double | , |
minimumScore | , | ||
protected | , | ||
public | , | ||
public | |||
) |
minimum score for considering the outcome of the scanmatching good
|
protected |
this sets the neff based resampling threshold
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
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.
|
private |
Definition at line 233 of file gridslamprocessor_tree.cpp.
|
inlineprivate |
|
private |
Definition at line 207 of file gridslamprocessor_tree.cpp.
|
inlineprivate |
scanmatches all the particles
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.
|
inline |
Definition at line 152 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]
|
private |
Definition at line 198 of file gridslamprocessor_tree.cpp.
|
protected |
Definition at line 253 of file gridslamprocessor.h.
|
protected |
Definition at line 276 of file gridslamprocessor.h.
|
protected |
the laser beams
Definition at line 252 of file gridslamprocessor.h.
|
protected |
Definition at line 272 of file gridslamprocessor.h.
|
protected |
the particle indexes after resampling (internally used)
Definition at line 260 of file gridslamprocessor.h.
|
protected |
Definition at line 307 of file gridslamprocessor.h.
|
protected |
Definition at line 273 of file gridslamprocessor.h.
|
protected |
Definition at line 276 of file gridslamprocessor.h.
ScanMatcher GMapping::GridSlamProcessor::m_matcher |
the scanmatcher algorithm
Definition at line 167 of file gridslamprocessor.h.
|
protected |
the motion model
Definition at line 266 of file gridslamprocessor.h.
|
protected |
Definition at line 274 of file gridslamprocessor.h.
|
protected |
Definition at line 304 of file gridslamprocessor.h.
|
protected |
the particles
Definition at line 257 of file gridslamprocessor.h.
|
protected |
Definition at line 275 of file gridslamprocessor.h.
|
protected |
Definition at line 272 of file gridslamprocessor.h.
|
protected |
the particle weights (internally used)
Definition at line 263 of file gridslamprocessor.h.
|
protected |
Definition at line 254 of file gridslamprocessor.h.