Main MRPT website > C++ reference
MRPT logo

CBeaconMap.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 CBeaconMap_H
00029 #define CBeaconMap_H
00030 
00031 #include <mrpt/slam/CMetricMap.h>
00032 #include <mrpt/slam/CBeacon.h>
00033 #include <mrpt/utils/CSerializable.h>
00034 #include <mrpt/math/CMatrix.h>
00035 #include <mrpt/utils/CDynamicGrid.h>
00036 #include <mrpt/utils/CLoadableOptions.h>
00037 
00038 #include <mrpt/maps/link_pragmas.h>
00039 
00040 namespace mrpt
00041 {
00042 namespace slam
00043 {
00044         using namespace mrpt::utils;
00045         using namespace mrpt::math;
00046 
00047         class CObservationBeaconRanges;
00048 
00049 
00050         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CBeaconMap, CMetricMap ,MAPS_IMPEXP )
00051 
00052         /** A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
00053          * <br>
00054          *  The individual beacons are defined as mrpt::slam::CBeacon objects.
00055          * <br>
00056          *  When invoking CBeaconMap::insertObservation(), landmarks will be extracted and fused into the map.
00057          *   The only currently supported observation type is mrpt::slam::CObservationBeaconRanges.
00058          *   See insertionOptions and likelihoodOptions for parameters used when creating and fusing beacon landmarks.
00059          * <br>
00060          *   Use "TInsertionOptions::insertAsMonteCarlo" to select between 2 different behaviors:
00061          *              - Initial PDF of beacons: MonteCarlo, after convergence, pass to Gaussians; or
00062          *              - Initial PDF of beacons: SOG, after convergence, a single Gaussian.
00063          *
00064          *   Refer to the papers: []
00065          *
00066          * \sa CMetricMap
00067          */
00068         class MAPS_IMPEXP CBeaconMap : public CMetricMap
00069         {
00070                 // This must be added to any CSerializable derived class:
00071                 DEFINE_SERIALIZABLE( CBeaconMap )
00072 
00073         public:
00074                 typedef std::deque<CBeacon>                                     TSequenceBeacons;
00075                 typedef std::deque<CBeacon>::iterator                   iterator;
00076                 typedef std::deque<CBeacon>::const_iterator     const_iterator;
00077 
00078         protected:
00079                 /** The individual beacons */
00080                 TSequenceBeacons                m_beacons;
00081 
00082                 /** Clear the map, erasing all landmarks.
00083                  */
00084                 virtual void  internal_clear();
00085 
00086                  /** Insert the observation information into this map. This method must be implemented
00087                   *    in derived classes.
00088                   * \param obs The observation
00089                   * \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)
00090                   *
00091                   * \sa CObservation::insertObservationInto
00092                   */
00093                  virtual bool  internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00094 
00095         public:
00096                 /** Constructor */
00097                 CBeaconMap();
00098 
00099                 void resize(const size_t N); //!< Resize the number of SOG modes
00100 
00101                 /** Access to individual beacons */
00102                 const CBeacon& operator [](size_t i) const {
00103                         ASSERT_(i<m_beacons.size())
00104                         return  m_beacons[i];
00105                 }
00106                 /** Access to individual beacons */
00107                 const CBeacon& get(size_t i) const{
00108                         ASSERT_(i<m_beacons.size())
00109                         return  m_beacons[i];
00110                 }
00111                 /** Access to individual beacons */
00112                 CBeacon& operator [](size_t i) {
00113                         ASSERT_(i<m_beacons.size())
00114                         return  m_beacons[i];
00115                 }
00116                 /** Access to individual beacons */
00117                 CBeacon& get(size_t i)  {
00118                         ASSERT_(i<m_beacons.size())
00119                         return  m_beacons[i];
00120                 }
00121 
00122                 iterator begin() { return m_beacons.begin(); }
00123                 const_iterator begin() const { return m_beacons.begin(); }
00124                 iterator end() { return m_beacons.end(); }
00125                 const_iterator end() const { return m_beacons.end(); }
00126 
00127                 /** Inserts a copy of the given mode into the SOG */
00128                 void push_back(const CBeacon& m) {
00129                         m_beacons.push_back( m );
00130                 }
00131 
00132                 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
00133                  *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
00134                  * \param  otherMap                                       [IN] The other map to compute the matching with.
00135                  * \param  otherMapPose                           [IN] The 6D pose of the other map as seen from "this".
00136                  * \param  minDistForCorr                         [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
00137                  * \param  minMahaDistForCorr             [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
00138                  *
00139                  * \return The matching ratio [0,1]
00140                  * \sa computeMatchingWith2D
00141                  */
00142                 float  compute3DMatchingRatio(
00143                                 const CMetricMap        *otherMap,
00144                                 const CPose3D           &otherMapPose,
00145                                 float                           minDistForCorr = 0.10f,
00146                                 float                           minMahaDistForCorr = 2.0f
00147                                 ) const;
00148 
00149                  /** With this struct options are provided to the likelihood computations.
00150                   */
00151                  struct MAPS_IMPEXP TLikelihoodOptions : public utils::CLoadableOptions
00152                  {
00153                  public:
00154                         /** Initilization of default parameters
00155                          */
00156                          TLikelihoodOptions();
00157 
00158                         /** See utils::CLoadableOptions
00159                           */
00160                         void  loadFromConfigFile(
00161                                 const mrpt::utils::CConfigFileBase      &source,
00162                                 const std::string &section);
00163 
00164                         /** See utils::CLoadableOptions
00165                           */
00166                         void  dumpToTextStream(CStream  &out) const;
00167 
00168                          /** The standard deviation used for Beacon ranges likelihood (default=0.08m).
00169                            */
00170                          float                  rangeStd;
00171 
00172                  } likelihoodOptions;
00173 
00174                  /** This struct contains data for choosing the method by which new beacons are inserted in the map.
00175                   */
00176                  struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions
00177                  {
00178                  public:
00179                         /** Initilization of default parameters
00180                          */
00181                          TInsertionOptions();
00182 
00183                         /** See utils::CLoadableOptions
00184                           */
00185                         void  loadFromConfigFile(
00186                                 const mrpt::utils::CConfigFileBase      &source,
00187                                 const std::string &section);
00188 
00189                         /** See utils::CLoadableOptions
00190                           */
00191                         void  dumpToTextStream(CStream  &out) const;
00192 
00193                         /** Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussians (see mrpt::slam::CBeacon).
00194                           * \sa MC_performResampling
00195                           */
00196                         bool    insertAsMonteCarlo;
00197 
00198                         /** Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation: the default values (both 0), makes the beacons to be in the same horizontal plane that the sensors, that is, 2D SLAM - the min/max values are -90/90.
00199                           */
00200                         float   maxElevation_deg,minElevation_deg;
00201 
00202                         /** Number of particles per meter of range, i.e. per meter of the "radius of the ring".
00203                           */
00204                         unsigned int    MC_numSamplesPerMeter;
00205 
00206                         /** The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (default=0,4).
00207                           */
00208                         float                   MC_maxStdToGauss;
00209 
00210                         /** Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing a given sample (default=5).
00211                           */
00212                         float                   MC_thresholdNegligible;
00213 
00214                         /** If set to false (default), the samples will be generated the first time a beacon is observed, and their weights just updated subsequently - if set to "true", fewer samples will be required since the particles will be resamples when necessary, and a small "noise" will be added to avoid depletion.
00215                           */
00216                         bool                    MC_performResampling;
00217 
00218                         /** The std.dev. of the Gaussian noise to be added to each sample after resampling, only if MC_performResampling=true.
00219                           */
00220                         float                   MC_afterResamplingNoise;
00221 
00222                         /** Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mode (default=20).
00223                           */
00224                         float                   SOG_thresholdNegligible;
00225 
00226                         /** A parameter for initializing 2D/3D SOGs
00227                           */
00228                         float                   SOG_maxDistBetweenGaussians;
00229 
00230                         /** Constant used to compute the std. dev. int the tangent direction when creating the Gaussians.
00231                           */
00232                         float                   SOG_separationConstant;
00233 
00234                  } insertionOptions;
00235 
00236                 /** Save to a MATLAB script which displays 3D error ellipses for the map.
00237                  *      \param file             The name of the file to save the script to.
00238                  *  \param style        The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
00239                  *  \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)
00240                  *
00241                  *  \return Returns false if any error occured, true elsewere.
00242                  */
00243                 bool  saveToMATLABScript3D(
00244                         std::string     file,
00245                         const char      *style="b",
00246                         float           confInterval = 0.95f ) const;
00247 
00248 
00249                 /** Returns the stored landmarks count.
00250                  */
00251                 size_t  size() const;
00252 
00253 
00254                 /** Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map.
00255                  *
00256                  *  In the current implementation, this method behaves in a different way according to the nature of
00257                  *   the observation's class:
00258                  *              - "mrpt::slam::CObservation2DRangeScan": This calls "computeLikelihood_RSLC_2007".
00259                  *              - "mrpt::slam::CObservationStereoImages": This calls "computeLikelihood_SIFT_LandmarkMap".
00260                  *
00261                  * \param takenFrom The robot's pose the observation is supposed to be taken from.
00262                  * \param obs The observation.
00263                  * \return This method returns a likelihood value > 0.
00264                  *
00265                  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
00266                  */
00267                 double   computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00268 
00269                 /** Computes the matchings between this and another 2D points map.
00270                    This includes finding:
00271                                 - The set of points pairs in each map
00272                                 - The mean squared distance between corresponding pairs.
00273                    This method is the most time critical one into the ICP algorithm.
00274 
00275                  * \param  otherMap                                       [IN] The other map to compute the matching with.
00276                  * \param  otherMapPose                           [IN] The pose of the other map as seen from "this".
00277                  * \param  maxDistForCorrespondence   [IN] Maximum 2D linear distance between two points to be matched.
00278                  * \param  maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
00279                  * \param  angularDistPivotPoint      [IN] The point used to calculate distances from in both maps.
00280                  * \param  correspondences                        [OUT] The detected matchings pairs.
00281                  * \param  correspondencesRatio           [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
00282                  * \param  sumSqrDist                             [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
00283                  * \param  covariance                             [OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired.
00284                  * \param  onlyKeepTheClosest         [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
00285                  *
00286                  * \sa compute3DMatchingRatio
00287                  */
00288                 void  computeMatchingWith2D(
00289                                 const CMetricMap                                                *otherMap,
00290                                 const CPose2D                                                   &otherMapPose,
00291                                 float                                                                   maxDistForCorrespondence,
00292                                 float                                                                   maxAngularDistForCorrespondence,
00293                                 const CPose2D                                                   &angularDistPivotPoint,
00294                                 TMatchingPairList                                               &correspondences,
00295                                 float                                                                   &correspondencesRatio,
00296                                 float                                                                   *sumSqrDist     = NULL,
00297                                 bool                                                                    onlyKeepTheClosest = false,
00298                                 bool                                                                    onlyUniqueRobust = false ) const;
00299 
00300                 /** Perform a search for correspondences between "this" and another lansmarks map:
00301                   *  Firsly, the landmarks' descriptor is used to find correspondences, then inconsistent ones removed by
00302                   *    looking at their 3D poses.
00303                   * \param otherMap [IN] The other map.
00304                   * \param correspondences [OUT] The matched pairs between maps.
00305                   * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
00306                   * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
00307                   */
00308                 void  computeMatchingWith3DLandmarks(
00309                                 const mrpt::slam::CBeaconMap                                    *otherMap,
00310                                 TMatchingPairList                                               &correspondences,
00311                                 float                                                                   &correspondencesRatio,
00312                                 std::vector<bool>                                               &otherCorrespondences) const;
00313 
00314                 /** Changes the reference system of the map to a given 3D pose.
00315                   */
00316                 void  changeCoordinatesReference( const CPose3D &newOrg );
00317 
00318                 /** Changes the reference system of the map "otherMap" and save the result in "this" map.
00319                   */
00320                 void  changeCoordinatesReference( const CPose3D &newOrg, const mrpt::slam::CBeaconMap *otherMap );
00321 
00322 
00323                 /** Returns true if the map is empty/no observation has been inserted.
00324                    */
00325                 bool  isEmpty() const;
00326 
00327                 /** Simulates a reading toward each of the beacons in the landmarks map, if any.
00328                   * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
00329                   * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
00330                   * \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.
00331                   * 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.
00332                   */
00333                 void  simulateBeaconReadings(
00334             const CPose3D                                       &in_robotPose,
00335                         const CPoint3D                                  &in_sensorLocationOnRobot,
00336                         CObservationBeaconRanges                &out_Observations ) const;
00337 
00338                 /** 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).
00339                   *  In the case of this class, these files are generated:
00340                   *             - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
00341                   *             - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
00342                   *             - "filNamePrefix"+"_covs.m": A textual representation (see saveToTextFile)
00343                   */
00344                 void  saveMetricMapRepresentationToFile(
00345                         const std::string       &filNamePrefix ) const;
00346 
00347                 /** Save a text file with a row per beacon, containing this 11 elements:
00348                   *  - X Y Z: Mean values
00349                   *  - VX VY VZ: Variances of each dimension (C11, C22, C33)
00350                   *  - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
00351                   *  - C12, C13, C23: Cross covariances
00352                   */
00353                 void saveToTextFile(const std::string &fil) const;
00354 
00355                 /** Returns a 3D object representing the map.
00356                   */
00357                 void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;
00358 
00359                 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
00360                   *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
00361                   */
00362                 void  auxParticleFilterCleanUp();
00363 
00364                 /** Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
00365                   */
00366                 const CBeacon * getBeaconByID( CBeacon::TBeaconID  id ) const;
00367 
00368                 /** Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
00369                   */
00370                 CBeacon * getBeaconByID( CBeacon::TBeaconID  id );
00371 
00372         }; // End of class def.
00373 
00374 
00375         } // End of namespace
00376 } // End of namespace
00377 
00378 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011