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 CLandmarksMap_H 00029 #define CLandmarksMap_H 00030 00031 #include <mrpt/slam/CMetricMap.h> 00032 #include <mrpt/slam/CLandmark.h> 00033 #include <mrpt/slam/CObservationImage.h> 00034 //#include <mrpt/slam/CObservationStereoImages.h> 00035 #include <mrpt/slam/CObservation2DRangeScan.h> 00036 #include <mrpt/slam/CObservationGPS.h> 00037 #include <mrpt/slam/CObservationBearingRange.h> 00038 #include <mrpt/utils/CSerializable.h> 00039 #include <mrpt/math/CMatrix.h> 00040 #include <mrpt/utils/CDynamicGrid.h> 00041 #include <mrpt/utils/CLoadableOptions.h> 00042 #include <mrpt/gui/CDisplayWindow.h> 00043 00044 #include <map> 00045 00046 namespace mrpt 00047 { 00048 namespace slam 00049 { 00050 class CObservationBeaconRanges; 00051 class CObservationStereoImages; 00052 00053 /** Internal use. 00054 */ 00055 typedef std::vector<CLandmark> TSequenceLandmarks; 00056 00057 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CLandmarksMap, CMetricMap ) 00058 00059 /** A class for storing a map of 3D probabilistic landmarks. 00060 * <br> 00061 * Currently these types of landmarks are defined: (see mrpt::slam::CLandmark) 00062 * - For "visual landmarks" from images: features with associated descriptors. 00063 * - For laser scanners: each of the range measuremnts, as "occupancy" landmarks. 00064 * - For grid maps: "Panoramic descriptor" feature points. 00065 * - For range-only localization and SLAM: Beacons. It is also supported the simulation of expected beacon-to-sensor readings, observation likelihood,... 00066 * <br> 00067 * <b>How to load landmarks from observations:</b><br> 00068 * When invoking CLandmarksMap::insertObservation(), the values in CLandmarksMap::insertionOptions will 00069 * determinate the kind of landmarks that will be extracted and fused into the map. Supported feature 00070 * extraction processes are listed next: 00071 * 00072 <table> 00073 <tr> <td><b>Observation class:</b></td> <td><b>Generated Landmarks:</b></td> <td><b>Comments:</b></td> </tr> 00074 <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is created for each SIFT detected in the image, 00075 <br>2) Correspondences between SIFTs features and existing ones are finded by computeMatchingWith3DLandmarks, 00076 <br>3) The corresponding feaures are fused, and the new ones added, with an initial uncertainty according to insertionOptions</td> </tr> 00077 <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is separately procesed by the method for CObservationImage observations </td> </tr> 00078 <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO... </td> </tr> 00079 <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A landmark is added for each range in the scan, with its appropiate covariance matrix derived from the jacobians matrixes. </td> </tr> 00080 </table> 00081 * 00082 * \sa CMetricMap 00083 */ 00084 class MRPTDLLIMPEXP CLandmarksMap : public CMetricMap 00085 { 00086 // This must be added to any CSerializable derived class: 00087 DEFINE_SERIALIZABLE( CLandmarksMap ) 00088 00089 private: 00090 //void importMapMaxID( CLandmarksMap &sourceMap ); 00091 /** Auxiliary windows for displaying images 00092 */ 00093 mrpt::gui::CDisplayWindow *wind1, *wind2; 00094 00095 public: 00096 00097 static mrpt::utils::TColorf COLOR_LANDMARKS_IN_3DSCENES; //!< The color of landmark ellipsoids in CLandmarksMap::getAs3DObject 00098 00099 00100 /** The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation 00101 */ 00102 struct MRPTDLLIMPEXP TCustomSequenceLandmarks 00103 { 00104 private: 00105 /** The actual list: 00106 */ 00107 TSequenceLandmarks m_landmarks; 00108 00109 /** A grid-map with the set of landmarks falling into each cell. 00110 * \todo Use the KD-tree instead? 00111 */ 00112 CDynamicGrid<vector_int> m_grid; 00113 00114 /** Auxiliary variables used in "getLargestDistanceFromOrigin" 00115 * \sa getLargestDistanceFromOrigin 00116 */ 00117 mutable float m_largestDistanceFromOrigin; 00118 00119 /** Auxiliary variables used in "getLargestDistanceFromOrigin" 00120 * \sa getLargestDistanceFromOrigin 00121 */ 00122 mutable bool m_largestDistanceFromOriginIsUpdated; 00123 00124 public: 00125 /** Default constructor 00126 */ 00127 TCustomSequenceLandmarks(); 00128 00129 typedef TSequenceLandmarks::iterator iterator; 00130 iterator begin() { return m_landmarks.begin(); }; 00131 iterator end() { return m_landmarks.end(); }; 00132 void clear(); 00133 size_t size() const { return m_landmarks.size(); }; 00134 00135 typedef TSequenceLandmarks::const_iterator const_iterator; 00136 const_iterator begin() const { return m_landmarks.begin(); }; 00137 const_iterator end() const { return m_landmarks.end(); }; 00138 00139 /** The object is copied, thus the original copy passed as a parameter can be released. 00140 */ 00141 void push_back( const CLandmark &lm); 00142 CLandmark* get(unsigned int indx); 00143 const CLandmark* get(unsigned int indx) const; 00144 void isToBeModified(unsigned int indx); 00145 void hasBeenModified(unsigned int indx); 00146 void hasBeenModifiedAll(); 00147 void erase(unsigned int indx); 00148 00149 CDynamicGrid<vector_int>* getGrid() { return &m_grid; } 00150 00151 /** Returns the landmark with a given landmrk ID, or NULL if not found 00152 */ 00153 const CLandmark* getByID( CLandmark::TLandmarkID ID ) const; 00154 00155 /** Returns the landmark with a given beacon ID, or NULL if not found 00156 */ 00157 const CLandmark* getByBeaconID( unsigned int ID ) const; 00158 00159 /** This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one). 00160 */ 00161 float getLargestDistanceFromOrigin() const; 00162 00163 } landmarks; 00164 00165 /** Constructor 00166 */ 00167 CLandmarksMap(); 00168 00169 /** Virtual destructor. 00170 */ 00171 virtual ~CLandmarksMap(); 00172 00173 00174 /**** FAMD ***/ 00175 /** Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks 00176 */ 00177 static std::map<std::pair<mrpt::slam::CLandmark::TLandmarkID, mrpt::slam::CLandmark::TLandmarkID>, double> _mEDD; 00178 static mrpt::slam::CLandmark::TLandmarkID _mapMaxID; 00179 static bool _maxIDUpdated; 00180 00181 mrpt::slam::CLandmark::TLandmarkID getMapMaxID(); 00182 /**** END FAMD *****/ 00183 00184 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00185 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00186 * \param otherMap [IN] The other map to compute the matching with. 00187 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00188 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00189 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00190 * 00191 * \return The matching ratio [0,1] 00192 * \sa computeMatchingWith2D 00193 */ 00194 float compute3DMatchingRatio( 00195 const CMetricMap *otherMap, 00196 const CPose3D &otherMapPose, 00197 float minDistForCorr = 0.10f, 00198 float minMahaDistForCorr = 2.0f 00199 ) const; 00200 00201 /** With this struct options are provided to the observation insertion process. 00202 */ 00203 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions 00204 { 00205 public: 00206 /** Initilization of default parameters 00207 */ 00208 TInsertionOptions( ); 00209 00210 /** See utils::CLoadableOptions 00211 */ 00212 void loadFromConfigFile( 00213 const mrpt::utils::CConfigFileBase &source, 00214 const std::string §ion); 00215 00216 /** See utils::CLoadableOptions 00217 */ 00218 void dumpToTextStream(CStream &out) const; 00219 00220 /** If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D features. 00221 */ 00222 bool insert_SIFTs_from_monocular_images; 00223 00224 /** If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D features. 00225 */ 00226 bool insert_SIFTs_from_stereo_images; 00227 00228 /** If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for each range. 00229 */ 00230 bool insert_Landmarks_from_range_scans; 00231 00232 /** [For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as correspondence (Default=0.4) 00233 */ 00234 float SiftCorrRatioThreshold; 00235 00236 /** [For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0.5) 00237 */ 00238 float SiftLikelihoodThreshold; 00239 00240 /****************************************** FAMD ******************************************/ 00241 /** [For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as correspondence (Default=200) 00242 */ 00243 float SiftEDDThreshold; 00244 00245 /** [For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 00246 * 0: Our method -> Euclidean Distance between Descriptors and 3D position 00247 * 1: Sim, Elinas, Griffin, Little -> Euclidean Distance between Descriptors 00248 */ 00249 unsigned int SIFTMatching3DMethod; 00250 00251 /** [For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 00252 * 0: Our method -> Euclidean Distance between Descriptors and 3D position 00253 * 1: Sim, Elinas, Griffin, Little -> 3D position 00254 */ 00255 unsigned int SIFTLikelihoodMethod; 00256 00257 /****************************************** END FAMD ******************************************/ 00258 00259 /** [For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m) 00260 */ 00261 float SIFTsLoadDistanceOfTheMean; 00262 00263 /** [For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perpendicular to the main directiom (Default = 0.05f) 00264 */ 00265 float SIFTsLoadEllipsoidWidth; 00266 00267 /** [For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for the Jacobbian to project stereo images to 3D) 00268 */ 00269 float SIFTs_stdXY, SIFTs_stdDisparity; 00270 00271 /** Number of points to extract in the image 00272 */ 00273 int SIFTs_numberOfKLTKeypoints; 00274 00275 /** Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation. 00276 */ 00277 float SIFTs_stereo_maxDepth; 00278 00279 /** Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential match. 00280 */ 00281 float SIFTs_epipolar_TH; 00282 00283 /** Indicates if the images (as well as the SIFT detected features) should be shown in a window. 00284 */ 00285 bool PLOT_IMAGES; 00286 00287 } insertionOptions; 00288 00289 /** With this struct options are provided to the likelihood computations. 00290 */ 00291 struct MRPTDLLIMPEXP TLikelihoodOptions : public utils::CLoadableOptions 00292 { 00293 public: 00294 /** Initilization of default parameters 00295 */ 00296 TLikelihoodOptions(); 00297 00298 /** See utils::CLoadableOptions 00299 */ 00300 void loadFromConfigFile( 00301 const mrpt::utils::CConfigFileBase &source, 00302 const std::string §ion); 00303 00304 /** See utils::CLoadableOptions 00305 */ 00306 void dumpToTextStream(CStream &out) const; 00307 00308 /** The number of rays from a 2D range scan will be decimated by this factor (default = 1, no decimation) 00309 */ 00310 unsigned int rangeScan2D_decimation; 00311 00312 double SIFTs_sigma_euclidean_dist; 00313 00314 double SIFTs_sigma_descriptor_dist; 00315 00316 float SIFTs_mahaDist_std; 00317 00318 float SIFTnullCorrespondenceDistance; 00319 00320 /** Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood computation. 00321 */ 00322 int SIFTs_decimation; 00323 00324 /** The standard deviation used for Beacon ranges likelihood (default=0.08). 00325 */ 00326 float beaconRangesStd; 00327 00328 /** The ratio between gaussian and uniform distribution (default=1). 00329 */ 00330 float alphaRatio; 00331 00332 /** Maximun reliable beacon range value (default=20) 00333 */ 00334 float beaconMaxRange; 00335 00336 /** This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS observation 00337 * compose with de sensor position on the robot. 00338 */ 00339 struct MRPTDLLIMPEXP TGPSOrigin 00340 { 00341 public: 00342 TGPSOrigin(); 00343 00344 /** Longitud del Origen del GPS (en grados) 00345 */ 00346 double longitude; 00347 00348 /** Latitud del Origen del GPS (en grados) 00349 */ 00350 double latitude; 00351 00352 /** Altitud del Origen del GPS (en metros) 00353 */ 00354 double altitude; 00355 00356 /** Estas tres opciones sirven para encajar mapas de GPS con posiciones absolutas con 00357 * mapas de otros sensores (como laser :D) se obtienen facilmente con el programa matlab map_matching 00358 * ang : Rotación del mapa del GPS (para encajarlo en grados) 00359 * x_shift: Desplazamiento en x relativo al robot (en metros) 00360 * y_shift: Desplazamiento en y relativo al robot (en metros) 00361 */ 00362 double ang, 00363 x_shift, 00364 y_shift; 00365 00366 /** Número mínimo de satelites para tener en cuenta los datos 00367 */ 00368 unsigned int min_sat; 00369 00370 } GPSOrigin; 00371 00372 /** A constant "sigma" for GPS localization data (in meters) 00373 */ 00374 float GPS_sigma; 00375 00376 00377 00378 } likelihoodOptions; 00379 00380 /** This struct stores extra results from invoking insertObservation 00381 */ 00382 struct MRPTDLLIMPEXP TInsertionResults 00383 { 00384 /** The number of SIFT detected in left and right images respectively 00385 */ 00386 00387 unsigned int nSiftL, nSiftR; 00388 00389 00390 } insertionResults; 00391 00392 /** With this struct options are provided to the fusion process. 00393 */ 00394 struct MRPTDLLIMPEXP TFuseOptions 00395 { 00396 /** Initialization 00397 */ 00398 TFuseOptions(); 00399 00400 /** Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds. 00401 */ 00402 unsigned int minTimesSeen; 00403 00404 /** See "minTimesSeen" 00405 */ 00406 float ellapsedTime; 00407 00408 } fuseOptions; 00409 00410 00411 /** Save to a text file. 00412 * In line "i" there are the (x,y,z) mean values of the i'th landmark + type of landmark + # times seen + timestamp + RGB/descriptor + ID 00413 * 00414 * Returns false if any error occured, true elsewere. 00415 */ 00416 bool saveToTextFile(std::string file); 00417 00418 /** Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY plane). 00419 * \param file The name of the file to save the script to. 00420 * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities) 00421 * \param stdCount The ellipsoids will be drawn from the center to "stdCount" times the "standard deviations". (default is 2std = 95% confidence intervals) 00422 * 00423 * \return Returns false if any error occured, true elsewere. 00424 */ 00425 bool saveToMATLABScript2D( 00426 std::string file, 00427 const char *style="b", 00428 float stdCount = 2.0f ); 00429 00430 /** Save to a MATLAB script which displays 3D error ellipses for the map. 00431 * \param file The name of the file to save the script to. 00432 * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities) 00433 * \param stdCount The ellipsoids will be drawn from the center to a given confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95 confidence intervals) 00434 * 00435 * \return Returns false if any error occured, true elsewere. 00436 */ 00437 bool saveToMATLABScript3D( 00438 std::string file, 00439 const char *style="b", 00440 float confInterval = 0.95f ) const ; 00441 00442 /** Clear the map, erasing all landmarks. 00443 */ 00444 void clear(); 00445 00446 /** Returns the stored landmarks count. 00447 */ 00448 size_t size() const; 00449 00450 /** Copy. 00451 */ 00452 //void operator = (const CLandmarksMap& obj); 00453 00454 /** Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map. 00455 * This is the implementation of the algorithm reported in the paper: 00456 <em>J.L. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal, "A Consensus-based Approach for Estimating the Observation Likelihood of Accurate Range Sensors", in IEEE International Conference on Robotics and Automation (ICRA), Rome (Italy), Apr 10-14, 2007</em> 00457 */ 00458 double computeLikelihood_RSLC_2007( const CLandmarksMap *s, const CPose2D &sensorPose); 00459 00460 /** Loads into this landmarks map the SIFT features extracted from an image observation (Previous contents of map will be erased) 00461 * The robot is assumed to be at the origin of the map. 00462 * Some options may be applicable from "insertionOptions" (insertionOptions.SIFTsLoadDistanceOfTheMean) 00463 */ 00464 void loadSiftFeaturesFromImageObservation( const CObservationImage &obs ); 00465 00466 /** Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of stereo-image (Previous contents of map will be erased) 00467 * The robot is assumed to be at the origin of the map. 00468 * Some options may be applicable from "insertionOptions" 00469 */ 00470 void loadSiftFeaturesFromStereoImageObservation( const CObservationStereoImages &obs, mrpt::slam::CLandmark::TLandmarkID fID ); 00471 00472 /** Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous contents of map will be erased) 00473 * \param obs The observation 00474 * \param robotPose The robot pose in the map (Default = the origin) 00475 * Some options may be applicable from "insertionOptions" 00476 */ 00477 void loadOccupancyFeaturesFrom2DRangeScan( 00478 const CObservation2DRangeScan &obs, 00479 const CPose3D *robotPose = NULL, 00480 unsigned int downSampleFactor = 1); 00481 00482 /** Insert the observation information into this map. This method must be implemented 00483 * in derived classes. 00484 * \param obs The observation 00485 * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg) 00486 * 00487 * \sa CObservation::insertObservationInto 00488 */ 00489 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00490 00491 /** Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00492 * 00493 * In the current implementation, this method behaves in a different way according to the nature of 00494 * the observation's class: 00495 * - "mrpt::slam::CObservation2DRangeScan": This calls "computeLikelihood_RSLC_2007". 00496 * - "mrpt::slam::CObservationStereoImages": This calls "computeLikelihood_SIFT_LandmarkMap". 00497 * 00498 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00499 * \param obs The observation. 00500 * \return This method returns a likelihood value > 0. 00501 * 00502 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00503 */ 00504 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00505 00506 void computeMatchingWith2D( 00507 const CMetricMap *otherMap, 00508 const CPose2D &otherMapPose, 00509 float maxDistForCorrespondence, 00510 float maxAngularDistForCorrespondence, 00511 const CPose2D &angularDistPivotPoint, 00512 TMatchingPairList &correspondences, 00513 float &correspondencesRatio, 00514 float *sumSqrDist = NULL, 00515 bool onlyKeepTheClosest = false, 00516 bool onlyUniqueRobust = false ) const; 00517 00518 /** Perform a search for correspondences between "this" and another lansmarks map: 00519 * In this class, the matching is established solely based on the landmarks' IDs. 00520 * \param otherMap [IN] The other map. 00521 * \param correspondences [OUT] The matched pairs between maps. 00522 * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap 00523 * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence. 00524 */ 00525 void computeMatchingWith3DLandmarks( 00526 const mrpt::slam::CLandmarksMap *otherMap, 00527 TMatchingPairList &correspondences, 00528 float &correspondencesRatio, 00529 std::vector<bool> &otherCorrespondences) const; 00530 00531 /** Changes the reference system of the map to a given 3D pose. 00532 */ 00533 void changeCoordinatesReference( const CPose3D &newOrg ); 00534 00535 /** Changes the reference system of the map "otherMap" and save the result in "this" map. 00536 */ 00537 void changeCoordinatesReference( const CPose3D &newOrg, const mrpt::slam::CLandmarksMap *otherMap ); 00538 00539 /** Fuses the contents of another map with this one, updating "this" object with the result. 00540 * This process involves fusing corresponding landmarks, then adding the new ones. 00541 * \param other The other landmarkmap, whose landmarks will be inserted into "this" 00542 * \param justInsertAllOfThem If set to "true", all the landmarks in "other" will be inserted into "this" without checking for possible correspondences (may appear duplicates ones, etc...) 00543 */ 00544 void fuseWith( CLandmarksMap &other, bool justInsertAllOfThem = false ); 00545 00546 /** Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map. 00547 * See paper: JJAA 2006 00548 */ 00549 double computeLikelihood_SIFT_LandmarkMap( CLandmarksMap *map, 00550 TMatchingPairList *correspondences = NULL, 00551 std::vector<bool> *otherCorrespondences = NULL); 00552 00553 /** Returns true if the map is empty/no observation has been inserted. 00554 */ 00555 bool isEmpty() const; 00556 00557 /** Simulates a noisy reading toward each of the beacons in the landmarks map, if any. 00558 * \param in_robotPose This robot pose is used to simulate the ranges to each beacon. 00559 * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot 00560 * \param out_Observations The results will be stored here. NOTICE that the fields "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance" and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before calling this function. 00561 * An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range. 00562 */ 00563 void simulateBeaconReadings( 00564 const CPose3D &in_robotPose, 00565 const CPoint3D &in_sensorLocationOnRobot, 00566 CObservationBeaconRanges &out_Observations ) const; 00567 00568 /** Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the landmarks map, if any. 00569 * \param in_robotPose The robot pose. 00570 * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot 00571 * \param out_Observations The results will be stored here. 00572 * \param sensorDetectsIDs If this is set to false, all the landmarks will be sensed with INVALID_LANDMARK_ID as ID. 00573 * \param in_stdRange The sigma of the sensor noise in range (meters). 00574 * \param in_stdYaw The sigma of the sensor noise in yaw (radians). 00575 * \param in_stdPitch The sigma of the sensor noise in pitch (radians). 00576 * \note The fields "CObservationBearingRange::fieldOfView","CObservationBearingRange::maxSensorDistance" and "CObservationBearingRange::minSensorDistance" MUST BE FILLED OUT before calling this function. 00577 * An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range or field of view- 00578 */ 00579 void simulateRangeBearingReadings( 00580 const CPose3D &in_robotPose, 00581 const CPose3D &in_sensorLocationOnRobot, 00582 CObservationBearingRange &out_Observations, 00583 bool sensorDetectsIDs = true, 00584 const float &in_stdRange = 0.01f, 00585 const float &in_stdYaw = DEG2RAD(0.1f), 00586 const float &in_stdPitch = DEG2RAD(0.1f) 00587 ) const; 00588 00589 00590 /** 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). 00591 * In the case of this class, these files are generated: 00592 * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses. 00593 * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D. 00594 */ 00595 void saveMetricMapRepresentationToFile( 00596 const std::string &filNamePrefix ) const; 00597 00598 /** Returns a 3D object representing the map. 00599 * \sa COLOR_LANDMARKS_IN_3DSCENES 00600 */ 00601 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00602 00603 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00604 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00605 */ 00606 void auxParticleFilterCleanUp(); 00607 00608 }; // End of class def. 00609 00610 00611 } // End of namespace 00612 } // End of namespace 00613 00614 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009 |