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 §ion); 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:40:17 UTC 2011 |