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