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