Main MRPT website > C++ reference
MRPT logo

CMetricMap.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 CMetricMap_H
00029 #define CMetricMap_H
00030 
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/opengl/CSetOfObjects.h>
00033 
00034 #include <mrpt/slam/CObservation.h>
00035 #include <mrpt/utils/TMatchingPair.h>
00036 
00037 #include <mrpt/poses/CPoint2D.h>
00038 #include <mrpt/poses/CPoint3D.h>
00039 #include <mrpt/poses/CPose2D.h>
00040 #include <mrpt/poses/CPose3D.h>
00041 
00042 #include <mrpt/utils/CObservable.h>
00043 #include <mrpt/slam/CMetricMapEvents.h>
00044 
00045 namespace mrpt
00046 {
00047         namespace slam
00048         {
00049                 using namespace mrpt::utils;
00050 
00051                 class CObservation;
00052                 class CPointsMap;
00053                 class CSimplePointsMap;
00054                 class CSimpleMap;
00055                 class CSensoryFrame;
00056 
00057                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CMetricMap, mrpt::utils::CSerializable, OBS_IMPEXP )
00058 
00059                 /** Declares a virtual base class for all metric maps storage classes.
00060                            In this class virtual methods are provided to allow the insertion
00061                                 of any type of "CObservation" objects into the metric map, thus
00062                                 updating the map (doesn't matter if it is a 2D/3D grid or a points
00063                                 map).
00064                            <b>IMPORTANT</b>: Observations doesn't include any information about the
00065                                 robot pose beliefs, just the raw observation and information about
00066                                 the sensor pose relative to the robot mobile base coordinates origin.
00067                  *
00068                  *  Note that all metric maps implement this mrpt::utils::CObservable interface,
00069                  *   emitting the following events:
00070                  *        - mrpt::slam::mrptEventMetricMapClear: Upon call of the ::clear() method.
00071                  *    - mrpt::slam::mrptEventMetricMapInsert: Upon insertion of an observation that effectively modifies the map (e.g. inserting an image into a grid map will NOT raise an event, inserting a laser scan will).
00072                  *
00073                  * \sa CObservation, CSensoryFrame, CMultiMetricMap
00074                  */
00075                 class OBS_IMPEXP CMetricMap :
00076                         public mrpt::utils::CSerializable,
00077                         public mrpt::utils::CObservable
00078                 {
00079                         // This must be added to any CSerializable derived class:
00080                         DEFINE_VIRTUAL_SERIALIZABLE( CMetricMap )
00081 
00082                 private:
00083                         /** Internal method called by clear() */
00084                         virtual void  internal_clear() = 0;
00085 
00086                         /** Hook for each time a "internal_insertObservation" returns "true"
00087                           * This is called automatically from insertObservation() when internal_insertObservation returns true.
00088                           */
00089                         virtual void OnPostSuccesfulInsertObs(const CObservation *)
00090                         {
00091                                 // Default: do nothing
00092                         }
00093 
00094                         /** Internal method called by insertObservation() */
00095                         virtual bool  internal_insertObservation(
00096                                 const CObservation *obs,
00097                                 const CPose3D *robotPose = NULL ) = 0;
00098 
00099                 public:
00100                         /** Erase all the contents of the map */
00101                         void  clear();
00102 
00103                         /** Returns true if the map is empty/no observation has been inserted.
00104                           */
00105                         virtual bool  isEmpty() const = 0;
00106 
00107                         /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
00108                          *  This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
00109                          *   given by the "poses::CPosePDF" in the CSimpleMap object.
00110                          *
00111                          * \sa insertObservation, CSimpleMap
00112                          * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
00113                          */
00114                         void  loadFromProbabilisticPosesAndObservations( const CSimpleMap &Map );
00115 
00116                         /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
00117                          *  This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
00118                          *   given by the "poses::CPosePDF" in the CSimpleMap object.
00119                          *
00120                          * \sa insertObservation, CSimpleMap
00121                          * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
00122                          */
00123                         inline void  loadFromSimpleMap( const CSimpleMap &Map ) {  loadFromProbabilisticPosesAndObservations(Map); }
00124 
00125                         /** Insert the observation information into this map. This method must be implemented
00126                          *    in derived classes.
00127                          * \param obs The observation
00128                          * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use the origin.
00129                          *
00130                          * \sa CObservation::insertObservationInto
00131                          */
00132                         inline bool  insertObservation(
00133                                 const CObservation *obs,
00134                                 const CPose3D *robotPose = NULL )
00135                         {
00136                                 bool done = internal_insertObservation(obs,robotPose);
00137                                 if (done)
00138                                 {
00139                                         OnPostSuccesfulInsertObs(obs);
00140                                         publishEvent( mrptEventMetricMapInsert(this,obs,robotPose) );
00141                                 }
00142                                 return done;
00143                         }
00144 
00145                         /** A wrapper for smart pointers, just calls the non-smart pointer version. */
00146                         inline bool  insertObservationPtr(
00147                                 const CObservationPtr &obs,
00148                                 const CPose3D *robotPose = NULL )
00149                         {
00150                                 MRPT_START
00151                                 if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); }
00152                                 return insertObservation(obs.pointer(),robotPose);
00153                                 MRPT_END
00154                         }
00155 
00156                         /** Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
00157                          *
00158                          * \param takenFrom The robot's pose the observation is supposed to be taken from.
00159                          * \param obs The observation.
00160                          * \return This method returns a log-likelihood.
00161                          *
00162                          * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
00163                          */
00164                         virtual double   computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ) = 0;
00165 
00166                         /** Computes the log-likelihood of a given observation given an arbitrary robot 2D pose.
00167                          *
00168                          * \param takenFrom The robot's pose the observation is supposed to be taken from.
00169                          * \param obs The observation.
00170                          * \return This method returns a log-likelihood.
00171                          *
00172                          * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
00173                          */
00174                         double   computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom )
00175                         {
00176                                 return computeObservationLikelihood(obs,CPose3D(takenFrom));
00177                         }
00178 
00179                         /** Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. an occupancy grid map cannot with an image).
00180                          * \param obs The observation.
00181                          * \sa computeObservationLikelihood
00182                          */
00183                         virtual bool canComputeObservationLikelihood( const CObservation *obs )
00184                         {
00185                                 return true; // Unless implemented otherwise, we can always compute the likelihood.
00186                         }
00187 
00188                         /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame.
00189                          *
00190                          * \param takenFrom The robot's pose the observation is supposed to be taken from.
00191                          * \param sf The set of observations in a CSensoryFrame.
00192                          * \return This method returns a log-likelihood.
00193                          * \sa canComputeObservationsLikelihood
00194                          */
00195                         double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom );
00196 
00197                         /** Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. an occupancy grid map cannot with an image).
00198                          * \param sf The observations.
00199                          * \sa canComputeObservationLikelihood
00200                          */
00201                         bool canComputeObservationsLikelihood( const CSensoryFrame &sf );
00202 
00203                         /** Constructor
00204                           */
00205                         CMetricMap();
00206 
00207                         /** Destructor
00208                           */
00209                         virtual ~CMetricMap();
00210 
00211 #ifdef MRPT_BACKCOMPATIB_08X    // For backward compatibility
00212                         typedef mrpt::utils::TMatchingPair      TMatchingPair;
00213                         typedef mrpt::utils::TMatchingPairPtr   TMatchingPairPtr;
00214                         typedef mrpt::utils::TMatchingPairList  TMatchingPairList;
00215 #endif
00216 
00217                         /** Computes the matchings between this and another 2D points map.
00218                            This includes finding:
00219                                         - The set of points pairs in each map
00220                                         - The mean squared distance between corresponding pairs.
00221                            This method is the most time critical one into the ICP algorithm.
00222 
00223                          * \param  otherMap                                       [IN] The other map to compute the matching with.
00224                          * \param  otherMapPose                           [IN] The pose of the other map as seen from "this".
00225                          * \param  maxDistForCorrespondence   [IN] Maximum 2D linear distance between two points to be matched.
00226                          * \param  maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
00227                          * \param  angularDistPivotPoint      [IN] The point used to calculate distances from in both maps.
00228                          * \param  correspondences                        [OUT] The detected matchings pairs.
00229                          * \param  correspondencesRatio           [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
00230                          * \param  sumSqrDist                             [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
00231                          * \param  onlyKeepTheClosest         [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
00232                          *
00233                          * \sa compute3DMatchingRatio
00234                          */
00235                         virtual void  computeMatchingWith2D(
00236                                 const CMetricMap                                                *otherMap,
00237                                 const CPose2D                                                   &otherMapPose,
00238                                 float                                                                   maxDistForCorrespondence,
00239                                 float                                                                   maxAngularDistForCorrespondence,
00240                                 const CPose2D                                                   &angularDistPivotPoint,
00241                                 TMatchingPairList                                               &correspondences,
00242                                 float                                                                   &correspondencesRatio,
00243                                 float                                                                   *sumSqrDist     = NULL,
00244                                 bool                                                                    onlyKeepTheClosest = true,
00245                                 bool                                                                    onlyUniqueRobust = false ) const
00246                         {
00247                                 MRPT_START
00248                                 THROW_EXCEPTION("Virtual method not implemented in derived class.")
00249                                 MRPT_END
00250                         }
00251 
00252                         /** Computes the matchings between this and another 3D points map - method used in 3D-ICP.
00253                            This method finds the set of point pairs in each map.
00254 
00255                            The method is the most time critical one into the ICP algorithm.
00256 
00257                          * \param  otherMap                                       [IN] The other map to compute the matching with.
00258                          * \param  otherMapPose                           [IN] The pose of the other map as seen from "this".
00259                          * \param  maxDistForCorrespondence   [IN] Maximum 2D linear distance between two points to be matched.
00260                          * \param  maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
00261                          * \param  angularDistPivotPoint      [IN] The point used to calculate distances from in both maps.
00262                          * \param  correspondences                        [OUT] The detected matchings pairs.
00263                          * \param  correspondencesRatio           [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
00264                          * \param  sumSqrDist                             [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
00265                          * \param  onlyKeepTheClosest         [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
00266                          *
00267                          * \sa compute3DMatchingRatio
00268                          */
00269                         virtual void  computeMatchingWith3D(
00270                                 const CMetricMap                                                *otherMap,
00271                                 const CPose3D                                                   &otherMapPose,
00272                                 float                                                                   maxDistForCorrespondence,
00273                                 float                                                                   maxAngularDistForCorrespondence,
00274                                 const CPoint3D                                                  &angularDistPivotPoint,
00275                                 TMatchingPairList                                               &correspondences,
00276                                 float                                                                   &correspondencesRatio,
00277                                 float                                                                   *sumSqrDist     = NULL,
00278                                 bool                                                                    onlyKeepTheClosest = true,
00279                                 bool                                                                    onlyUniqueRobust = false ) const
00280                         {
00281                                 MRPT_START
00282                                 THROW_EXCEPTION("Virtual method not implemented in derived class.")
00283                                 MRPT_END
00284                         }
00285 
00286 
00287                         /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
00288                          *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
00289                          * \param  otherMap                                       [IN] The other map to compute the matching with.
00290                          * \param  otherMapPose                           [IN] The 6D pose of the other map as seen from "this".
00291                          * \param  minDistForCorr                         [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
00292                          * \param  minMahaDistForCorr             [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
00293                          *
00294                          * \return The matching ratio [0,1]
00295                          * \sa computeMatchingWith2D
00296                          */
00297                         virtual float  compute3DMatchingRatio(
00298                                 const CMetricMap                                                                *otherMap,
00299                                 const CPose3D                                                   &otherMapPose,
00300                                 float                                                                   minDistForCorr = 0.10f,
00301                                 float                                                                   minMahaDistForCorr = 2.0f
00302                         ) const = 0;
00303 
00304                         /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
00305                           */
00306                         virtual void  saveMetricMapRepresentationToFile(
00307                                 const std::string       &filNamePrefix
00308                         ) const = 0;
00309 
00310                         /** Returns a 3D object representing the map.
00311                           */
00312                         virtual void  getAs3DObject( mrpt::opengl::CSetOfObjectsPtr     &outObj ) const = 0;
00313 
00314                         /** When set to true (default=false), calling "getAs3DObject" will have no effects.
00315                           */
00316                         bool                    m_disableSaveAs3DObject;
00317 
00318                         /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
00319                           *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
00320                           */
00321                         virtual void  auxParticleFilterCleanUp() = 0;
00322 
00323                         /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
00324                           */
00325                         virtual float squareDistanceToClosestCorrespondence(
00326                                 const float   &x0,
00327                                 const float   &y0 ) const
00328                         {
00329                                 MRPT_START
00330                                 THROW_EXCEPTION("Virtual method not implemented in derived class.")
00331                                 MRPT_END
00332                         }
00333 
00334 
00335                         /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
00336                           * Otherwise, return NULL
00337                           */
00338                         virtual const CSimplePointsMap * getAsSimplePointsMap() const { return NULL; }
00339                         virtual       CSimplePointsMap * getAsSimplePointsMap()       { return NULL; }
00340 
00341 
00342                 }; // End of class def.
00343 
00344                 /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
00345                   */
00346                 typedef std::deque<CMetricMap*> TMetricMapList;
00347 
00348         } // End of namespace
00349 } // End of namespace
00350 
00351 #endif



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