Main MRPT website > C++ reference
MRPT logo

CHMTSLAM.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CHMTSLAM_H
00029 #define CHMTSLAM_H
00030 
00031 #include <mrpt/synch.h>
00032 #include <mrpt/system.h>
00033 #include <mrpt/utils/CDebugOutputCapable.h>
00034 #include <mrpt/utils/CMessageQueue.h>
00035 
00036 #include <mrpt/hmtslam/HMT_SLAM_common.h>
00037 #include <mrpt/hmtslam/CLocalMetricHypothesis.h>
00038 #include <mrpt/hmtslam/CHierarchicalMHMap.h>
00039 
00040 #include <mrpt/hmtslam/CTopLCDetector_GridMatching.h>
00041 #include <mrpt/hmtslam/CTopLCDetector_FabMap.h>
00042 
00043 #include <mrpt/hmtslam/link_pragmas.h>
00044 
00045 #include <mrpt/slam/CICP.h>
00046 #include <mrpt/slam/CPointsMap.h>
00047 #include <mrpt/slam/TKLDParams.h>
00048 
00049 #include <mrpt/slam/CActionCollection.h>
00050 
00051 #include <mrpt/opengl/COpenGLScene.h>
00052 
00053 #include <queue>
00054 
00055 
00056 namespace mrpt
00057 {
00058         /** Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM. */
00059         namespace hmtslam
00060         {
00061                 using namespace mrpt::utils;
00062                 using namespace mrpt::system;
00063                 using namespace mrpt::math;
00064                 using namespace mrpt::bayes;
00065                 using namespace mrpt::slam;
00066 
00067 
00068                 class CHMTSLAM;
00069                 class CLSLAMAlgorithmBase;
00070                 class CLSLAM_RBPF_2DLASER;
00071 
00072                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CHMTSLAM, mrpt::utils::CSerializable, HMTSLAM_IMPEXP )
00073 
00074                 /** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
00075                  *
00076                  *   The main entry points for a user are pushAction() and pushObservations(). Several parameters can be modified
00077                  *   through m_options.
00078                  *
00079                  *  The mathematical models of this approach have been reported in:
00080                  *              - 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.
00081                  *              - ...
00082                  *
00083                  *  More information in the wiki page: http://www.mrpt.org/HMT-SLAM. A complete working application can be found in "MRPT/apps/hmt-slam".
00084                  *
00085                  *  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
00086                  *    it can be saved with "stream << slam_object;" and restored with "stream >> slam_object;". Alternatively, the methods CHMTSLAM::saveState and CHMTSLAM::loadState
00087                  *    can be invoked, which in turn call internally to the CSerializable interface.
00088                  *
00089                  * \sa CHierarchicalMHMap
00090                  */
00091                 class HMTSLAM_IMPEXP CHMTSLAM : public mrpt::utils::CDebugOutputCapable, public mrpt::utils::CSerializable
00092                 {
00093                         friend class HMTSLAM_IMPEXP CLocalMetricHypothesis;
00094                         friend class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER;
00095                         friend class HMTSLAM_IMPEXP CTopLCDetector_GridMatching;
00096                         friend class HMTSLAM_IMPEXP CTopLCDetector_FabMap;
00097 
00098                         // This must be added to any CSerializable derived class:
00099                         DEFINE_SERIALIZABLE( CHMTSLAM )
00100 
00101                 protected:
00102 
00103 
00104                         /** @name Inter-thread communication queues:
00105                                 @{ */
00106                         /** Message definition:
00107                                 - From: LSLAM
00108                                 - To: AA
00109                                 - Meaning: Reconsider the graph partition for the given LMH. Compute SSO for the new node(s) "newPoseIDs".
00110                                 */
00111                         /*struct TMessageAA
00112                         {
00113                                 CLocalMetricHypothesis  *LMH;
00114                                 TPoseIDList                             newPoseIDs;
00115                         };*/
00116 
00117                         /** Message definition:
00118                                 - From: AA
00119                                 - To: LSLAM
00120                                 - Meaning: The partitioning of robot poses.
00121                                 */
00122                         struct TMessageLSLAMfromAA
00123                         {
00124                                 THypothesisID                           hypothesisID; //!< The hypothesis ID (LMH) for these results.
00125                                 std::vector< TPoseIDList >      partitions;
00126 
00127                                 void dumpToConsole( ) const;  //!< for debugging only
00128                         };
00129                         typedef stlplus::smart_ptr<TMessageLSLAMfromAA> TMessageLSLAMfromAAPtr;
00130 
00131                         /** Message definition:
00132                                 - From: LSLAM
00133                                 - To: TBI
00134                                 - Meaning: One or more areas are to be considered by the TBI engines.
00135                                 */
00136                         struct TMessageLSLAMtoTBI
00137                         {
00138                                 CLocalMetricHypothesis  *LMH;           //!< The LMH
00139                                 TNodeIDList                              areaIDs;       //!< The areas to consider.
00140                         };
00141                         typedef stlplus::smart_ptr<TMessageLSLAMtoTBI> TMessageLSLAMtoTBIPtr;
00142 
00143                         /** Message definition:
00144                                 - From: TBI
00145                                 - To: LSLAM
00146                                 - Meaning:
00147                                 */
00148                         struct TMessageLSLAMfromTBI
00149                         {
00150                                 THypothesisID                           hypothesisID; //!< The hypothesis ID (LMH) for these results.
00151 
00152                                 CHMHMapNode::TNodeID            cur_area; //!< The area for who the loop-closure has been computed.
00153 
00154                                 struct TBI_info
00155                                 {
00156                                         TBI_info() : log_lik(0),delta_new_cur(0)
00157                                         {}
00158 
00159                                         double          log_lik;  //!< Log likelihood for this loop-closure.
00160 
00161                                         /** Depending on the loop-closure engine, an guess of the relative pose of "area_new" relative to "cur_area" is given here.
00162                                           *  If the SOG contains 0 modes, then the engine does not provide this information.
00163                                           */
00164                                         CPose3DPDFSOG   delta_new_cur;
00165                                 };
00166 
00167                                 /** The meat is here: only feasible loop closures from "cur_area" are included here, with associated data.
00168                                   */
00169                                 std::map< CHMHMapNode::TNodeID, TBI_info >      loopClosureData;
00170 
00171                                 //EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00172                         };
00173                         typedef stlplus::smart_ptr<TMessageLSLAMfromTBI>        TMessageLSLAMfromTBIPtr;
00174 
00175 
00176                         utils::CMessageQueue    m_LSLAM_queue;  //!< LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA
00177 
00178                         /** @} */
00179 
00180                         /** The Area Abstraction (AA) method, invoked from LSLAM.
00181                           * \param LMH (IN) The LMH which to this query applies.
00182                           * \param newPoseIDs (IN) The new poseIDs to be added to the graph partitioner.
00183                           * \return A structure with all return data. Memory to be freed by user.
00184                           * \note The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section).
00185                           */
00186                         static TMessageLSLAMfromAAPtr areaAbstraction(
00187                                 CLocalMetricHypothesis  *LMH,
00188                                 const TPoseIDList               &newPoseIDs );
00189 
00190 
00191                         /** The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM.
00192                           * \param LMH (IN) The LMH which to this query applies.
00193                           * \param areaID (IN) The area ID to consider for potential loop-closures.
00194                           * \note The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section).
00195                           */
00196                         static TMessageLSLAMfromTBIPtr TBI_main_method(
00197                                 CLocalMetricHypothesis  *LMH,
00198                                 const CHMHMapNode::TNodeID &areaID
00199                                 );
00200 
00201                         /** @name Related to the input queue:
00202                                 @{ */
00203                 public:
00204                         /** Empty the input queue. */
00205                         void  clearInputQueue();
00206 
00207                         /** Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions)
00208                           * \sa pushAction,pushObservations, inputQueueSize
00209                           */
00210                         bool  isInputQueueEmpty();
00211 
00212                         /** Returns the number of objects waiting for processing in the input queue.
00213                           * \sa pushAction,pushObservations, isInputQueueEmpty
00214                           */
00215                         size_t inputQueueSize();
00216 
00217                         /** Here the user can enter an action into the system (will go to the SLAM process).
00218                           *  This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
00219                           * \sa pushObservations,pushObservation
00220                           */
00221                         void  pushAction( const CActionCollectionPtr &acts );
00222 
00223                         /** Here the user can enter observations into the system (will go to the SLAM process).
00224                           *  This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
00225                           * \sa pushAction,pushObservation
00226                           */
00227                         void  pushObservations( const CSensoryFramePtr &sf );
00228 
00229                         /** Here the user can enter an observation into the system (will go to the SLAM process).
00230                           *  This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.
00231                           * \sa pushAction,pushObservation
00232                           */
00233                         void  pushObservation( const CObservationPtr &obs );
00234 
00235                         enum TLSlamMethod
00236                         {
00237                                 lsmRBPF_2DLASER = 1
00238                         };
00239 
00240                 protected:
00241                         /** Used from the LSLAM thread to retrieve the next object from the queue.
00242                           * \return The object, or NULL if empty.
00243                           */
00244                         CSerializablePtr getNextObjectFromInputQueue();
00245 
00246                         /** The queue of pending actions/observations supplied by the user waiting for being processed. */
00247                         std::queue<CSerializablePtr>            m_inputQueue;
00248 
00249                         /** Critical section for accessing  m_inputQueue */
00250                         synch::CCriticalSection m_inputQueue_cs;
00251 
00252                         /** Critical section for accessing m_map */
00253                         synch::CCriticalSection m_map_cs;
00254 
00255                         synch::CCriticalSection m_LMHs_cs; //!< Critical section for accessing m_LMHs
00256 
00257                         /** @} */
00258 
00259 
00260                         /** @name Threads stuff
00261                                 @{ */
00262 
00263                         /** The function for the "Local SLAM" thread. */
00264                         void thread_LSLAM( );
00265 
00266                         /** The function for the "TBI" thread. */
00267                         void thread_TBI( );
00268 
00269                         /** The function for the "3D viewer" thread. */
00270                         void thread_3D_viewer( );
00271                         /** Threads handles */
00272                         TThreadHandle                   m_hThread_LSLAM,
00273                                                                         m_hThread_TBI,
00274                                                                         m_hThread_3D_viewer;
00275                         /** @} */
00276 
00277 
00278                         /** @name HMT-SLAM sub-processes.
00279                                 @{ */
00280                         void LSLAM_process_message( const CMessage &msg ); //!< Auxiliary method within thread_LSLAM
00281 
00282                         /** No critical section locks are assumed at the entrance of this method.
00283                           */
00284                         void LSLAM_process_message_from_AA( const TMessageLSLAMfromAA &myMsg );
00285 
00286                         /** No critical section locks are assumed at the entrance of this method.
00287                           */
00288                         void LSLAM_process_message_from_TBI( const TMessageLSLAMfromTBI &myMsg );
00289 
00290                         /** Topological Loop Closure: Performs all the required operations
00291                                 to close a loop between two areas which have been determined
00292                                 to be the same.
00293                          */
00294                         void perform_TLC(
00295                                 CLocalMetricHypothesis                                  &LMH,
00296                                 const CHMHMapNode::TNodeID                              areaInLMH,
00297                                 const CHMHMapNode::TNodeID                              areaLoopClosure,
00298                                 const mrpt::poses::CPose3DPDFGaussian   &pose1wrt2
00299                                 );
00300 
00301 
00302                         /** @} */
00303 
00304                         /** @name The different SLAM algorithms that can be invoked from the LSLAM thread.
00305                                 @{ */
00306 
00307                         /** An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" or "loadState".
00308                           */
00309                         CLSLAMAlgorithmBase             *m_LSLAM_method;
00310 
00311                         /** @} */
00312 
00313                         /** @name The different Loop-Closure modules that are to be executed in the TBI thread.
00314                                 @{ */
00315                 protected:
00316 
00317                         typedef CTopLCDetectorBase*             (*TLopLCDetectorFactory)(CHMTSLAM*);
00318 
00319                         std::map<std::string,TLopLCDetectorFactory>   m_registeredLCDetectors;
00320 
00321                         /** The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState".  */
00322                         std::deque<CTopLCDetectorBase*> m_topLCdets;
00323 
00324                         /** The critical section for accessing m_topLCdets */
00325                         synch::CCriticalSection m_topLCdets_cs;
00326                 public:
00327 
00328                         /** Must be invoked before calling  initializeEmptyMap, so LC objects can be created. */
00329                         void registerLoopClosureDetector(
00330                                 const std::string       &name,
00331                                 CTopLCDetectorBase*             (*ptrCreateObject)(CHMTSLAM*)
00332                                 );
00333 
00334                         /** The class factory for topological loop closure detectors.
00335                           *  Possible values are enumerated in TOptions::TLC_detectors
00336                           *
00337                           * \exception std::exception On unknown name.
00338                           */
00339                         CTopLCDetectorBase* loopClosureDetector_factory(const std::string  &name);
00340 
00341 
00342                         /** @} */
00343                 protected:
00344 
00345 
00346                         /** Termination flag for signaling all threads to terminate */
00347                         bool                                    m_terminateThreads;
00348 
00349                         /** Threads termination flags:
00350                           */
00351                         bool                                    m_terminationFlag_LSLAM,
00352                                                                         m_terminationFlag_TBI,
00353                                                                         m_terminationFlag_3D_viewer;
00354 
00355                         /** Generates a new and unique area textual label (currently this generates "0","1",...) */
00356                         static std::string              generateUniqueAreaLabel();
00357 
00358                         /** Generates a new and unique pose ID */
00359                         static TPoseID                  generatePoseID();
00360 
00361                         /** Generates a new and unique hypothesis ID */
00362                         static THypothesisID    generateHypothesisID();
00363 
00364                         static int64_t                  m_nextAreaLabel;
00365                         static TPoseID                  m_nextPoseID;
00366                         static THypothesisID    m_nextHypID;
00367 
00368 
00369                 public:
00370                         /** Default constructor
00371                           *  \param debug_out_stream If debug output messages should be redirected to any other stream apart from std::cout
00372                           */
00373                         CHMTSLAM( );
00374 
00375                         CHMTSLAM(const CHMTSLAM &o) { THROW_EXCEPTION("This object cannot be copied."); }
00376                         const CHMTSLAM& operator =(const CHMTSLAM &o) { THROW_EXCEPTION("This object cannot be copied."); }
00377 
00378                         /** Destructor
00379                           */
00380                         virtual ~CHMTSLAM();
00381 
00382                         /** 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.
00383                           */
00384                         bool abortedDueToErrors();
00385 
00386 
00387                         /** @name High-level map management
00388                                 @{ */
00389 
00390                         /** Loads the options from a config file. */
00391                         void loadOptions( const std::string &configFile );
00392                         /** Loads the options from a config source */
00393                         void loadOptions( const mrpt::utils::CConfigFileBase &cfgSource );
00394 
00395                         /** 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. */
00396                         void  initializeEmptyMap();
00397 
00398                         /** Save the state of the whole HMT-SLAM framework to some binary stream (e.g. a file).
00399                           * \return true if everything goes OK.
00400                           * \sa loadState
00401                           */
00402                         bool saveState( CStream &out ) const;
00403 
00404                         /** Load the state of the whole HMT-SLAM framework from some binary stream (e.g. a file).
00405                           * \return true if everything goes OK.
00406                           * \sa saveState
00407                           */
00408                         bool loadState( CStream &in );
00409                         /** @} */
00410 
00411                         /** @name The important data.
00412                                 @{ */
00413                         CHierarchicalMHMap                                                                      m_map;  //!< The hiearchical, multi-hypothesis graph-based map.
00414                         aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t m_LMHs;        //!< The list of LMHs at each instant.
00415                         /** @} */
00416 
00417                         /** Called from LSLAM thread when log files must be created.
00418                           */
00419                         void generateLogFiles(unsigned int nIteration);
00420 
00421 
00422                         /** Gets a 3D representation of the current state of the whole mapping framework.
00423                           */
00424                         void  getAs3DScene( COpenGLScene        &outScene );
00425 
00426                 protected:
00427                         /** A variety of options and configuration params (private, use loadOptions).
00428                           */
00429                         struct TOptions : public utils::CLoadableOptions
00430                         {
00431                                 /** Initialization of default params
00432                                   */
00433                                 TOptions();
00434 
00435                                 /** Load parameters from configuration source
00436                                   */
00437                                 void  loadFromConfigFile(
00438                                         const mrpt::utils::CConfigFileBase      &source,
00439                                         const std::string               &section);
00440 
00441                                 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
00442                                   */
00443                                 void  dumpToTextStream(CStream  &out) const;
00444 
00445 
00446                                 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.
00447                                 int                     LOG_FREQUENCY;          //!< [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
00448 
00449                                 /** [LSLAM] The method to use for local SLAM
00450                                   */
00451                                 TLSlamMethod   SLAM_METHOD;
00452 
00453                                 /** [LSLAM] Minimum distance (and heading) difference between observations inserted in the map.
00454                                 */
00455                                 float           SLAM_MIN_DIST_BETWEEN_OBS, SLAM_MIN_HEADING_BETWEEN_OBS;
00456 
00457                                 /** [LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0) */
00458                                 float           MIN_ODOMETRY_STD_XY;
00459 
00460                                 /** [LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry increments (Default=0) */
00461                                 float           MIN_ODOMETRY_STD_PHI;
00462 
00463                                 /** [VIEW3D] The height of the areas' spheres.
00464                                   */
00465                                 float           VIEW3D_AREA_SPHERES_HEIGHT;
00466 
00467                                 /** [VIEW3D] The radius of the areas' spheres.
00468                                   */
00469                                 float           VIEW3D_AREA_SPHERES_RADIUS;
00470 
00471                                 /** 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!)
00472                                   */
00473                                 vector_float  stds_Q_no_odo;
00474 
00475                                 /** [AA] The options for the partitioning algorithm
00476                                 */
00477                                 CIncrementalMapPartitioner::TOptions    AA_options;
00478 
00479                                 TSetOfMetricMapInitializers             defaultMapsInitializers;  //!< The default set of maps to be created in each particle
00480 
00481                                 CMultiMetricMap::TOptions               defaultMapsOptions;             //!< The default options for the CMultiMetricMap in each particle.
00482 
00483                                 bayes::CParticleFilter::TParticleFilterOptions pf_options;      //!< These params are used from every LMH object.
00484 
00485                                 TKLDParams                                              KLD_params;
00486 
00487                                 int  random_seed;       //!< 0 means randomize, use any other value to have repetitive experiments.
00488 
00489                                 /** A list of topological loop-closure detectors to use: can be one or more from this list:
00490                                   *  'gridmaps': Occupancy Grid matching.
00491                                   *  'fabmap': Mark Cummins' image matching framework.
00492                                   */
00493                                 vector_string                           TLC_detectors;
00494 
00495                                 CTopLCDetector_GridMatching::TOptions   TLC_grid_options;       //!< Options passed to this TLC constructor
00496                                 CTopLCDetector_FabMap::TOptions TLC_fabmap_options;     //!< Options passed to this TLC constructor
00497 
00498                         } m_options;
00499 
00500                 }; // End of class CHMTSLAM.
00501 
00502 
00503                 /** Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
00504                   */
00505                 class HMTSLAM_IMPEXP CLSLAMAlgorithmBase
00506                 {
00507                         friend class HMTSLAM_IMPEXP CLocalMetricHypothesis;
00508                 protected:
00509                         safe_ptr<CHMTSLAM>      m_parent;
00510 
00511                 public:
00512                         /** Constructor
00513                           */
00514                         CLSLAMAlgorithmBase( CHMTSLAM *parent ) : m_parent(parent) { }
00515 
00516                         /** Destructor
00517                           */
00518                         virtual ~CLSLAMAlgorithmBase() {}
00519 
00520                         /** Main entry point from HMT-SLAM: process some actions & observations.
00521                           *  The passed action/observation will be deleted, so a copy must be made if necessary.
00522                           *  This method must be in charge of updating the robot pose estimates and also to update the
00523                           *   map when required.
00524                           *
00525                           * \param LMH   The local metric hypothesis which must be updated by this SLAM algorithm.
00526                           * \param act   The action to process (or NULL).
00527                           * \param sf    The observations to process (or NULL).
00528                           */
00529                         virtual void processOneLMH(
00530                                 CLocalMetricHypothesis  *LMH,
00531                                 const CActionCollectionPtr      &act,
00532                                 const CSensoryFramePtr          &sf ) = 0;
00533 
00534 
00535                         /** The PF algorithm implementation.
00536                           */
00537                         virtual void  prediction_and_update_pfAuxiliaryPFOptimal(
00538                                 CLocalMetricHypothesis                  * LMH,
00539                                 const mrpt::slam::CActionCollection     * action,
00540                                 const mrpt::slam::CSensoryFrame         * observation,
00541                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) = 0;
00542 
00543                         /** The PF algorithm implementation.  */
00544                         virtual void  prediction_and_update_pfOptimalProposal(
00545                                 CLocalMetricHypothesis                  *LMH,
00546                                 const mrpt::slam::CActionCollection     * action,
00547                                 const mrpt::slam::CSensoryFrame         * observation,
00548                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) = 0;
00549 
00550                 }; // end of class CLSLAMAlgorithmBase
00551 
00552 
00553                 /** Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
00554                   *  This class is used internally in mrpt::slam::CHMTSLAM
00555                   */
00556                 class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER : public CLSLAMAlgorithmBase
00557                 {
00558                         friend class HMTSLAM_IMPEXP CLocalMetricHypothesis;
00559 
00560                 public:
00561                         /** Constructor
00562                           */
00563                         CLSLAM_RBPF_2DLASER( CHMTSLAM *parent );
00564 
00565                         /** Destructor
00566                           */
00567                         virtual ~CLSLAM_RBPF_2DLASER();
00568 
00569                         /** Main entry point from HMT-SLAM: process some actions & observations.
00570                           *  The passed action/observation will be deleted, so a copy must be made if necessary.
00571                           *  This method must be in charge of updating the robot pose estimates and also to update the
00572                           *   map when required.
00573                           *
00574                           * \param LMH   The local metric hypothesis which must be updated by this SLAM algorithm.
00575                           * \param act   The action to process (or NULL).
00576                           * \param sf    The observations to process (or NULL).
00577                           */
00578                         void processOneLMH(
00579                                 CLocalMetricHypothesis  *LMH,
00580                                 const CActionCollectionPtr      &act,
00581                                 const CSensoryFramePtr          &sf );
00582 
00583                         /** The PF algorithm implementation.  */
00584                         void  prediction_and_update_pfAuxiliaryPFOptimal(
00585                                 CLocalMetricHypothesis                  *LMH,
00586                                 const mrpt::slam::CActionCollection     * action,
00587                                 const mrpt::slam::CSensoryFrame         * observation,
00588                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00589 
00590                         /** The PF algorithm implementation.  */
00591                         void  prediction_and_update_pfOptimalProposal(
00592                                 CLocalMetricHypothesis                  *LMH,
00593                                 const mrpt::slam::CActionCollection     * action,
00594                                 const mrpt::slam::CSensoryFrame         * observation,
00595                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00596 
00597                 protected:
00598                         bool                                    m_insertNewRobotPose; //!<  For use within PF callback methods
00599 
00600                         /** Auxiliary structure
00601                           */
00602                         struct TPathBin
00603                         {
00604                                 TPathBin() : x(),y(),phi()
00605                                 {}
00606 
00607                                 vector_int              x,y,phi;
00608 
00609                                 /** For debugging purposes!
00610                                   */
00611                                 void  dumpToStdOut() const;
00612                         };
00613 
00614 
00615                         /** 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".
00616                                 */
00617                         void  loadTPathBinFromPath(
00618                                 TPathBin        &outBin,
00619                                 std::map<TPoseID,CPose3D>               *path = NULL,
00620                                 CPose2D                                                 *newPose = NULL );
00621 
00622                         /** 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.
00623                                 */
00624                         int  findTPathBinIntoSet(
00625                                 TPathBin                                                &desiredBin,
00626                                 std::deque<TPathBin>                    &theSet
00627                                 );
00628 
00629                         /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
00630                                 */
00631                         static double  particlesEvaluator_AuxPFOptimal(
00632                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00633                                 const CParticleFilterCapable    *obj,
00634                                 size_t                                  index,
00635                                 const void                              *action,
00636                                 const void                              *observation );
00637 
00638                         /** Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".
00639                           */
00640                         static double  auxiliarComputeObservationLikelihood(
00641                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00642                                 const CParticleFilterCapable            *obj,
00643                                 size_t                                          particleIndexForMap,
00644                                 const CSensoryFrame                     *observation,
00645                                 const CPose2D                           *x );
00646 
00647                 }; // end class CLSLAM_RBPF_2DLASER
00648 
00649         } // End of namespace
00650 } // End of namespace
00651 
00652 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011