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.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |