Main MRPT website > C++ reference
MRPT logo

CMultiMetricMapPDF.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 CMultiMetricMapPDF_H
00029 #define CMultiMetricMapPDF_H
00030 
00031 #include <mrpt/slam/CMultiMetricMap.h>
00032 #include <mrpt/slam/CSimpleMap.h>
00033 #include <mrpt/poses/CPosePDFParticles.h>
00034 #include <mrpt/poses/CPose3DPDFParticles.h>
00035 
00036 #include <mrpt/poses/CPoseRandomSampler.h>
00037 
00038 #include <mrpt/bayes/CParticleFilterCapable.h>
00039 #include <mrpt/utils/CLoadableOptions.h>
00040 #include <mrpt/slam/CICP.h>
00041 
00042 #include <mrpt/slam/PF_implementations_data.h>
00043 
00044 #include <mrpt/slam/link_pragmas.h>
00045 
00046 namespace mrpt
00047 {
00048 namespace slam
00049 {
00050         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CRBPFParticleData, mrpt::utils::CSerializable, SLAM_IMPEXP )
00051 
00052         /** Auxiliary class used in mrpt::slam::CMultiMetricMapPDF
00053           */
00054         class SLAM_IMPEXP CRBPFParticleData : public mrpt::utils::CSerializable
00055         {
00056                 // This must be added to any CSerializable derived class:
00057                 DEFINE_SERIALIZABLE( CRBPFParticleData )
00058         public:
00059                 CRBPFParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
00060                   mapTillNow( mapsInitializers ),
00061                   robotPath()
00062                 {
00063                 }
00064 
00065                 CMultiMetricMap                 mapTillNow;
00066                 std::deque<TPose3D>             robotPath;
00067         };
00068 
00069 
00070         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CMultiMetricMapPDF, mrpt::utils::CSerializable, SLAM_IMPEXP )
00071 
00072         /** Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (This class is the base of RBPF-SLAM applications).
00073          *   This class is used internally by the map building algorithm in "mrpt::slam::CMetricMapBuilderRBPF"
00074      *
00075          * \sa mrpt::slam::CMetricMapBuilderRBPF
00076          */
00077         class SLAM_IMPEXP CMultiMetricMapPDF :
00078                 public mrpt::utils::CSerializable,
00079                 public mrpt::bayes::CParticleFilterData<CRBPFParticleData>,
00080                 public mrpt::bayes::CParticleFilterCapable,
00081                 public mrpt::slam::PF_implementation<CRBPFParticleData,CMultiMetricMapPDF>
00082         {
00083                 friend class CMetricMapBuilderRBPF;
00084                 //template <class PARTICLE_TYPE, class MYSELF> friend class PF_implementation;
00085 
00086                 // This must be added to any CSerializable derived class:
00087                 DEFINE_SERIALIZABLE( CMultiMetricMapPDF )
00088 
00089                 // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
00090                 IMPLEMENT_PARTICLE_FILTER_CAPABLE(CRBPFParticleData);
00091 
00092         protected:
00093                 /** The PF algorithm implementation.
00094                   */
00095                 void  prediction_and_update_pfStandardProposal(
00096                         const mrpt::slam::CActionCollection     * action,
00097                         const mrpt::slam::CSensoryFrame         * observation,
00098                         const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00099 
00100                 /** The PF algorithm implementation.
00101                   */
00102                 void  prediction_and_update_pfOptimalProposal(
00103                         const mrpt::slam::CActionCollection     * action,
00104                         const mrpt::slam::CSensoryFrame         * observation,
00105                         const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00106 
00107                 /** The PF algorithm implementation.
00108                   */
00109                 void  prediction_and_update_pfAuxiliaryPFOptimal(
00110                         const mrpt::slam::CActionCollection     * action,
00111                         const mrpt::slam::CSensoryFrame         * observation,
00112                         const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00113 
00114 
00115         private:
00116                 /** Internal buffer for the averaged map.
00117                   */
00118                 CMultiMetricMap                 averageMap;
00119                 bool                                    averageMapIsUpdated;
00120 
00121                 /** The SFs and their corresponding pose estimations:
00122                  */
00123                 CSimpleMap      SFs;
00124 
00125                 /** A mapping between indexes in the SFs to indexes in the robot paths from particles.
00126                   */
00127                 std::vector<uint32_t>   SF2robotPath;
00128 
00129 
00130                 /** Entropy aux. function
00131                   */
00132                 float  H(float p);
00133 
00134         public:
00135 
00136                 /** The struct for passing extra simulation parameters to the prediction/update stage
00137                  *    when running a particle filter.
00138                  * \sa prediction_and_update
00139                  */
00140                 struct SLAM_IMPEXP TPredictionParams : public utils::CLoadableOptions
00141                 {
00142                         /** Default settings method.
00143                           */
00144                         TPredictionParams();
00145 
00146                         /** See utils::CLoadableOptions
00147                           */
00148                         void  loadFromConfigFile(
00149                                 const mrpt::utils::CConfigFileBase  &source,
00150                                 const std::string &section);
00151 
00152                         /** See utils::CLoadableOptions
00153                           */
00154                         void  dumpToTextStream(CStream  &out) const;
00155 
00156                         /** [pf optimal proposal only]  Only for PF algorithm=2 (Exact "pfOptimalProposal")
00157                          *   Select the map on which to calculate the optimal
00158                          *    proposal distribution. Values:
00159                          *   0: Gridmap   -> Uses Scan matching-based approximation (based on Stachniss' work)
00160                          *   1: Landmarks -> Uses matching to approximate optimal
00161                          *   2: Beacons   -> Used for exact optimal proposal in RO-SLAM
00162                          *   3: Pointsmap -> Uses Scan matching-based approximation with a map of points (based on Stachniss' work)
00163                          *  Default = 0
00164                          */
00165                         int             pfOptimalProposal_mapSelection;
00166 
00167 
00168                         /** [prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum quality ratio [0,1] of the alignment such as it will be accepted. Otherwise, raw odometry is used for those bad cases (default=0.7).
00169                           */
00170                         float           ICPGlobalAlign_MinQuality;
00171 
00172                         /** [update stage] If the likelihood is computed through the occupancy grid map, then this structure is passed to the map when updating the particles weights in the update stage.
00173                           */
00174                         COccupancyGridMap2D::TLikelihoodOptions         update_gridMapLikelihoodOptions;
00175 
00176                         TKLDParams              KLD_params;
00177 
00178                         CICP::TConfigParams             icp_params; //!< ICP parameters, used only when "PF_algorithm=2" in the particle filter.
00179 
00180                 } options;
00181 
00182                 /** Constructor
00183                   */
00184                 CMultiMetricMapPDF(
00185                         const bayes::CParticleFilter::TParticleFilterOptions    &opts = bayes::CParticleFilter::TParticleFilterOptions(),
00186                         const mrpt::slam::TSetOfMetricMapInitializers               *mapsInitializers = NULL,
00187                         const TPredictionParams                                             *predictionOptions = NULL );
00188 
00189                 /** Destructor
00190                  */
00191                 virtual ~CMultiMetricMapPDF();
00192 
00193                 /** Clear all elements of the maps, and restore all paths to a single starting pose */
00194                 void  clear( const CPose2D &initialPose );
00195 
00196                 /** Clear all elements of the maps, and restore all paths to a single starting pose */
00197                 void  clear( const CPose3D &initialPose );
00198 
00199                  /** Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.
00200                   * \sa getEstimatedPosePDF
00201                   */
00202                 void  getEstimatedPosePDFAtTime(
00203                         size_t                          timeStep,
00204                         CPose3DPDFParticles &out_estimation ) const;
00205 
00206                  /** Returns the current estimate of the robot pose, as a particles PDF.
00207                   * \sa getEstimatedPosePDFAtTime
00208                   */
00209                 void  getEstimatedPosePDF( CPose3DPDFParticles  &out_estimation ) const;
00210 
00211                 /** Returns the weighted averaged map based on the current best estimation. If you need a persistent copy of this object, please use "CSerializable::duplicate" and use the copy.
00212                   */
00213                 CMultiMetricMap * getCurrentMetricMapEstimation( );
00214 
00215                 /** Returns a pointer to the current most likely map (associated to the most likely particle).
00216                   */
00217                 CMultiMetricMap  * getCurrentMostLikelyMetricMap( );
00218 
00219                 /** Get the number of CSensoryFrame inserted into the internal member SFs */
00220                 size_t  getNumberOfObservationsInSimplemap() const { return SFs.size(); }
00221 
00222                 /** Insert an observation to the map, at each particle's pose and to each particle's metric map.
00223                   * \param sf The SF to be inserted
00224                   */
00225                 void  insertObservation(CSensoryFrame   &sf);
00226 
00227                 /** Return the path (in absolute coordinate poses) for the i'th particle.
00228                   * \exception On index out of bounds
00229                   */
00230                 void  getPath(size_t i, std::deque<math::TPose3D> &out_path) const;
00231 
00232                 /** Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance.
00233                   */
00234                 double  getCurrentEntropyOfPaths();
00235 
00236                 /** Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti and W.Burgard.
00237                   */
00238                 double  getCurrentJointEntropy();
00239 
00240                 /** Update the poses estimation of the member "SFs" according to the current path belief.
00241                   */
00242                 void  updateSensoryFrameSequence();
00243 
00244                 /** A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).
00245                   */
00246                 void  saveCurrentPathEstimationToTextFile( const std::string  &fil );
00247 
00248                 /** An index [0,1] measuring how much information an observation aports to the map (Typ. threshold=0.07)
00249                   */
00250                 float                                           newInfoIndex;
00251 
00252          private:
00253                 /** Rebuild the "expected" grid map. Used internally, do not call
00254                   */
00255                 void  rebuildAverageMap();
00256 
00257 
00258 
00259         //protected:
00260         public:
00261                         /** \name Virtual methods that the PF_implementations assume exist.
00262                             @{ */
00263 
00264                         /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
00265                         const TPose3D * getLastPose(const size_t i) const;
00266 
00267                         void PF_SLAM_implementation_custom_update_particle_with_new_pose(
00268                                 CParticleDataContent *particleData,
00269                                 const TPose3D &newPose) const;
00270 
00271                         // The base implementation is fine for this class.
00272                         // void PF_SLAM_implementation_replaceByNewParticleSet( ...
00273 
00274                         bool PF_SLAM_implementation_doWeHaveValidObservations(
00275                                 const CParticleList     &particles,
00276                                 const CSensoryFrame *sf) const;
00277 
00278                         bool PF_SLAM_implementation_skipRobotMovement() const;
00279 
00280                         /** Evaluate the observation likelihood for one particle at a given location */
00281                         double PF_SLAM_computeObservationLikelihoodForParticle(
00282                                 const CParticleFilter::TParticleFilterOptions   &PF_options,
00283                                 const size_t                    particleIndexForMap,
00284                                 const CSensoryFrame             &observation,
00285                                 const CPose3D                   &x ) const;
00286                         /** @} */
00287 
00288 
00289         }; // End of class def.
00290 
00291         } // End of namespace
00292 } // End of namespace
00293 
00294 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011