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