Main MRPT website > C++ reference
MRPT logo
CHMTSLAM.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CHMTSLAM_H
29 #define CHMTSLAM_H
30 
31 #include <mrpt/synch.h>
32 #include <mrpt/system.h>
33 #include <mrpt/utils/CDebugOutputCapable.h>
34 #include <mrpt/utils/CMessageQueue.h>
35 
36 #include <mrpt/hmtslam/HMT_SLAM_common.h>
37 #include <mrpt/hmtslam/CLocalMetricHypothesis.h>
38 #include <mrpt/hmtslam/CHierarchicalMHMap.h>
39 
40 #include <mrpt/hmtslam/CTopLCDetector_GridMatching.h>
41 #include <mrpt/hmtslam/CTopLCDetector_FabMap.h>
42 
43 #include <mrpt/hmtslam/link_pragmas.h>
44 
45 #include <mrpt/slam/CICP.h>
46 #include <mrpt/slam/CPointsMap.h>
47 #include <mrpt/slam/TKLDParams.h>
48 
49 #include <mrpt/slam/CActionCollection.h>
50 
51 #include <mrpt/opengl/COpenGLScene.h>
52 
53 #include <queue>
54 
55 
56 namespace mrpt
57 {
58  /** Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM. \ingroup mrpt_hmtslam_grp */
59  namespace hmtslam
60  {
61  using namespace mrpt::utils;
62  using namespace mrpt::system;
63  using namespace mrpt::math;
64  using namespace mrpt::bayes;
65  using namespace mrpt::slam;
66 
67 
68  class CHMTSLAM;
69  class CLSLAMAlgorithmBase;
70  class CLSLAM_RBPF_2DLASER;
71 
72  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CHMTSLAM, mrpt::utils::CSerializable, HMTSLAM_IMPEXP )
73 
74  /** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
75  *
76  * The main entry points for a user are pushAction() and pushObservations(). Several parameters can be modified
77  * through m_options.
78  *
79  * The mathematical models of this approach have been reported in:
80  * - Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J., "Towards a Unified Bayesian Approach to Hybrid Metric-Topological SLAM", in IEEE Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
81  * - ...
82  *
83  * More information in the wiki page: http://www.mrpt.org/HMT-SLAM . A complete working application can be found in "MRPT/apps/hmt-slam".
84  *
85  * The complete state of the SLAM framework is serializable, so it can be saved and restore to/from a binary dump. This class implements mrpt::utils::CSerializable, so
86  * it can be saved with "stream << slam_object;" and restored with "stream >> slam_object;". Alternatively, the methods CHMTSLAM::saveState and CHMTSLAM::loadState
87  * can be invoked, which in turn call internally to the CSerializable interface.
88  *
89  * \sa CHierarchicalMHMap
90  * \ingroup mrpt_hmtslam_grp
91  */
92  class HMTSLAM_IMPEXP CHMTSLAM : public mrpt::utils::CDebugOutputCapable, public mrpt::utils::CSerializable
93  {
94  friend class HMTSLAM_IMPEXP CLocalMetricHypothesis;
95  friend class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER;
96  friend class HMTSLAM_IMPEXP CTopLCDetector_GridMatching;
97  friend class HMTSLAM_IMPEXP CTopLCDetector_FabMap;
98 
99  // This must be added to any CSerializable derived class:
100  DEFINE_SERIALIZABLE( CHMTSLAM )
101 
102  protected:
103 
104 
105  /** @name Inter-thread communication queues:
106  @{ */
107  /** Message definition:
108  - From: LSLAM
109  - To: AA
110  - Meaning: Reconsider the graph partition for the given LMH. Compute SSO for the new node(s) "newPoseIDs".
111  */
112  /*struct TMessageAA
113  {
114  CLocalMetricHypothesis *LMH;
115  TPoseIDList newPoseIDs;
116  };*/
117 
118  /** Message definition:
119  - From: AA
120  - To: LSLAM
121  - Meaning: The partitioning of robot poses.
122  */
123  struct TMessageLSLAMfromAA
124  {
125  THypothesisID hypothesisID; //!< The hypothesis ID (LMH) for these results.
126  std::vector< TPoseIDList > partitions;
127 
128  void dumpToConsole( ) const; //!< for debugging only
129  };
130  typedef stlplus::smart_ptr<TMessageLSLAMfromAA> TMessageLSLAMfromAAPtr;
131 
132  /** Message definition:
133  - From: LSLAM
134  - To: TBI
135  - Meaning: One or more areas are to be considered by the TBI engines.
136  */
137  struct TMessageLSLAMtoTBI
138  {
139  CLocalMetricHypothesis *LMH; //!< The LMH
140  TNodeIDList areaIDs; //!< The areas to consider.
141  };
142  typedef stlplus::smart_ptr<TMessageLSLAMtoTBI> TMessageLSLAMtoTBIPtr;
143 
144  /** Message definition:
145  - From: TBI
146  - To: LSLAM
147  - Meaning:
148  */
149  struct TMessageLSLAMfromTBI
150  {
151  THypothesisID hypothesisID; //!< The hypothesis ID (LMH) for these results.
152 
153  CHMHMapNode::TNodeID cur_area; //!< The area for who the loop-closure has been computed.
154 
155  struct TBI_info
156  {
157  TBI_info() : log_lik(0),delta_new_cur(0)
158  {}
159 
160  double log_lik; //!< Log likelihood for this loop-closure.
161 
162  /** Depending on the loop-closure engine, an guess of the relative pose of "area_new" relative to "cur_area" is given here.
163  * If the SOG contains 0 modes, then the engine does not provide this information.
164  */
165  CPose3DPDFSOG delta_new_cur;
166  };
167 
168  /** The meat is here: only feasible loop closures from "cur_area" are included here, with associated data.
169  */
170  std::map< CHMHMapNode::TNodeID, TBI_info > loopClosureData;
171 
172  //EIGEN_MAKE_ALIGNED_OPERATOR_NEW
173  };
174  typedef stlplus::smart_ptr<TMessageLSLAMfromTBI> TMessageLSLAMfromTBIPtr;
175 
176 
177  utils::CMessageQueue m_LSLAM_queue; //!< LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA
178 
179  /** @} */
180 
181  /** The Area Abstraction (AA) method, invoked from LSLAM.
182  * \param LMH (IN) The LMH which to this query applies.
183  * \param newPoseIDs (IN) The new poseIDs to be added to the graph partitioner.
184  * \return A structure with all return data. Memory to be freed by user.
185  * \note The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section).
186  */
187  static TMessageLSLAMfromAAPtr areaAbstraction(
188  CLocalMetricHypothesis *LMH,
189  const TPoseIDList &newPoseIDs );
190 
191 
192  /** The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM.
193  * \param LMH (IN) The LMH which to this query applies.
194  * \param areaID (IN) The area ID to consider for potential loop-closures.
195  * \note The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section).
196  */
197  static TMessageLSLAMfromTBIPtr TBI_main_method(
198  CLocalMetricHypothesis *LMH,
199  const CHMHMapNode::TNodeID &areaID
200  );
201 
202  /** @name Related to the input queue:
203  @{ */
204  public:
205  /** Empty the input queue. */
206  void clearInputQueue();
207 
208  /** Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions)
209  * \sa pushAction,pushObservations, inputQueueSize
210  */
211  bool isInputQueueEmpty();
212 
213  /** Returns the number of objects waiting for processing in the input queue.
214  * \sa pushAction,pushObservations, isInputQueueEmpty
215  */
216  size_t inputQueueSize();
217 
218  /** Here the user can enter an action into the system (will go to the SLAM process).
219  * This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
220  * \sa pushObservations,pushObservation
221  */
222  void pushAction( const CActionCollectionPtr &acts );
223 
224  /** Here the user can enter observations into the system (will go to the SLAM process).
225  * This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
226  * \sa pushAction,pushObservation
227  */
228  void pushObservations( const CSensoryFramePtr &sf );
229 
230  /** Here the user can enter an observation into the system (will go to the SLAM process).
231  * This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
232  * \sa pushAction,pushObservation
233  */
234  void pushObservation( const CObservationPtr &obs );
235 
236  enum TLSlamMethod
237  {
238  lsmRBPF_2DLASER = 1
239  };
240 
241  protected:
242  /** Used from the LSLAM thread to retrieve the next object from the queue.
243  * \return The object, or NULL if empty.
244  */
245  CSerializablePtr getNextObjectFromInputQueue();
246 
247  /** The queue of pending actions/observations supplied by the user waiting for being processed. */
248  std::queue<CSerializablePtr> m_inputQueue;
249 
250  /** Critical section for accessing m_inputQueue */
251  synch::CCriticalSection m_inputQueue_cs;
252 
253  /** Critical section for accessing m_map */
254  synch::CCriticalSection m_map_cs;
255 
256  synch::CCriticalSection m_LMHs_cs; //!< Critical section for accessing m_LMHs
257 
258  /** @} */
259 
260 
261  /** @name Threads stuff
262  @{ */
263 
264  /** The function for the "Local SLAM" thread. */
265  void thread_LSLAM( );
266 
267  /** The function for the "TBI" thread. */
268  void thread_TBI( );
269 
270  /** The function for the "3D viewer" thread. */
271  void thread_3D_viewer( );
272  /** Threads handles */
273  TThreadHandle m_hThread_LSLAM,
274  m_hThread_TBI,
275  m_hThread_3D_viewer;
276  /** @} */
277 
278 
279  /** @name HMT-SLAM sub-processes.
280  @{ */
281  void LSLAM_process_message( const CMessage &msg ); //!< Auxiliary method within thread_LSLAM
282 
283  /** No critical section locks are assumed at the entrance of this method.
284  */
285  void LSLAM_process_message_from_AA( const TMessageLSLAMfromAA &myMsg );
286 
287  /** No critical section locks are assumed at the entrance of this method.
288  */
289  void LSLAM_process_message_from_TBI( const TMessageLSLAMfromTBI &myMsg );
290 
291  /** Topological Loop Closure: Performs all the required operations
292  to close a loop between two areas which have been determined
293  to be the same.
294  */
295  void perform_TLC(
296  CLocalMetricHypothesis &LMH,
297  const CHMHMapNode::TNodeID areaInLMH,
298  const CHMHMapNode::TNodeID areaLoopClosure,
299  const mrpt::poses::CPose3DPDFGaussian &pose1wrt2
300  );
301 
302 
303  /** @} */
304 
305  /** @name The different SLAM algorithms that can be invoked from the LSLAM thread.
306  @{ */
307 
308  /** An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" or "loadState".
309  */
310  CLSLAMAlgorithmBase *m_LSLAM_method;
311 
312  /** @} */
313 
314  /** @name The different Loop-Closure modules that are to be executed in the TBI thread.
315  @{ */
316  protected:
317 
318  typedef CTopLCDetectorBase* (*TLopLCDetectorFactory)(CHMTSLAM*);
319 
320  std::map<std::string,TLopLCDetectorFactory> m_registeredLCDetectors;
321 
322  /** The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState". */
323  std::deque<CTopLCDetectorBase*> m_topLCdets;
324 
325  /** The critical section for accessing m_topLCdets */
326  synch::CCriticalSection m_topLCdets_cs;
327  public:
328 
329  /** Must be invoked before calling initializeEmptyMap, so LC objects can be created. */
330  void registerLoopClosureDetector(
331  const std::string &name,
332  CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*)
333  );
334 
335  /** The class factory for topological loop closure detectors.
336  * Possible values are enumerated in TOptions::TLC_detectors
337  *
338  * \exception std::exception On unknown name.
339  */
340  CTopLCDetectorBase* loopClosureDetector_factory(const std::string &name);
341 
342 
343  /** @} */
344  protected:
345 
346 
347  /** Termination flag for signaling all threads to terminate */
348  bool m_terminateThreads;
349 
350  /** Threads termination flags:
351  */
352  bool m_terminationFlag_LSLAM,
353  m_terminationFlag_TBI,
354  m_terminationFlag_3D_viewer;
355 
356  /** Generates a new and unique area textual label (currently this generates "0","1",...) */
357  static std::string generateUniqueAreaLabel();
358 
359  /** Generates a new and unique pose ID */
360  static TPoseID generatePoseID();
361 
362  /** Generates a new and unique hypothesis ID */
363  static THypothesisID generateHypothesisID();
364 
365  static int64_t m_nextAreaLabel;
366  static TPoseID m_nextPoseID;
367  static THypothesisID m_nextHypID;
368 
369 
370  public:
371  /** Default constructor
372  * \param debug_out_stream If debug output messages should be redirected to any other stream apart from std::cout
373  */
374  CHMTSLAM( );
375 
376  CHMTSLAM(const CHMTSLAM &o) { THROW_EXCEPTION("This object cannot be copied."); }
377  const CHMTSLAM& operator =(const CHMTSLAM &o) { THROW_EXCEPTION("This object cannot be copied."); }
378 
379  /** Destructor
380  */
381  virtual ~CHMTSLAM();
382 
383  /** Return true if an exception has been caught in any thread leading to the end of the mapping application: no more actions/observations will be processed from now on.
384  */
385  bool abortedDueToErrors();
386 
387 
388  /** @name High-level map management
389  @{ */
390 
391  /** Loads the options from a config file. */
392  void loadOptions( const std::string &configFile );
393  /** Loads the options from a config source */
394  void loadOptions( const mrpt::utils::CConfigFileBase &cfgSource );
395 
396  /** Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory) - this must be called AFTER loading the options with CHMTSLAM::loadOptions. */
397  void initializeEmptyMap();
398 
399  /** Save the state of the whole HMT-SLAM framework to some binary stream (e.g. a file).
400  * \return true if everything goes OK.
401  * \sa loadState
402  */
403  bool saveState( CStream &out ) const;
404 
405  /** Load the state of the whole HMT-SLAM framework from some binary stream (e.g. a file).
406  * \return true if everything goes OK.
407  * \sa saveState
408  */
409  bool loadState( CStream &in );
410  /** @} */
411 
412  /** @name The important data.
413  @{ */
414  CHierarchicalMHMap m_map; //!< The hiearchical, multi-hypothesis graph-based map.
415  aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t m_LMHs; //!< The list of LMHs at each instant.
416  /** @} */
417 
418  /** Called from LSLAM thread when log files must be created.
419  */
420  void generateLogFiles(unsigned int nIteration);
421 
422 
423  /** Gets a 3D representation of the current state of the whole mapping framework.
424  */
425  void getAs3DScene( COpenGLScene &outScene );
426 
427  protected:
428  /** A variety of options and configuration params (private, use loadOptions).
429  */
430  struct TOptions : public utils::CLoadableOptions
431  {
432  /** Initialization of default params
433  */
434  TOptions();
435 
436  /** Load parameters from configuration source
437  */
438  void loadFromConfigFile(
439  const mrpt::utils::CConfigFileBase &source,
440  const std::string &section);
441 
442  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
443  */
444  void dumpToTextStream(CStream &out) const;
445 
446 
447  std::string LOG_OUTPUT_DIR; //!< [LOGGING] If it is not an empty string (""), a directory with that name will be created and log files save there.
448  int LOG_FREQUENCY; //!< [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
449 
450  /** [LSLAM] The method to use for local SLAM
451  */
452  TLSlamMethod SLAM_METHOD;
453 
454  /** [LSLAM] Minimum distance (and heading) difference between observations inserted in the map.
455  */
456  float SLAM_MIN_DIST_BETWEEN_OBS, SLAM_MIN_HEADING_BETWEEN_OBS;
457 
458  /** [LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0) */
459  float MIN_ODOMETRY_STD_XY;
460 
461  /** [LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry increments (Default=0) */
462  float MIN_ODOMETRY_STD_PHI;
463 
464  /** [VIEW3D] The height of the areas' spheres.
465  */
466  float VIEW3D_AREA_SPHERES_HEIGHT;
467 
468  /** [VIEW3D] The radius of the areas' spheres.
469  */
470  float VIEW3D_AREA_SPHERES_RADIUS;
471 
472  /** A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y: In meters, phi: radians (but in degrees when loading from a configuration ini-file!)
473  */
474  vector_float stds_Q_no_odo;
475 
476  /** [AA] The options for the partitioning algorithm
477  */
478  CIncrementalMapPartitioner::TOptions AA_options;
479 
480  TSetOfMetricMapInitializers defaultMapsInitializers; //!< The default set of maps to be created in each particle
481 
482  CMultiMetricMap::TOptions defaultMapsOptions; //!< The default options for the CMultiMetricMap in each particle.
483 
484  bayes::CParticleFilter::TParticleFilterOptions pf_options; //!< These params are used from every LMH object.
485 
486  TKLDParams KLD_params;
487 
488  int random_seed; //!< 0 means randomize, use any other value to have repetitive experiments.
489 
490  /** A list of topological loop-closure detectors to use: can be one or more from this list:
491  * 'gridmaps': Occupancy Grid matching.
492  * 'fabmap': Mark Cummins' image matching framework.
493  */
494  vector_string TLC_detectors;
495 
496  CTopLCDetector_GridMatching::TOptions TLC_grid_options; //!< Options passed to this TLC constructor
497  CTopLCDetector_FabMap::TOptions TLC_fabmap_options; //!< Options passed to this TLC constructor
498 
499  } m_options;
500 
501  }; // End of class CHMTSLAM.
502 
503 
504  /** Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
505  */
506  class HMTSLAM_IMPEXP CLSLAMAlgorithmBase
507  {
508  friend class HMTSLAM_IMPEXP CLocalMetricHypothesis;
509  protected:
510  safe_ptr<CHMTSLAM> m_parent;
511 
512  public:
513  /** Constructor
514  */
515  CLSLAMAlgorithmBase( CHMTSLAM *parent ) : m_parent(parent) { }
516 
517  /** Destructor
518  */
519  virtual ~CLSLAMAlgorithmBase() {}
520 
521  /** Main entry point from HMT-SLAM: process some actions & observations.
522  * The passed action/observation will be deleted, so a copy must be made if necessary.
523  * This method must be in charge of updating the robot pose estimates and also to update the
524  * map when required.
525  *
526  * \param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
527  * \param act The action to process (or NULL).
528  * \param sf The observations to process (or NULL).
529  */
530  virtual void processOneLMH(
531  CLocalMetricHypothesis *LMH,
532  const CActionCollectionPtr &act,
533  const CSensoryFramePtr &sf ) = 0;
534 
535 
536  /** The PF algorithm implementation.
537  */
538  virtual void prediction_and_update_pfAuxiliaryPFOptimal(
539  CLocalMetricHypothesis * LMH,
540  const mrpt::slam::CActionCollection * action,
541  const mrpt::slam::CSensoryFrame * observation,
542  const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) = 0;
543 
544  /** The PF algorithm implementation. */
545  virtual void prediction_and_update_pfOptimalProposal(
546  CLocalMetricHypothesis *LMH,
547  const mrpt::slam::CActionCollection * action,
548  const mrpt::slam::CSensoryFrame * observation,
549  const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) = 0;
550 
551  }; // end of class CLSLAMAlgorithmBase
552 
553 
554  /** Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
555  * This class is used internally in mrpt::slam::CHMTSLAM
556  */
557  class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER : public CLSLAMAlgorithmBase
558  {
559  friend class HMTSLAM_IMPEXP CLocalMetricHypothesis;
560 
561  public:
562  /** Constructor
563  */
564  CLSLAM_RBPF_2DLASER( CHMTSLAM *parent );
565 
566  /** Destructor
567  */
568  virtual ~CLSLAM_RBPF_2DLASER();
569 
570  /** Main entry point from HMT-SLAM: process some actions & observations.
571  * The passed action/observation will be deleted, so a copy must be made if necessary.
572  * This method must be in charge of updating the robot pose estimates and also to update the
573  * map when required.
574  *
575  * \param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
576  * \param act The action to process (or NULL).
577  * \param sf The observations to process (or NULL).
578  */
579  void processOneLMH(
580  CLocalMetricHypothesis *LMH,
581  const CActionCollectionPtr &act,
582  const CSensoryFramePtr &sf );
583 
584  /** The PF algorithm implementation. */
585  void prediction_and_update_pfAuxiliaryPFOptimal(
586  CLocalMetricHypothesis *LMH,
587  const mrpt::slam::CActionCollection * action,
588  const mrpt::slam::CSensoryFrame * observation,
589  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
590 
591  /** The PF algorithm implementation. */
592  void prediction_and_update_pfOptimalProposal(
593  CLocalMetricHypothesis *LMH,
594  const mrpt::slam::CActionCollection * action,
595  const mrpt::slam::CSensoryFrame * observation,
596  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
597 
598  protected:
599  bool m_insertNewRobotPose; //!< For use within PF callback methods
600 
601  /** Auxiliary structure
602  */
603  struct TPathBin
604  {
605  TPathBin() : x(),y(),phi()
606  {}
607 
608  vector_int x,y,phi;
609 
610  /** For debugging purposes!
611  */
612  void dumpToStdOut() const;
613  };
614 
615 
616  /** Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
617  */
618  void loadTPathBinFromPath(
619  TPathBin &outBin,
620  std::map<TPoseID,CPose3D> *path = NULL,
621  CPose2D *newPose = NULL );
622 
623  /** Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found.
624  */
625  int findTPathBinIntoSet(
626  TPathBin &desiredBin,
627  std::deque<TPathBin> &theSet
628  );
629 
630  /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
631  */
632  static double particlesEvaluator_AuxPFOptimal(
633  const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
634  const CParticleFilterCapable *obj,
635  size_t index,
636  const void *action,
637  const void *observation );
638 
639  /** Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".
640  */
641  static double auxiliarComputeObservationLikelihood(
642  const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
643  const CParticleFilterCapable *obj,
644  size_t particleIndexForMap,
645  const CSensoryFrame *observation,
646  const CPose2D *x );
647 
648  }; // end class CLSLAM_RBPF_2DLASER
649 
650  } // End of namespace
651 } // End of namespace
652 
653 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013