gridslamprocessor.h
Go to the documentation of this file.
1 #ifndef GRIDSLAMPROCESSOR_H
2 #define GRIDSLAMPROCESSOR_H
3 
4 #include <climits>
5 #include <limits>
6 #include <fstream>
7 #include <vector>
8 #include <deque>
10 #include <gmapping/utils/point.h>
12 #include <gmapping/log/sensorlog.h>
17 #include <gmapping/gridfastslam/gridfastslam_export.h>
18 
19 
20 namespace GMapping {
21 
35  class GRIDFASTSLAM_EXPORT GridSlamProcessor{
36  public:
37 
38 
43  struct TNode{
51  TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0);
52 
55  ~TNode();
56 
59 
61  double weight;
62 
64  double accWeight;
65 
66  double gweight;
67 
68 
71 
74 
76  unsigned int childs;
77 
79  mutable unsigned int visitCounter;
80 
82  mutable bool flag;
83  };
84 
85  typedef std::vector<GridSlamProcessor::TNode*> TNodeVector;
86  typedef std::deque<GridSlamProcessor::TNode*> TNodeDeque;
87 
89  struct Particle{
93  Particle(const ScanMatcherMap& map);
94 
96  inline operator double() const {return weight;}
98  inline operator OrientedPoint() const {return pose;}
102  inline void setWeight(double w) {weight=w;}
107 
110 
112  double weight;
113 
115  double weightSum;
116 
117  double gweight;
118 
121 
124  };
125 
126 
127  typedef std::vector<Particle> ParticleVector;
128 
131 
135  GridSlamProcessor(std::ostream& infoStr);
136 
139  GridSlamProcessor* clone() const;
140 
142  virtual ~GridSlamProcessor();
143 
144  //methods for accessing the parameters
145  void setSensorMap(const SensorMap& smap);
146  void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta,
147  OrientedPoint initialPose=OrientedPoint(0,0,0));
148  void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt,
149  int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0);
150  void setMotionModelParameters(double srr, double srt, double str, double stt);
151  void setUpdateDistances(double linear, double angular, double resampleThreshold);
152  void setUpdatePeriod(double p) {period_=p;}
153 
154  //the "core" algorithm
155  void processTruePos(const OdometryReading& odometry);
156  bool processScan(const RangeReading & reading, int adaptParticles=0);
157 
163  TNodeVector getTrajectories() const;
164  void integrateScanSequence(TNode* node);
165 
169  std::ofstream& outputStream();
171  std::ostream& infoStream();
173  inline const ParticleVector& getParticles() const {return m_particles; }
174 
175  inline const std::vector<unsigned int>& getIndexes() const{return m_indexes; }
176  int getBestParticleIndex() const;
177  //callbacks
178  virtual void onOdometryUpdate();
179  virtual void onResampleUpdate();
180  virtual void onScanmatchUpdate();
181 
182  //accessor methods
184  MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public);
185 
187  MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public);
188 
190  MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public);
191 
193  MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public);
194 
196  MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public);
197 
199  MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public);
200 
202  MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public);
203 
205  MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public);
206 
208  MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public);
209 
211  MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public);
212 
214  MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public);
215 
217  MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public);
218 
220  MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public);
221 
223  MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public);
224 
226  MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public);
227 
229  MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public);
230 
231 
233  STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public);
234 
236  STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public);
237 
239  STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public);
240 
242  STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public);
243 
245  PARAM_SET_GET(double, minimumScore, protected, public, public);
246 
247  protected:
250 
252  unsigned int m_beams;
254  double period_;
255 
257  ParticleVector m_particles;
258 
260  std::vector<unsigned int> m_indexes;
261 
263  std::vector<double> m_weights;
264 
267 
269  PARAM_SET_GET(double, resampleThreshold, protected, public, public);
270 
271  //state
272  int m_count, m_readingCount;
276  double m_linearDistance, m_angularDistance;
277  PARAM_GET(double, neff, protected, public);
278 
279  //processing parameters (size of the map)
280  PARAM_GET(double, xmin, protected, public);
281  PARAM_GET(double, ymin, protected, public);
282  PARAM_GET(double, xmax, protected, public);
283  PARAM_GET(double, ymax, protected, public);
284  //processing parameters (resolution of the map)
285  PARAM_GET(double, delta, protected, public);
286 
287  //registration score (if a scan score is above this threshold it is registered in the map)
288  PARAM_SET_GET(double, regScore, protected, public, public);
289  //registration score (if a scan score is below this threshold a scan matching failure is reported)
290  PARAM_SET_GET(double, critScore, protected, public, public);
291  //registration score maximum move allowed between consecutive scans
292  PARAM_SET_GET(double, maxMove, protected, public, public);
293 
294  //process a scan each time the robot translates of linearThresholdDistance
295  PARAM_SET_GET(double, linearThresholdDistance, protected, public, public);
296 
297  //process a scan each time the robot rotates more than angularThresholdDistance
298  PARAM_SET_GET(double, angularThresholdDistance, protected, public, public);
299 
300  //smoothing factor for the likelihood
301  PARAM_SET_GET(double, obsSigmaGain, protected, public, public);
302 
303  //stream in which to write the gfs file
304  std::ofstream m_outputStream;
305 
306  // stream in which to write the messages
307  std::ostream& m_infoStream;
308 
309 
310  // the functions below performs side effect on the internal structure,
311  //should be called only inside the processScan method
312  private:
313 
315  inline void scanMatch(const double *plainReading);
317  inline void normalize();
318 
319  // return if a resampling occured or not
320  inline bool resample(const double* plainReading, int adaptParticles,
321  const RangeReading* rr=0);
322 
323  //tree utilities
324 
325  void updateTreeWeights(bool weightsAlreadyNormalized = false);
326  void resetTree();
327  double propagateWeights();
328 
329  };
330 
331 typedef std::multimap<const GridSlamProcessor::TNode*, GridSlamProcessor::TNode*> TNodeMultimap;
332 
333 
334 #include "gmapping/gridfastslam/gridslamprocessor.hxx"
335 
336 };
337 
338 #endif
const char *const *argv double delta
Definition: gfs2stream.cpp:19
bool neff
Definition: gfs2stream.cpp:39
void normalize(const Iterator &begin, const Iterator &end)
std::vector< double > m_weights
#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)
Definition: macro_params.h:4
std::vector< GridSlamProcessor::TNode * > TNodeVector
std::vector< unsigned int > m_indexes
const std::vector< unsigned int > & getIndexes() const
std::vector< Particle > ParticleVector
#define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)
Definition: macro_params.h:27
std::deque< GridSlamProcessor::TNode * > TNodeDeque
std::multimap< const GridSlamProcessor::TNode *, GridSlamProcessor::TNode * > TNodeMultimap
const ParticleVector & getParticles() const
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
#define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)
Definition: macro_params.h:17
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
orientedpoint< double, double > OrientedPoint
Definition: point.h:203
#define PARAM_GET(type, name, qualifier, getqualifier)
Definition: macro_params.h:13


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20