Main MRPT website > C++ reference
MRPT logo

CLandmarksMap.h

Go to the documentation of this file.
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 &section);
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 &section);
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.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011