Main MRPT website > C++ reference
MRPT logo

CLocalMetricHypothesis.h

Go to the documentation of this file.
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.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011