00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 00036 #include <mrpt/poses/CPoint2D.h> 00037 #include <mrpt/poses/CPoint3D.h> 00038 #include <mrpt/poses/CPose2D.h> 00039 #include <mrpt/poses/CPose3D.h> 00040 00041 00042 namespace mrpt 00043 { 00044 namespace slam 00045 { 00046 using namespace mrpt::utils; 00047 00048 class CObservation; 00049 class CPointsMap; 00050 class CSensFrameProbSequence; 00051 class CSensoryFrame; 00052 00053 /** Declares a virtual base class for all metric maps storage classes. 00054 In this class virtual methods are provided to allow the insertion 00055 of any type of "CObservation" objects into the metric map, thus 00056 updating the map (doesn't matter if it is a 2D/3D grid or a points 00057 map). 00058 <b>IMPORTANT</b>: Observations doesn't include any information about the 00059 robot pose beliefs, just the raw observation and information about 00060 the sensor pose relative to the robot mobile base coordinates origin. 00061 * 00062 * \sa CObservation, CSensoryFrame, CMultiMetricMap 00063 */ 00064 class MRPTDLLIMPEXP CMetricMap : public mrpt::utils::CSerializable 00065 { 00066 // This must be added to any CSerializable derived class: 00067 DEFINE_VIRTUAL_SERIALIZABLE( CMetricMap ) 00068 00069 public: 00070 /** Erase all the contents of the map 00071 */ 00072 virtual void clear() = 0; 00073 00074 /** Returns true if the map is empty/no observation has been inserted. 00075 */ 00076 virtual bool isEmpty() const = 0; 00077 00078 /** Load the map contents from a CSensFrameProbSequence object, erasing all previous content of the map. 00079 * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as 00080 * given by the "poses::CPosePDF" in the CSensFrameProbSequence object. 00081 * 00082 * \sa insertObservation, CSensFrameProbSequence 00083 * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc... 00084 */ 00085 void loadFromProbabilisticPosesAndObservations( CSensFrameProbSequence &sfSeq ); 00086 00087 /** Insert the observation information into this map. This method must be implemented 00088 * in derived classes. 00089 * \param obs The observation 00090 * \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. 00091 * 00092 * \sa CObservation::insertObservationInto 00093 */ 00094 virtual bool insertObservation( 00095 const CObservation *obs, 00096 const CPose3D *robotPose = NULL ) = 0; 00097 00098 /** A wrapper for smart pointers, just calls the non-smart pointer version. */ 00099 bool insertObservationPtr( 00100 const CObservationPtr &obs, 00101 const CPose3D *robotPose = NULL ) 00102 { 00103 MRPT_TRY_START; 00104 if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); } 00105 return insertObservation(obs.pointer(),robotPose); 00106 MRPT_TRY_END; 00107 } 00108 00109 /** Computes the log-likelihood of a given observation given an arbitrary robot 3D pose. 00110 * 00111 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00112 * \param obs The observation. 00113 * \return This method returns a log-likelihood. 00114 * 00115 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00116 */ 00117 virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ) = 0; 00118 00119 /** Computes the log-likelihood of a given observation given an arbitrary robot 2D pose. 00120 * 00121 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00122 * \param obs The observation. 00123 * \return This method returns a log-likelihood. 00124 * 00125 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00126 */ 00127 double computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom ) 00128 { 00129 return computeObservationLikelihood(obs,CPose3D(takenFrom)); 00130 } 00131 00132 /** 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). 00133 * \param obs The observation. 00134 * \sa computeObservationLikelihood 00135 */ 00136 virtual bool canComputeObservationLikelihood( const CObservation *obs ) 00137 { 00138 return true; // Unless implemented otherwise, we can always compute the likelihood. 00139 } 00140 00141 /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame. 00142 * 00143 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00144 * \param sf The set of observations in a CSensoryFrame. 00145 * \return This method returns a log-likelihood. 00146 * \sa canComputeObservationsLikelihood 00147 */ 00148 double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom ); 00149 00150 /** 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). 00151 * \param sf The observations. 00152 * \sa canComputeObservationLikelihood 00153 */ 00154 bool canComputeObservationsLikelihood( const CSensoryFrame &sf ); 00155 00156 /** Constructor 00157 */ 00158 CMetricMap(); 00159 00160 /** Destructor 00161 */ 00162 virtual ~CMetricMap(); 00163 00164 /** Aligns an observation to its maximum likelihood pose (in 2D) into this map, by hill climbing in values computed with "computeObservationLikelihood". 00165 * \param obs The observation to be aligned 00166 * \param in_initialEstimatedPose The initial input to the algorithm, an initial "guess" for the observation pose in the map. 00167 * \param out_ResultingPose The resulting pose from this algorithm 00168 */ 00169 void alignBylikelihoodHillClimbing( CObservation *obs, CPose2D in_initialEstimatedPose, CPose2D &out_ResultingPose ); 00170 00171 /** An structure for returning the points pair-matchings 00172 * \sa ComputeMatchingWith 00173 */ 00174 struct MRPTDLLIMPEXP TMatchingPair 00175 { 00176 TMatchingPair() : 00177 this_idx(0), other_idx(0), 00178 this_x(0),this_y(0),this_z(0), 00179 other_x(0),other_y(0),other_z(0), 00180 errorSquareAfterTransformation(0) 00181 { 00182 } 00183 00184 TMatchingPair( unsigned int _this_idx,unsigned int _other_idx, float _this_x, float _this_y,float _this_z, float _other_x,float _other_y,float _other_z ) : 00185 this_idx(_this_idx), other_idx(_other_idx), 00186 this_x(_this_x),this_y(_this_y),this_z(_this_z), 00187 other_x(_other_x),other_y(_other_y),other_z(_other_z), 00188 errorSquareAfterTransformation(0) 00189 { 00190 } 00191 00192 unsigned int this_idx; 00193 unsigned int other_idx; 00194 float this_x,this_y,this_z; 00195 float other_x,other_y,other_z; 00196 float errorSquareAfterTransformation; 00197 00198 }; 00199 00200 typedef TMatchingPair* TMatchingPairPtr; 00201 00202 /** A list of TMatchingPair 00203 */ 00204 class MRPTDLLIMPEXP TMatchingPairList : public std::deque<TMatchingPair> 00205 { 00206 public: 00207 00208 /** Checks if the given index from the "other" map appears in the list. 00209 */ 00210 bool indexOtherMapHasCorrespondence(unsigned int idx); 00211 00212 /** Saves the correspondences to a text file 00213 */ 00214 void dumpToFile(const char *fileName); 00215 00216 /** Saves the correspondences as a MATLAB script which draws them. 00217 */ 00218 void saveAsMATLABScript( const std::string &filName ); 00219 00220 /** Computes the overall square error between the 2D points in the list of correspondences, given the 2D transformation "q" 00221 * \f[ \sum\limits_i e_i \f] 00222 * Where \f$ e_i \f$ are the elements of the square error vector as computed by computeSquareErrorVector 00223 * \sa squareErrorVector, overallSquareErrorAndPoints 00224 */ 00225 float overallSquareError( const CPose2D &q ) const; 00226 00227 /** Computes the overall square error between the 2D points in the list of correspondences, given the 2D transformation "q", and return the transformed points as well. 00228 * \f[ \sum\limits_i e_i \f] 00229 * Where \f$ e_i \f$ are the elements of the square error vector as computed by computeSquareErrorVector 00230 * \sa squareErrorVector 00231 */ 00232 float overallSquareErrorAndPoints( 00233 const CPose2D &q, 00234 vector_float &xs, 00235 vector_float &ys ) const; 00236 00237 00238 /** Returns a vector with the square error between each pair of correspondences in the list, given the 2D transformation "q" 00239 * Each element \f$ e_i \f$ is the square distance between the "this" (global) point and the "other" (local) point transformed through "q": 00240 * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f] 00241 * \sa overallSquareError 00242 */ 00243 void squareErrorVector(const CPose2D &q, vector_float &out_sqErrs ) const; 00244 00245 /** Returns a vector with the square error between each pair of correspondences in the list and the transformed "other" (local) points, given the 2D transformation "q" 00246 * Each element \f$ e_i \f$ is the square distance between the "this" (global) point and the "other" (local) point transformed through "q": 00247 * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f] 00248 * \sa overallSquareError 00249 */ 00250 void squareErrorVector( 00251 const CPose2D &q, 00252 vector_float &out_sqErrs, 00253 vector_float &xs, 00254 vector_float &ys ) const; 00255 00256 /** Test whether the given pair "p" is within the pairings */ 00257 bool contains (const TMatchingPair &p) const; 00258 00259 }; 00260 00261 /** Computes the matchings between this and another 2D points map. 00262 This includes finding: 00263 - The set of points pairs in each map 00264 - The mean squared distance between corresponding pairs. 00265 This method is the most time critical one into the ICP algorithm. 00266 00267 * \param otherMap [IN] The other map to compute the matching with. 00268 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00269 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched. 00270 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00271 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00272 * \param correspondences [OUT] The detected matchings pairs. 00273 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00274 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00275 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00276 * 00277 * \sa compute3DMatchingRatio 00278 */ 00279 virtual void computeMatchingWith2D( 00280 const CMetricMap *otherMap, 00281 const CPose2D &otherMapPose, 00282 float maxDistForCorrespondence, 00283 float maxAngularDistForCorrespondence, 00284 const CPose2D &angularDistPivotPoint, 00285 TMatchingPairList &correspondences, 00286 float &correspondencesRatio, 00287 float *sumSqrDist = NULL, 00288 bool onlyKeepTheClosest = true, 00289 bool onlyUniqueRobust = false ) const 00290 { 00291 MRPT_TRY_START 00292 THROW_EXCEPTION("Virtual method not implemented in derived class.") 00293 MRPT_TRY_END 00294 } 00295 00296 /** Computes the matchings between this and another 3D points map - method used in 3D-ICP. 00297 This method finds the set of point pairs in each map. 00298 00299 The method is the most time critical one into the ICP algorithm. 00300 00301 * \param otherMap [IN] The other map to compute the matching with. 00302 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00303 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched. 00304 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00305 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00306 * \param correspondences [OUT] The detected matchings pairs. 00307 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00308 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00309 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00310 * 00311 * \sa compute3DMatchingRatio 00312 */ 00313 virtual void computeMatchingWith3D( 00314 const CMetricMap *otherMap, 00315 const CPose3D &otherMapPose, 00316 float maxDistForCorrespondence, 00317 float maxAngularDistForCorrespondence, 00318 const CPoint3D &angularDistPivotPoint, 00319 TMatchingPairList &correspondences, 00320 float &correspondencesRatio, 00321 float *sumSqrDist = NULL, 00322 bool onlyKeepTheClosest = true, 00323 bool onlyUniqueRobust = false ) const 00324 { 00325 MRPT_TRY_START 00326 THROW_EXCEPTION("Virtual method not implemented in derived class.") 00327 MRPT_TRY_END 00328 } 00329 00330 00331 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00332 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00333 * \param otherMap [IN] The other map to compute the matching with. 00334 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00335 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00336 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00337 * 00338 * \return The matching ratio [0,1] 00339 * \sa computeMatchingWith2D 00340 */ 00341 virtual float compute3DMatchingRatio( 00342 const CMetricMap *otherMap, 00343 const CPose3D &otherMapPose, 00344 float minDistForCorr = 0.10f, 00345 float minMahaDistForCorr = 2.0f 00346 ) const = 0; 00347 00348 /** 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). 00349 */ 00350 virtual void saveMetricMapRepresentationToFile( 00351 const std::string &filNamePrefix 00352 ) const = 0; 00353 00354 /** Returns a 3D object representing the map. 00355 */ 00356 virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0; 00357 00358 /** When set to true (default=false), calling "getAs3DObject" will have no effects. 00359 */ 00360 bool m_disableSaveAs3DObject; 00361 00362 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00363 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00364 */ 00365 virtual void auxParticleFilterCleanUp() = 0; 00366 00367 00368 /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map. 00369 */ 00370 virtual float squareDistanceToClosestCorrespondence( 00371 const float &x0, 00372 const float &y0 ) const 00373 { 00374 MRPT_TRY_START 00375 THROW_EXCEPTION("Virtual method not implemented in derived class.") 00376 MRPT_TRY_END 00377 } 00378 00379 00380 }; // End of class def. 00381 00382 DEFINE_SERIALIZABLE_PRE( CMetricMap ) 00383 00384 /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class): 00385 */ 00386 typedef std::deque<CMetricMap*> TMetricMapList; 00387 00388 /** A comparison operator, for sorting lists of TMatchingPair's, first order by this_idx, if equals, by other_idx 00389 */ 00390 bool operator < (const mrpt::slam::CMetricMap::TMatchingPair& a, const mrpt::slam::CMetricMap::TMatchingPair& b); 00391 00392 /** A comparison operator 00393 */ 00394 bool operator == (const mrpt::slam::CMetricMap::TMatchingPair& a,const mrpt::slam::CMetricMap::TMatchingPair& b); 00395 00396 /** A comparison operator 00397 */ 00398 bool operator == (const mrpt::slam::CMetricMap::TMatchingPairList& a,const mrpt::slam::CMetricMap::TMatchingPairList& b); 00399 00400 } // End of namespace 00401 } // End of namespace 00402 00403 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |