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