gridslamprocessor.h
Go to the documentation of this file.
00001 #ifndef GRIDSLAMPROCESSOR_H
00002 #define GRIDSLAMPROCESSOR_H
00003 
00004 #include <climits>
00005 #include <limits>
00006 #include <fstream>
00007 #include <vector>
00008 #include <deque>
00009 #include <gmapping/particlefilter/particlefilter.h>
00010 #include <gmapping/utils/point.h>
00011 #include <gmapping/utils/macro_params.h>
00012 #include <gmapping/log/sensorlog.h>
00013 #include <gmapping/sensor/sensor_range/rangesensor.h>
00014 #include <gmapping/sensor/sensor_range/rangereading.h>
00015 #include <gmapping/scanmatcher/scanmatcher.h>
00016 #include "motionmodel.h"
00017 
00018 
00019 namespace GMapping {
00020 
00034   class GridSlamProcessor{
00035   public:
00036 
00037     
00042     struct TNode{
00050       TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0);
00051 
00054       ~TNode();
00055 
00057       OrientedPoint pose; 
00058       
00060       double weight;
00061 
00063       double accWeight;
00064 
00065       double gweight;
00066 
00067 
00069       TNode* parent;
00070 
00072       const RangeReading* reading;
00073 
00075       unsigned int childs;
00076 
00078       mutable unsigned int visitCounter;
00079 
00081       mutable bool flag;
00082     };
00083     
00084     typedef std::vector<GridSlamProcessor::TNode*> TNodeVector;
00085     typedef std::deque<GridSlamProcessor::TNode*> TNodeDeque;
00086     
00088     struct Particle{
00092       Particle(const ScanMatcherMap& map);
00093 
00095       inline operator double() const {return weight;}
00097       inline operator OrientedPoint() const {return pose;}
00101       inline void setWeight(double w) {weight=w;}
00103       ScanMatcherMap map;
00105       OrientedPoint pose;
00106 
00108       OrientedPoint previousPose;
00109 
00111       double weight;
00112 
00114       double weightSum;
00115 
00116       double gweight;
00117 
00119       int previousIndex;
00120 
00122       TNode* node; 
00123     };
00124         
00125     
00126     typedef std::vector<Particle> ParticleVector;
00127     
00129     GridSlamProcessor();
00130 
00134     GridSlamProcessor(std::ostream& infoStr);
00135     
00138     GridSlamProcessor* clone() const;
00139     
00141     virtual ~GridSlamProcessor();
00142     
00143     //methods for accessing the parameters
00144     void setSensorMap(const SensorMap& smap);
00145     void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, 
00146               OrientedPoint initialPose=OrientedPoint(0,0,0));
00147     void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, 
00148                                int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0);
00149     void setMotionModelParameters(double srr, double srt, double str, double stt);
00150     void setUpdateDistances(double linear, double angular, double resampleThreshold);
00151     void setUpdatePeriod(double p) {period_=p;}
00152     
00153     //the "core" algorithm
00154     void processTruePos(const OdometryReading& odometry);
00155     bool processScan(const RangeReading & reading, int adaptParticles=0);
00156     
00162     TNodeVector getTrajectories() const;
00163     void integrateScanSequence(TNode* node);
00164     
00166     ScanMatcher m_matcher;
00168     std::ofstream& outputStream();
00170     std::ostream& infoStream();
00172     inline const ParticleVector& getParticles() const {return m_particles; }
00173     
00174     inline const std::vector<unsigned int>& getIndexes() const{return m_indexes; }
00175     int getBestParticleIndex() const;
00176     //callbacks
00177     virtual void onOdometryUpdate();
00178     virtual void onResampleUpdate();
00179     virtual void onScanmatchUpdate();
00180         
00181     //accessor methods
00183     MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public);
00184 
00186     MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public);
00187 
00189     MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public);
00190 
00192     MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public);
00193 
00195     MEMBER_PARAM_SET_GET(m_matcher, int,    kernelSize, protected, public, public);
00196 
00198     MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public);
00199 
00201     MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public);
00202 
00204     MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public);
00205 
00207     MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public);
00208 
00210     MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public);
00211 
00213     MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public);
00214 
00216     MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public);
00217 
00219     MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public);
00220 
00222     MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public);
00223 
00225     MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public);
00226 
00228     MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public);
00229 
00230 
00232     STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public);
00233 
00235     STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public);
00236 
00238     STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public);
00239 
00241     STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public);
00242                 
00244     PARAM_SET_GET(double, minimumScore, protected, public, public);
00245 
00246   protected:
00248     GridSlamProcessor(const GridSlamProcessor& gsp);
00249  
00251     unsigned int m_beams;
00252     double last_update_time_;
00253     double period_;
00254     
00256     ParticleVector m_particles;
00257 
00259     std::vector<unsigned int> m_indexes;
00260 
00262     std::vector<double> m_weights;
00263     
00265     MotionModel m_motionModel;
00266 
00268     PARAM_SET_GET(double, resampleThreshold, protected, public, public);
00269       
00270     //state
00271     int  m_count, m_readingCount;
00272     OrientedPoint m_lastPartPose;
00273     OrientedPoint m_odoPose;
00274     OrientedPoint m_pose;
00275     double m_linearDistance, m_angularDistance;
00276     PARAM_GET(double, neff, protected, public);
00277       
00278     //processing parameters (size of the map)
00279     PARAM_GET(double, xmin, protected, public);
00280     PARAM_GET(double, ymin, protected, public);
00281     PARAM_GET(double, xmax, protected, public);
00282     PARAM_GET(double, ymax, protected, public);
00283     //processing parameters (resolution of the map)
00284     PARAM_GET(double, delta, protected, public);
00285         
00286     //registration score (if a scan score is above this threshold it is registered in the map)
00287     PARAM_SET_GET(double, regScore, protected, public, public);
00288     //registration score (if a scan score is below this threshold a scan matching failure is reported)
00289     PARAM_SET_GET(double, critScore, protected, public, public);
00290     //registration score maximum move allowed between consecutive scans
00291     PARAM_SET_GET(double, maxMove, protected, public, public);
00292         
00293     //process a scan each time the robot translates of linearThresholdDistance
00294     PARAM_SET_GET(double, linearThresholdDistance, protected, public, public);
00295 
00296     //process a scan each time the robot rotates more than angularThresholdDistance
00297     PARAM_SET_GET(double, angularThresholdDistance, protected, public, public);
00298     
00299     //smoothing factor for the likelihood
00300     PARAM_SET_GET(double, obsSigmaGain, protected, public, public);
00301         
00302     //stream in which to write the gfs file
00303     std::ofstream m_outputStream;
00304 
00305     // stream in which to write the messages
00306     std::ostream& m_infoStream;
00307     
00308     
00309     // the functions below performs side effect on the internal structure,
00310     //should be called only inside the processScan method
00311   private:
00312     
00314     inline void scanMatch(const double *plainReading);
00316     inline void normalize();
00317     
00318     // return if a resampling occured or not
00319     inline bool resample(const double* plainReading, int adaptParticles, 
00320                          const RangeReading* rr=0);
00321     
00322     //tree utilities
00323     
00324     void updateTreeWeights(bool weightsAlreadyNormalized = false);
00325     void resetTree();
00326     double propagateWeights();
00327     
00328   };
00329 
00330 typedef std::multimap<const GridSlamProcessor::TNode*, GridSlamProcessor::TNode*> TNodeMultimap;
00331 
00332 
00333 #include "gridslamprocessor.hxx"
00334 
00335 };
00336 
00337 #endif


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:12