00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 typedef stlplus::smart_ptr<TMessageLSLAMfromTBI> TMessageLSLAMfromTBIPtr; 00172 00173 00174 utils::CMessageQueue m_LSLAM_queue; //!< LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA 00175 00176 /** @} */ 00177 00178 /** The Area Abstraction (AA) method, invoked from LSLAM. 00179 * \param LMH (IN) The LMH which to this query applies. 00180 * \param newPoseIDs (IN) The new poseIDs to be added to the graph partitioner. 00181 * \return A structure with all return data. Memory to be freed by user. 00182 * \note The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section). 00183 */ 00184 static TMessageLSLAMfromAAPtr areaAbstraction( 00185 CLocalMetricHypothesis *LMH, 00186 const TPoseIDList &newPoseIDs ); 00187 00188 00189 /** The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM. 00190 * \param LMH (IN) The LMH which to this query applies. 00191 * \param areaID (IN) The area ID to consider for potential loop-closures. 00192 * \note The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section). 00193 */ 00194 static TMessageLSLAMfromTBIPtr TBI_main_method( 00195 CLocalMetricHypothesis *LMH, 00196 const CHMHMapNode::TNodeID &areaID 00197 ); 00198 00199 /** @name Related to the input queue: 00200 @{ */ 00201 public: 00202 /** Empty the input queue. */ 00203 void clearInputQueue(); 00204 00205 /** Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions) 00206 * \sa pushAction,pushObservations, inputQueueSize 00207 */ 00208 bool isInputQueueEmpty(); 00209 00210 /** Returns the number of objects waiting for processing in the input queue. 00211 * \sa pushAction,pushObservations, isInputQueueEmpty 00212 */ 00213 size_t inputQueueSize(); 00214 00215 /** Here the user can enter an action into the system (will go to the SLAM process). 00216 * This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this. 00217 * \sa pushObservations,pushObservation 00218 */ 00219 void pushAction( const CActionCollectionPtr &acts ); 00220 00221 /** Here the user can enter observations into the system (will go to the SLAM process). 00222 * This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this. 00223 * \sa pushAction,pushObservation 00224 */ 00225 void pushObservations( const CSensoryFramePtr &sf ); 00226 00227 /** Here the user can enter an observation into the system (will go to the SLAM process). 00228 * This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this. 00229 * \sa pushAction,pushObservation 00230 */ 00231 void pushObservation( const CObservationPtr &obs ); 00232 00233 enum TLSlamMethod 00234 { 00235 lsmRBPF_2DLASER = 1 00236 }; 00237 00238 protected: 00239 /** Used from the LSLAM thread to retrieve the next object from the queue. 00240 * \return The object, or NULL if empty. 00241 */ 00242 CSerializablePtr getNextObjectFromInputQueue(); 00243 00244 /** The queue of pending actions/observations supplied by the user waiting for being processed. */ 00245 std::queue<CSerializablePtr> m_inputQueue; 00246 00247 /** Critical section for accessing m_inputQueue */ 00248 synch::CCriticalSection m_inputQueue_cs; 00249 00250 /** Critical section for accessing m_map */ 00251 synch::CCriticalSection m_map_cs; 00252 00253 synch::CCriticalSection m_LMHs_cs; //!< Critical section for accessing m_LMHs 00254 00255 /** @} */ 00256 00257 00258 /** @name Threads stuff 00259 @{ */ 00260 00261 /** The function for the "Local SLAM" thread. */ 00262 void thread_LSLAM( ); 00263 00264 /** The function for the "TBI" thread. */ 00265 void thread_TBI( ); 00266 00267 /** The function for the "3D viewer" thread. */ 00268 void thread_3D_viewer( ); 00269 /** Threads handles */ 00270 TThreadHandle m_hThread_LSLAM, 00271 m_hThread_TBI, 00272 m_hThread_3D_viewer; 00273 /** @} */ 00274 00275 00276 /** @name HMT-SLAM sub-processes. 00277 @{ */ 00278 void LSLAM_process_message( const CMessage &msg ); //!< Auxiliary method within thread_LSLAM 00279 00280 /** No critical section locks are assumed at the entrance of this method. 00281 */ 00282 void LSLAM_process_message_from_AA( const TMessageLSLAMfromAA &myMsg ); 00283 00284 /** No critical section locks are assumed at the entrance of this method. 00285 */ 00286 void LSLAM_process_message_from_TBI( const TMessageLSLAMfromTBI &myMsg ); 00287 00288 /** Topological Loop Closure: Performs all the required operations 00289 to close a loop between two areas which have been determined 00290 to be the same. 00291 */ 00292 void perform_TLC( 00293 CLocalMetricHypothesis &LMH, 00294 const CHMHMapNode::TNodeID areaInLMH, 00295 const CHMHMapNode::TNodeID areaLoopClosure, 00296 const mrpt::poses::CPose3DPDFGaussian &pose1wrt2 00297 ); 00298 00299 00300 /** @} */ 00301 00302 /** @name The different SLAM algorithms that can be invoked from the LSLAM thread. 00303 @{ */ 00304 00305 /** An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" or "loadState". 00306 */ 00307 CLSLAMAlgorithmBase *m_LSLAM_method; 00308 00309 /** @} */ 00310 00311 /** @name The different Loop-Closure modules that are to be executed in the TBI thread. 00312 @{ */ 00313 protected: 00314 00315 typedef CTopLCDetectorBase* (*TLopLCDetectorFactory)(CHMTSLAM*); 00316 00317 std::map<std::string,TLopLCDetectorFactory> m_registeredLCDetectors; 00318 00319 /** The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState". */ 00320 std::deque<CTopLCDetectorBase*> m_topLCdets; 00321 00322 /** The critical section for accessing m_topLCdets */ 00323 synch::CCriticalSection m_topLCdets_cs; 00324 public: 00325 00326 /** Must be invoked before calling initializeEmptyMap, so LC objects can be created. */ 00327 void registerLoopClosureDetector( 00328 const std::string &name, 00329 CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*) 00330 ); 00331 00332 /** The class factory for topological loop closure detectors. 00333 * Possible values are enumerated in TOptions::TLC_detectors 00334 * 00335 * \exception std::exception On unknown name. 00336 */ 00337 CTopLCDetectorBase* loopClosureDetector_factory(const std::string &name); 00338 00339 00340 /** @} */ 00341 protected: 00342 00343 00344 /** Termination flag for signaling all threads to terminate */ 00345 bool m_terminateThreads; 00346 00347 /** Threads termination flags: 00348 */ 00349 bool m_terminationFlag_LSLAM, 00350 m_terminationFlag_TBI, 00351 m_terminationFlag_3D_viewer; 00352 00353 /** Generates a new and unique area textual label (currently this generates "0","1",...) */ 00354 static std::string generateUniqueAreaLabel(); 00355 00356 /** Generates a new and unique pose ID */ 00357 static TPoseID generatePoseID(); 00358 00359 /** Generates a new and unique hypothesis ID */ 00360 static THypothesisID generateHypothesisID(); 00361 00362 static int64_t m_nextAreaLabel; 00363 static TPoseID m_nextPoseID; 00364 static THypothesisID m_nextHypID; 00365 00366 00367 public: 00368 /** Default constructor 00369 * \param debug_out_stream If debug output messages should be redirected to any other stream apart from std::cout 00370 */ 00371 CHMTSLAM( ); 00372 00373 CHMTSLAM(const CHMTSLAM &o) { THROW_EXCEPTION("This object cannot be copied."); } 00374 const CHMTSLAM& operator =(const CHMTSLAM &o) { THROW_EXCEPTION("This object cannot be copied."); } 00375 00376 /** Destructor 00377 */ 00378 virtual ~CHMTSLAM(); 00379 00380 /** 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. 00381 */ 00382 bool abortedDueToErrors(); 00383 00384 00385 /** @name High-level map management 00386 @{ */ 00387 00388 /** Loads the options from a config file. */ 00389 void loadOptions( const std::string &configFile ); 00390 /** Loads the options from a config source */ 00391 void loadOptions( const mrpt::utils::CConfigFileBase &cfgSource ); 00392 00393 /** 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. */ 00394 void initializeEmptyMap(); 00395 00396 /** Save the state of the whole HMT-SLAM framework to some binary stream (e.g. a file). 00397 * \return true if everything goes OK. 00398 * \sa loadState 00399 */ 00400 bool saveState( CStream &out ) const; 00401 00402 /** Load the state of the whole HMT-SLAM framework from some binary stream (e.g. a file). 00403 * \return true if everything goes OK. 00404 * \sa saveState 00405 */ 00406 bool loadState( CStream &in ); 00407 /** @} */ 00408 00409 /** @name The important data. 00410 @{ */ 00411 CHierarchicalMHMap m_map; //!< The hiearchical, multi-hypothesis graph-based map. 00412 std::map< THypothesisID, CLocalMetricHypothesis > m_LMHs; //!< The list of LMHs at each instant. 00413 /** @} */ 00414 00415 /** Called from LSLAM thread when log files must be created. 00416 */ 00417 void generateLogFiles(unsigned int nIteration); 00418 00419 00420 /** Gets a 3D representation of the current state of the whole mapping framework. 00421 */ 00422 void getAs3DScene( COpenGLScene &outScene ); 00423 00424 protected: 00425 /** A variety of options and configuration params (private, use loadOptions). 00426 */ 00427 struct TOptions : public utils::CLoadableOptions 00428 { 00429 /** Initialization of default params 00430 */ 00431 TOptions(); 00432 00433 /** Load parameters from configuration source 00434 */ 00435 void loadFromConfigFile( 00436 const mrpt::utils::CConfigFileBase &source, 00437 const std::string §ion); 00438 00439 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00440 */ 00441 void dumpToTextStream(CStream &out) const; 00442 00443 00444 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. 00445 int LOG_FREQUENCY; //!< [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated. 00446 bool LOG_SHOW3D; //!< [LOGGING] Display 3D view in real-time 00447 00448 /** [LSLAM] The method to use for local SLAM 00449 */ 00450 TLSlamMethod SLAM_METHOD; 00451 00452 /** [LSLAM] Minimum distance (and heading) difference between observations inserted in the map. 00453 */ 00454 float SLAM_MIN_DIST_BETWEEN_OBS, SLAM_MIN_HEADING_BETWEEN_OBS; 00455 00456 /** [LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0) */ 00457 float MIN_ODOMETRY_STD_XY; 00458 00459 /** [LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry increments (Default=0) */ 00460 float MIN_ODOMETRY_STD_PHI; 00461 00462 /** [VIEW3D] The height of the areas' spheres. 00463 */ 00464 float VIEW3D_AREA_SPHERES_HEIGHT; 00465 00466 /** [VIEW3D] The radius of the areas' spheres. 00467 */ 00468 float VIEW3D_AREA_SPHERES_RADIUS; 00469 00470 /** 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!) 00471 */ 00472 vector_float stds_Q_no_odo; 00473 00474 /** [AA] The options for the partitioning algorithm 00475 */ 00476 CIncrementalMapPartitioner::TOptions AA_options; 00477 00478 TSetOfMetricMapInitializers defaultMapsInitializers; //!< The default set of maps to be created in each particle 00479 00480 CMultiMetricMap::TOptions defaultMapsOptions; //!< The default options for the CMultiMetricMap in each particle. 00481 00482 bayes::CParticleFilter::TParticleFilterOptions pf_options; //!< These params are used from every LMH object. 00483 00484 TKLDParams KLD_params; 00485 00486 int random_seed; //!< 0 means randomize, use any other value to have repetitive experiments. 00487 00488 /** A list of topological loop-closure detectors to use: can be one or more from this list: 00489 * 'gridmaps': Occupancy Grid matching. 00490 * 'fabmap': Mark Cummins' image matching framework. 00491 */ 00492 vector_string TLC_detectors; 00493 00494 CTopLCDetector_GridMatching::TOptions TLC_grid_options; //!< Options passed to this TLC constructor 00495 CTopLCDetector_FabMap::TOptions TLC_fabmap_options; //!< Options passed to this TLC constructor 00496 00497 } m_options; 00498 00499 }; // End of class CHMTSLAM. 00500 00501 00502 /** Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM. 00503 */ 00504 class HMTSLAM_IMPEXP CLSLAMAlgorithmBase 00505 { 00506 friend class HMTSLAM_IMPEXP CLocalMetricHypothesis; 00507 protected: 00508 safe_ptr<CHMTSLAM> m_parent; 00509 00510 public: 00511 /** Constructor 00512 */ 00513 CLSLAMAlgorithmBase( CHMTSLAM *parent ) : m_parent(parent) { } 00514 00515 /** Destructor 00516 */ 00517 virtual ~CLSLAMAlgorithmBase() {} 00518 00519 /** Main entry point from HMT-SLAM: process some actions & observations. 00520 * The passed action/observation will be deleted, so a copy must be made if necessary. 00521 * This method must be in charge of updating the robot pose estimates and also to update the 00522 * map when required. 00523 * 00524 * \param LMH The local metric hypothesis which must be updated by this SLAM algorithm. 00525 * \param act The action to process (or NULL). 00526 * \param sf The observations to process (or NULL). 00527 */ 00528 virtual void processOneLMH( 00529 CLocalMetricHypothesis *LMH, 00530 const CActionCollectionPtr &act, 00531 const CSensoryFramePtr &sf ) = 0; 00532 00533 00534 /** The PF algorithm implementation. 00535 */ 00536 virtual void prediction_and_update_pfAuxiliaryPFOptimal( 00537 CLocalMetricHypothesis * LMH, 00538 const mrpt::slam::CActionCollection * action, 00539 const mrpt::slam::CSensoryFrame * observation, 00540 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) = 0; 00541 00542 /** The PF algorithm implementation. */ 00543 virtual void prediction_and_update_pfOptimalProposal( 00544 CLocalMetricHypothesis *LMH, 00545 const mrpt::slam::CActionCollection * action, 00546 const mrpt::slam::CSensoryFrame * observation, 00547 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) = 0; 00548 00549 }; // end of class CLSLAMAlgorithmBase 00550 00551 00552 /** Implements a 2D local SLAM method based on a RBPF over an occupancy grid map. 00553 * This class is used internally in mrpt::slam::CHMTSLAM 00554 */ 00555 class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER : public CLSLAMAlgorithmBase 00556 { 00557 friend class HMTSLAM_IMPEXP CLocalMetricHypothesis; 00558 00559 public: 00560 /** Constructor 00561 */ 00562 CLSLAM_RBPF_2DLASER( CHMTSLAM *parent ); 00563 00564 /** Destructor 00565 */ 00566 virtual ~CLSLAM_RBPF_2DLASER(); 00567 00568 /** Main entry point from HMT-SLAM: process some actions & observations. 00569 * The passed action/observation will be deleted, so a copy must be made if necessary. 00570 * This method must be in charge of updating the robot pose estimates and also to update the 00571 * map when required. 00572 * 00573 * \param LMH The local metric hypothesis which must be updated by this SLAM algorithm. 00574 * \param act The action to process (or NULL). 00575 * \param sf The observations to process (or NULL). 00576 */ 00577 void processOneLMH( 00578 CLocalMetricHypothesis *LMH, 00579 const CActionCollectionPtr &act, 00580 const CSensoryFramePtr &sf ); 00581 00582 /** The PF algorithm implementation. */ 00583 void prediction_and_update_pfAuxiliaryPFOptimal( 00584 CLocalMetricHypothesis *LMH, 00585 const mrpt::slam::CActionCollection * action, 00586 const mrpt::slam::CSensoryFrame * observation, 00587 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00588 00589 /** The PF algorithm implementation. */ 00590 void prediction_and_update_pfOptimalProposal( 00591 CLocalMetricHypothesis *LMH, 00592 const mrpt::slam::CActionCollection * action, 00593 const mrpt::slam::CSensoryFrame * observation, 00594 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00595 00596 protected: 00597 bool m_insertNewRobotPose; //!< For use within PF callback methods 00598 00599 /** Auxiliary structure 00600 */ 00601 struct TPathBin 00602 { 00603 TPathBin() : x(),y(),phi() 00604 {} 00605 00606 vector_int x,y,phi; 00607 00608 /** For debugging purposes! 00609 */ 00610 void dumpToStdOut() const; 00611 }; 00612 00613 00614 /** 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". 00615 */ 00616 void loadTPathBinFromPath( 00617 TPathBin &outBin, 00618 std::map<TPoseID,CPose3D> *path = NULL, 00619 CPose2D *newPose = NULL ); 00620 00621 /** 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. 00622 */ 00623 int findTPathBinIntoSet( 00624 TPathBin &desiredBin, 00625 std::deque<TPathBin> &theSet 00626 ); 00627 00628 /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal" 00629 */ 00630 static double particlesEvaluator_AuxPFOptimal( 00631 const bayes::CParticleFilter::TParticleFilterOptions &PF_options, 00632 const CParticleFilterCapable *obj, 00633 size_t index, 00634 const void *action, 00635 const void *observation ); 00636 00637 /** Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options". 00638 */ 00639 static double auxiliarComputeObservationLikelihood( 00640 const bayes::CParticleFilter::TParticleFilterOptions &PF_options, 00641 const CParticleFilterCapable *obj, 00642 size_t particleIndexForMap, 00643 const CSensoryFrame *observation, 00644 const CPose2D *x ); 00645 00646 }; // end class CLSLAM_RBPF_2DLASER 00647 00648 } // End of namespace 00649 } // End of namespace 00650 00651 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
