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 CLocalMetricHypothesis_H 00029 #define CLocalMetricHypothesis_H 00030 00031 #include <mrpt/synch.h> 00032 #include <mrpt/bayes/CParticleFilterCapable.h> 00033 00034 #include <mrpt/hmtslam/HMT_SLAM_common.h> 00035 #include <mrpt/hmtslam/CHMHMapNode.h> 00036 00037 #include <mrpt/slam/CMultiMetricMap.h> 00038 #include <mrpt/slam/CActionRobotMovement2D.h> 00039 #include <mrpt/slam/CIncrementalMapPartitioner.h> 00040 00041 #include <list> 00042 00043 namespace mrpt 00044 { 00045 namespace opengl 00046 { 00047 struct CSetOfObjectsPtr; 00048 } 00049 namespace poses 00050 { 00051 class CPose3DPDFParticles; 00052 } 00053 00054 namespace hmtslam 00055 { 00056 using namespace mrpt::slam; 00057 00058 class HMTSLAM_IMPEXP CHMTSLAM; 00059 class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER; 00060 00061 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CLSLAMParticleData, mrpt::utils::CSerializable, HMTSLAM_IMPEXP ) 00062 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CLocalMetricHypothesis, mrpt::utils::CSerializable, HMTSLAM_IMPEXP ) 00063 00064 /** Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data relative to each local metric particle ("a robot metric path hypothesis" and its associated metric map). 00065 */ 00066 class HMTSLAM_IMPEXP CLSLAMParticleData : public mrpt::utils::CSerializable 00067 { 00068 // This must be added to any CSerializable derived class: 00069 DEFINE_SERIALIZABLE( CLSLAMParticleData ) 00070 00071 public: 00072 CLSLAMParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) : 00073 metricMaps( mapsInitializers ), 00074 robotPoses() 00075 { 00076 } 00077 00078 virtual ~CLSLAMParticleData() 00079 { 00080 robotPoses.clear(); 00081 } 00082 00083 CMultiMetricMap metricMaps; 00084 std::map<TPoseID,CPose3D> robotPoses; 00085 }; 00086 00087 00088 /** This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs). 00089 * It has a set of particles representing the robot path in nearby poses. 00090 * \sa CHMTSLAM, CLSLAM_RBPF_2DLASER 00091 */ 00092 class HMTSLAM_IMPEXP CLocalMetricHypothesis : 00093 public bayes::CParticleFilterCapable, public bayes::CParticleFilterData<CLSLAMParticleData>, 00094 public mrpt::utils::CSerializable 00095 { 00096 friend class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER; 00097 00098 // This must be added to any CSerializable derived class: 00099 DEFINE_SERIALIZABLE( CLocalMetricHypothesis ) 00100 00101 public: 00102 /** Constructor (Default param only used from STL classes) 00103 */ 00104 CLocalMetricHypothesis( CHMTSLAM * parent = NULL ); 00105 00106 /** Destructor 00107 */ 00108 ~CLocalMetricHypothesis(); 00109 00110 synch::CCriticalSection m_lock; //!< Critical section for threads signaling they are working with the LMH. 00111 THypothesisID m_ID; //!< The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap). 00112 safe_ptr<CHMTSLAM> m_parent; //!< For quick access to our parent object. 00113 TPoseID m_currentRobotPose; //!< The current robot pose (its global unique ID) for this hypothesis. 00114 //TNodeIDList m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself). 00115 TNodeIDSet m_neighbors; //!< The list of all areas sourronding the current one (this includes the current area itself). 00116 std::map<TPoseID,CHMHMapNode::TNodeID> m_nodeIDmemberships; //!< The hybrid map node membership for each robot pose. 00117 std::map<TPoseID,CSensoryFrame> m_SFs; //!< The SF gathered at each robot pose. 00118 TPoseIDList m_posesPendingAddPartitioner; //!< The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop. 00119 TNodeIDList m_areasPendingTBI; //!< The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to search for potential loop-closures. Set in CHMTSLAM::LSLAM_process_message_from_AA, read in 00120 00121 double m_log_w; //!< Log-weight of this hypothesis. 00122 std::vector<std::map<TPoseID,double> > m_log_w_metric_history; //!< The historic log-weights of the metric observations inserted in this LMH, for each particle. 00123 //std::map<TPoseID,double> m_log_w_topol_history; //!< The historic log-weights of the topological observations inserted in this LMH. 00124 00125 CActionRobotMovement2D m_accumRobotMovement; //!< Used in CLSLAM_RBPF_2DLASER 00126 bool m_accumRobotMovementIsValid; //!< Used in CLSLAM_RBPF_2DLASER 00127 00128 /** Used by AA thread */ 00129 struct TRobotPosesPartitioning 00130 { 00131 synch::CCriticalSection lock; //!< CS to access the entire struct. 00132 CIncrementalMapPartitioner partitioner; 00133 std::map<uint32_t,TPoseID> idx2pose; //!< For the poses in "partitioner". 00134 00135 unsigned int pose2idx(const TPoseID &id) const; //!< Uses idx2pose to perform inverse searches. 00136 00137 } m_robotPosesGraph; 00138 00139 /** Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph, and each of the areas they belong to. 00140 * The metric maps are *not* included here for convenience, call m_metricMaps.getAs3DScene(). 00141 * The previous contents of "objs" will be discarded 00142 */ 00143 void getAs3DScene( mrpt::opengl::CSetOfObjectsPtr &objs ) const; 00144 00145 /** Returns the mean of each robot pose in this LMH, as computed from the set of particles. 00146 * \sa getPathParticles, getRelativePose 00147 */ 00148 void getMeans( std::map< TPoseID, CPose3D > &outList ) const; 00149 00150 /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles. 00151 * \sa getMeans, getPoseParticles 00152 */ 00153 void getPathParticles( std::map< TPoseID, CPose3DPDFParticles > &outList ) const; 00154 00155 /** Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles. 00156 * \sa getMeans, getPathParticles 00157 */ 00158 void getPoseParticles( const TPoseID &poseID, CPose3DPDFParticles &outPDF ) const; 00159 00160 /** Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH). 00161 * \sa getMeans, getPoseParticles 00162 */ 00163 void getRelativePose( 00164 const TPoseID &reference, 00165 const TPoseID &pose, 00166 CPose3DPDFParticles &outPDF ) const; 00167 00168 /** Describes the LMH in text. 00169 */ 00170 void dumpAsText(utils::CStringList &st) const; 00171 00172 /** Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric maps and change coords in the partitioning subsystem as well. 00173 */ 00174 void changeCoordinateOrigin( const TPoseID &newOrigin ); 00175 00176 /** Rebuild the metric maps of all particles from the observations and their estimated poses. */ 00177 void rebuildMetricMaps(); 00178 00179 /** Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses. */ 00180 //void rebuildSSOMatrix(); 00181 00182 /** Sets the number of particles to the initial number according to the PF options, and initialize them with no robot poses & empty metric maps. 00183 */ 00184 void clearRobotPoses(); 00185 00186 /** Returns the i'th particle hypothesis for the current robot pose. */ 00187 const CPose3D * getCurrentPose(const size_t &particleIdx) const; 00188 00189 /** Returns the i'th particle hypothesis for the current robot pose. */ 00190 CPose3D * getCurrentPose(const size_t &particleIdx); 00191 00192 /** Removes a given area from the LMH: 00193 * - The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH. 00194 * - Robot poses belonging to that area are removed from: 00195 * - the particles. 00196 * - the graph partitioner. 00197 * - the list of SFs. 00198 * - the list m_nodeIDmemberships. 00199 * - m_neighbors is updated. 00200 * - The weights of all particles are changed to remove the effects of the removed metric observations. 00201 * - After calling this the metric maps should be updated. 00202 * - This method internally calls updateAreaFromLMH 00203 */ 00204 void removeAreaFromLMH( const CHMHMapNode::TNodeID areaID ); 00205 00206 /** The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are referenced to the area's reference poseID, such as that reference is at the origin. 00207 * If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied to the area, and removed from the LMH. 00208 * \note The critical section m_map_cs is locked internally, unlock it before calling this. 00209 */ 00210 void updateAreaFromLMH( 00211 const CHMHMapNode::TNodeID areaID, 00212 bool eraseSFsFromLMH = false ); 00213 00214 00215 protected: 00216 00217 /** @name Virtual methods for Particle Filter implementation (just a wrapper interface, actually implemented in CHMTSLAM::m_LSLAM_method) 00218 @{ 00219 */ 00220 00221 /** The PF algorithm implementation. 00222 */ 00223 void prediction_and_update_pfAuxiliaryPFOptimal( 00224 const mrpt::slam::CActionCollection * action, 00225 const mrpt::slam::CSensoryFrame * observation, 00226 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00227 00228 /** The PF algorithm implementation. */ 00229 void prediction_and_update_pfOptimalProposal( 00230 const mrpt::slam::CActionCollection * action, 00231 const mrpt::slam::CSensoryFrame * observation, 00232 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00233 /** @} 00234 */ 00235 00236 00237 /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. 00238 */ 00239 mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb; 00240 00241 /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. 00242 */ 00243 mutable std::vector<double> m_maxLikelihood; 00244 00245 /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. 00246 */ 00247 mutable std::vector<CPose2D,Eigen::aligned_allocator<CPose2D> > m_movementDraws; 00248 00249 /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. 00250 */ 00251 mutable unsigned int m_movementDrawsIdx; 00252 00253 /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. 00254 */ 00255 mutable StdVector_CPose2D m_movementDrawMaximumLikelihood; 00256 00257 00258 /** The following implements: 00259 * - CParticleFilterCapable::getW 00260 * - CParticleFilterCapable::setW 00261 * - CParticleFilterCapable::particlesCount 00262 * - CParticleFilterCapable::normalizeWeights 00263 * - CParticleFilterCapable::ESS 00264 * - CParticleFilterCapable::performSubstitution 00265 */ 00266 IMPLEMENT_PARTICLE_FILTER_CAPABLE(CLSLAMParticleData) 00267 00268 }; // End of class def. 00269 00270 } // End of namespace 00271 } // End of namespace 00272 00273 #endif
Page generated by Doxygen 1.7.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |