Main MRPT website > C++ reference
MRPT logo

CHeightGridMap2D.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 
00029 #ifndef CHeightGridMap2D_H
00030 #define CHeightGridMap2D_H
00031 
00032 #include <mrpt/utils/CDynamicGrid.h>
00033 #include <mrpt/utils/CSerializable.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 #include <mrpt/math/geometry.h>
00036 #include <mrpt/system/os.h>
00037 #include <mrpt/utils/CLoadableOptions.h>
00038 #include <mrpt/utils/stl_extensions.h>
00039 
00040 #include <mrpt/slam/CMetricMap.h>
00041 
00042 #include <mrpt/maps/link_pragmas.h>
00043 
00044 namespace mrpt
00045 {
00046         namespace poses
00047         {
00048                 class CPose2D;
00049         }
00050         namespace slam
00051         {
00052                 using namespace mrpt::utils;
00053 
00054                 class CObservation;
00055 
00056                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CHeightGridMap2D, CMetricMap, MAPS_IMPEXP  )
00057 
00058                 /** The contents of each cell in a CHeightGridMap2D map.
00059                  **/
00060                 struct MAPS_IMPEXP THeightGridmapCell
00061                 {
00062                         /** Constructor
00063                           */
00064                         THeightGridmapCell() : h(0), w(0)
00065                         {}
00066 
00067                         /** The current average height (in meters).
00068                           */
00069                         float   h;
00070 
00071                         /** The current standard deviation of the height (in meters).
00072                           */
00073                         float   var;
00074 
00075                         /** Auxiliary variable for storing the incremental mean value (in meters).
00076                           */
00077                         float   u;
00078 
00079                         /** Auxiliary (in meters).
00080                           */
00081                         float   v;
00082 
00083 
00084                         /** [mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for each observation.
00085                           */
00086                         uint32_t        w;
00087                 };
00088 
00089                 /** A mesh representation of a surface which keeps the estimated height for each (x,y) location.
00090                   *  Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes.
00091                   *
00092                   *   Each cell contains the up-to-date average height from measured falling in that cell. Algorithms that can be used:
00093                   *             - mrSimpleAverage: Each cell only stores the current average value.
00094                   */
00095                 class MAPS_IMPEXP CHeightGridMap2D : public CMetricMap, public utils::CDynamicGrid<THeightGridmapCell>
00096                 {
00097                         // This must be added to any CSerializable derived class:
00098                         DEFINE_SERIALIZABLE( CHeightGridMap2D )
00099                 public:
00100 
00101                         /** Calls the base CMetricMap::clear
00102                           * Declared here to avoid ambiguity between the two clear() in both base classes.
00103                           */
00104                         inline void clear() { CMetricMap::clear(); }
00105 
00106                         float cell2float(const THeightGridmapCell& c) const
00107                         {
00108                                 return float(c.h);
00109                         }
00110 
00111                         /** The type of map representation to be used.
00112                           *  See mrpt::slam::CHeightGridMap2D for discussion.
00113                           */
00114                         enum TMapRepresentation
00115                         {
00116                                 mrSimpleAverage = 0
00117 //                              mrSlidingWindow
00118                         };
00119 
00120                         /** Constructor
00121                           */
00122                         CHeightGridMap2D(
00123                                 TMapRepresentation      mapType = mrSimpleAverage,
00124                                 float                           x_min = -2,
00125                                 float                           x_max = 2,
00126                                 float                           y_min = -2,
00127                                 float                           y_max = 2,
00128                                 float                           resolution = 0.1
00129                                 );
00130 
00131                          /** Returns true if the map is empty/no observation has been inserted.
00132                            */
00133                          bool  isEmpty() const;
00134 
00135                         /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
00136                          *
00137                          * \param takenFrom The robot's pose the observation is supposed to be taken from.
00138                          * \param obs The observation.
00139                          * \return This method returns a likelihood in the range [0,1].
00140                          *
00141                          * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
00142                          */
00143                          double  computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00144 
00145                         /** Parameters related with inserting observations into the map.
00146                           */
00147                         struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions
00148                         {
00149                                 /** Default values loader:
00150                                   */
00151                                 TInsertionOptions();
00152 
00153                                 /** See utils::CLoadableOptions
00154                                   */
00155                                 void  loadFromConfigFile(
00156                                         const mrpt::utils::CConfigFileBase  &source,
00157                                         const std::string &section);
00158 
00159                                 /** See utils::CLoadableOptions
00160                                   */
00161                                 void  dumpToTextStream(CStream  &out) const;
00162 
00163                                 /** Wether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the robot for this filter.
00164                                   */
00165                                 bool    filterByHeight;
00166 
00167                                 /** Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter.
00168                                   */
00169                                 float   z_min,z_max;
00170 
00171                                 float   minDistBetweenPointsWhenInserting;      //!< When inserting a scan, a point cloud is first created with this minimum distance between the 3D points (default=0).
00172 
00173                         } insertionOptions;
00174 
00175                         /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
00176                          *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
00177                          * \param  otherMap                                       [IN] The other map to compute the matching with.
00178                          * \param  otherMapPose                           [IN] The 6D pose of the other map as seen from "this".
00179                          * \param  minDistForCorr                         [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
00180                          * \param  minMahaDistForCorr             [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
00181                          *
00182                          * \return The matching ratio [0,1]
00183                          * \sa computeMatchingWith2D
00184                          */
00185                         float  compute3DMatchingRatio(
00186                                         const CMetricMap                                                *otherMap,
00187                                         const CPose3D                                                   &otherMapPose,
00188                                         float                                                                   minDistForCorr = 0.10f,
00189                                         float                                                                   minMahaDistForCorr = 2.0f
00190                                         ) const;
00191 
00192                         /** The implementation in this class just calls all the corresponding method of the contained metric maps.
00193                           */
00194                         void  saveMetricMapRepresentationToFile(
00195                                 const std::string       &filNamePrefix
00196                                 ) const;
00197 
00198                         /** Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless
00199                           *   it is specified otherwise in mrpt::
00200                           */
00201                         void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;
00202 
00203                         /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
00204                           *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
00205                           */
00206                         void  auxParticleFilterCleanUp();
00207 
00208                         /** Return the type of the gas distribution map, according to parameters passed on construction.
00209                           */
00210                         TMapRepresentation       getMapType();
00211 
00212 
00213                         /** Gets the intersection between a 3D line and a Height Grid map (taking into account the different heights of each individual cell).
00214                           */
00215                         bool intersectLine3D(const TLine3D &r1, TObject3D &obj) const;
00216 
00217                         /** Computes the minimum and maximum height in the grid.
00218                           * \return False if there is no observed cell yet.
00219                           */
00220                         bool getMinMaxHeight(float &z_min, float &z_max) const;
00221 
00222                         /** Return the number of cells with at least one height data inserted. */
00223                         size_t countObservedCells() const; 
00224 
00225                 protected:
00226 
00227                         /** The map representation type of this map.
00228                           */
00229                         TMapRepresentation              m_mapType;
00230 
00231                          /** Erase all the contents of the map
00232                           */
00233                          virtual void  internal_clear();
00234 
00235                          /** Insert the observation information into this map. This method must be implemented
00236                           *    in derived classes.
00237                           * \param obs The observation
00238                           * \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)
00239                           *
00240                           * \sa CObservation::insertObservationInto
00241                           */
00242                          virtual bool  internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00243 
00244                 };
00245 
00246 
00247         } // End of namespace
00248 
00249         namespace global_settings
00250         {
00251                 /** If set to true (default), mrpt::slam::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured
00252                   * Affects to:
00253                   *             - CHeightGridMap2D::getAs3DObject
00254                   */
00255                 extern MAPS_IMPEXP bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH;
00256         }
00257 
00258         // Specializations MUST occur at the same namespace:
00259         namespace utils
00260         {
00261                 template <>
00262                 struct TEnumTypeFiller<slam::CHeightGridMap2D::TMapRepresentation>
00263                 {
00264                         typedef slam::CHeightGridMap2D::TMapRepresentation enum_t;
00265                         static void fill(bimap<enum_t,std::string>  &m_map)
00266                         {
00267                                 m_map.insert(slam::CHeightGridMap2D::mrSimpleAverage,     "mrSimpleAverage");
00268                         }
00269                 };
00270         } // End of namespace
00271 
00272 } // End of namespace
00273 
00274 #endif



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