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