00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 &sfSeq ); 00115 00116 /** Insert the observation information into this map. This method must be implemented 00117 * in derived classes. 00118 * \param obs The observation 00119 * \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. 00120 * 00121 * \sa CObservation::insertObservationInto 00122 */ 00123 inline bool insertObservation( 00124 const CObservation *obs, 00125 const CPose3D *robotPose = NULL ) 00126 { 00127 bool done = internal_insertObservation(obs,robotPose); 00128 if (done) 00129 { 00130 OnPostSuccesfulInsertObs(obs); 00131 publishEvent( mrptEventMetricMapInsert(this,obs,robotPose) ); 00132 } 00133 return done; 00134 } 00135 00136 /** A wrapper for smart pointers, just calls the non-smart pointer version. */ 00137 inline bool insertObservationPtr( 00138 const CObservationPtr &obs, 00139 const CPose3D *robotPose = NULL ) 00140 { 00141 MRPT_START 00142 if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); } 00143 return insertObservation(obs.pointer(),robotPose); 00144 MRPT_END 00145 } 00146 00147 /** Computes the log-likelihood of a given observation given an arbitrary robot 3D pose. 00148 * 00149 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00150 * \param obs The observation. 00151 * \return This method returns a log-likelihood. 00152 * 00153 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00154 */ 00155 virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ) = 0; 00156 00157 /** Computes the log-likelihood of a given observation given an arbitrary robot 2D 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 double computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom ) 00166 { 00167 return computeObservationLikelihood(obs,CPose3D(takenFrom)); 00168 } 00169 00170 /** 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). 00171 * \param obs The observation. 00172 * \sa computeObservationLikelihood 00173 */ 00174 virtual bool canComputeObservationLikelihood( const CObservation *obs ) 00175 { 00176 return true; // Unless implemented otherwise, we can always compute the likelihood. 00177 } 00178 00179 /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame. 00180 * 00181 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00182 * \param sf The set of observations in a CSensoryFrame. 00183 * \return This method returns a log-likelihood. 00184 * \sa canComputeObservationsLikelihood 00185 */ 00186 double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom ); 00187 00188 /** 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). 00189 * \param sf The observations. 00190 * \sa canComputeObservationLikelihood 00191 */ 00192 bool canComputeObservationsLikelihood( const CSensoryFrame &sf ); 00193 00194 /** Constructor 00195 */ 00196 CMetricMap(); 00197 00198 /** Destructor 00199 */ 00200 virtual ~CMetricMap(); 00201 00202 /** Aligns an observation to its maximum likelihood pose (in 2D) into this map, by hill climbing in values computed with "computeObservationLikelihood". 00203 * \param obs The observation to be aligned 00204 * \param in_initialEstimatedPose The initial input to the algorithm, an initial "guess" for the observation pose in the map. 00205 * \param out_ResultingPose The resulting pose from this algorithm 00206 */ 00207 void alignBylikelihoodHillClimbing( CObservation *obs, CPose2D in_initialEstimatedPose, CPose2D &out_ResultingPose ); 00208 00209 typedef mrpt::utils::TMatchingPair TMatchingPair; 00210 typedef mrpt::utils::TMatchingPairPtr TMatchingPairPtr; 00211 typedef mrpt::utils::TMatchingPairList TMatchingPairList; 00212 00213 00214 /** Computes the matchings between this and another 2D points map. 00215 This includes finding: 00216 - The set of points pairs in each map 00217 - The mean squared distance between corresponding pairs. 00218 This method is the most time critical one into the ICP algorithm. 00219 00220 * \param otherMap [IN] The other map to compute the matching with. 00221 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00222 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched. 00223 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00224 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00225 * \param correspondences [OUT] The detected matchings pairs. 00226 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00227 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00228 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00229 * 00230 * \sa compute3DMatchingRatio 00231 */ 00232 virtual void computeMatchingWith2D( 00233 const CMetricMap *otherMap, 00234 const CPose2D &otherMapPose, 00235 float maxDistForCorrespondence, 00236 float maxAngularDistForCorrespondence, 00237 const CPose2D &angularDistPivotPoint, 00238 TMatchingPairList &correspondences, 00239 float &correspondencesRatio, 00240 float *sumSqrDist = NULL, 00241 bool onlyKeepTheClosest = true, 00242 bool onlyUniqueRobust = false ) const 00243 { 00244 MRPT_START 00245 THROW_EXCEPTION("Virtual method not implemented in derived class.") 00246 MRPT_END 00247 } 00248 00249 /** Computes the matchings between this and another 3D points map - method used in 3D-ICP. 00250 This method finds the set of point pairs in each map. 00251 00252 The method is the most time critical one into the ICP algorithm. 00253 00254 * \param otherMap [IN] The other map to compute the matching with. 00255 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00256 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched. 00257 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00258 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00259 * \param correspondences [OUT] The detected matchings pairs. 00260 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00261 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00262 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00263 * 00264 * \sa compute3DMatchingRatio 00265 */ 00266 virtual void computeMatchingWith3D( 00267 const CMetricMap *otherMap, 00268 const CPose3D &otherMapPose, 00269 float maxDistForCorrespondence, 00270 float maxAngularDistForCorrespondence, 00271 const CPoint3D &angularDistPivotPoint, 00272 TMatchingPairList &correspondences, 00273 float &correspondencesRatio, 00274 float *sumSqrDist = NULL, 00275 bool onlyKeepTheClosest = true, 00276 bool onlyUniqueRobust = false ) const 00277 { 00278 MRPT_START 00279 THROW_EXCEPTION("Virtual method not implemented in derived class.") 00280 MRPT_END 00281 } 00282 00283 00284 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00285 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00286 * \param otherMap [IN] The other map to compute the matching with. 00287 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00288 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00289 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00290 * 00291 * \return The matching ratio [0,1] 00292 * \sa computeMatchingWith2D 00293 */ 00294 virtual float compute3DMatchingRatio( 00295 const CMetricMap *otherMap, 00296 const CPose3D &otherMapPose, 00297 float minDistForCorr = 0.10f, 00298 float minMahaDistForCorr = 2.0f 00299 ) const = 0; 00300 00301 /** 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). 00302 */ 00303 virtual void saveMetricMapRepresentationToFile( 00304 const std::string &filNamePrefix 00305 ) const = 0; 00306 00307 /** Returns a 3D object representing the map. 00308 */ 00309 virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0; 00310 00311 /** When set to true (default=false), calling "getAs3DObject" will have no effects. 00312 */ 00313 bool m_disableSaveAs3DObject; 00314 00315 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00316 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00317 */ 00318 virtual void auxParticleFilterCleanUp() = 0; 00319 00320 /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map. 00321 */ 00322 virtual float squareDistanceToClosestCorrespondence( 00323 const float &x0, 00324 const float &y0 ) const 00325 { 00326 MRPT_START 00327 THROW_EXCEPTION("Virtual method not implemented in derived class.") 00328 MRPT_END 00329 } 00330 00331 00332 /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it. 00333 * Otherwise, return NULL 00334 */ 00335 virtual const CSimplePointsMap * getAsSimplePointsMap() const { return NULL; } 00336 virtual CSimplePointsMap * getAsSimplePointsMap() { return NULL; } 00337 00338 00339 }; // End of class def. 00340 00341 /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class): 00342 */ 00343 typedef std::deque<CMetricMap*> TMetricMapList; 00344 00345 } // End of namespace 00346 } // End of namespace 00347 00348 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
