Main MRPT website > C++ reference
MRPT logo

CGasConcentrationGridMap2D.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 CGasConcentrationGridMap2D_H
00030 #define CGasConcentrationGridMap2D_H
00031 
00032 #include <mrpt/utils/CDynamicGrid.h>
00033 #include <mrpt/utils/CSerializable.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 #include <mrpt/utils/CLoadableOptions.h>
00036 #include <mrpt/slam/CMetricMap.h>
00037 #include <mrpt/slam/CObservationGasSensors.h>
00038 
00039 #include <mrpt/maps/link_pragmas.h>
00040 
00041 namespace mrpt
00042 {
00043 namespace slam
00044 {
00045         using namespace mrpt::utils;
00046         using namespace mrpt::poses;
00047         using namespace mrpt::math;
00048 
00049         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CGasConcentrationGridMap2D , CMetricMap, MAPS_IMPEXP )
00050 
00051         // Pragma defined to ensure no structure packing: since we'll serialize TGasConcentrationCell to streams, we want it not to depend on compiler options, etc.
00052 #pragma pack(push,1)
00053 
00054         /** The contents of each cell in a CGasConcentrationGridMap2D map.
00055          **/
00056         struct MAPS_IMPEXP TGasConcentrationCell
00057         {
00058                 /** Constructor */
00059                 TGasConcentrationCell(double kfmean_dm_mean = 1e-20, double kfstd_dmmeanw = 0) :
00060                         kf_mean      (kfmean_dm_mean),
00061                         kf_std       (kfstd_dmmeanw),
00062                         dmv_var_mean (0)
00063                 { }
00064 
00065                 // *Note*: Use unions to share memory between data fields, since only a set
00066                 //          of the variables will be used for each mapping strategy.
00067                 // You can access to a "TGasConcentrationCell *cell" like: cell->kf_mean, cell->kf_std, etc..
00068                 //  but accessing cell->kf_mean would also modify (i.e. ARE the same memory slot) cell->dm_mean, for example.
00069 
00070                 union
00071                 {
00072                         double kf_mean; //!< [KF-methods only] The mean value of this cell
00073                         double dm_mean; //!< [Kernel-methods only] The cumulative weighted readings of this cell
00074 
00075                 };
00076                 union
00077                 {
00078                         double kf_std;    //!< [KF-methods only] The standard deviation value of this cell
00079                         double dm_mean_w; //!< [Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)*r0 )
00080                 };
00081 
00082                 double dmv_var_mean;   //!< [Kernel DM-V only] The cumulative weighted variance of this cell
00083         };
00084 #pragma pack(pop)
00085 
00086         /** CGasConcentrationGridMap2D represents a PDF of gas concentrations over a 2D area.
00087           *
00088           *  There are a number of methods available to build the gas grid-map, depending on the value of
00089           *    "TMapRepresentation maptype" passed in the constructor.
00090           *
00091           *  The following papers describe the mapping alternatives implemented here:
00092           *             - mrKernelDM: A kernel-based method:
00093           *             "Building gas concentration gridmaps with a mobile robot", Lilienthal, A. and Duckett, T., Robotics and Autonomous Systems, v.48, 2004.
00094           *
00095           *             - mrKernelDMV: A kernel-based method:
00096           *             "A Statistical Approach to Gas Distribution Modelling with Mobile Robots--The Kernel DM+ V Algorithm"
00097           *       , Lilienthal, A.J. and Reggente, M. and Trincavelli, M. and Blanco, J.L. and Gonzalez, J., IROS 2009.
00098           *
00099           * \sa mrpt::slam::CMetricMap, mrpt::utils::CDynamicGrid, The application icp-slam, mrpt::slam::CMultiMetricMap
00100           */
00101         class MAPS_IMPEXP CGasConcentrationGridMap2D : public CMetricMap, public utils::CDynamicGrid<TGasConcentrationCell>
00102         {
00103                 typedef utils::CDynamicGrid<TGasConcentrationCell> BASE;
00104 
00105                 // This must be added to any CSerializable derived class:
00106                 DEFINE_SERIALIZABLE( CGasConcentrationGridMap2D )
00107         public:
00108 
00109                 /** Calls the base CMetricMap::clear
00110                   * Declared here to avoid ambiguity between the two clear() in both base classes.
00111                   */
00112                 inline void clear() { CMetricMap::clear(); }
00113 
00114                 // This method is just used for the ::saveToTextFile() method in base class.
00115                 float cell2float(const TGasConcentrationCell& c) const
00116                 {
00117                         return c.kf_mean;
00118                 }
00119 
00120                 /** The type of map representation to be used.
00121                   */
00122                 enum TMapRepresentation
00123                 {
00124                         mrKernelDM = 0,   //
00125                         mrAchim = 0,      // Another alias for "mrKernelDM", for backward compatibility
00126                         mrKalmanFilter,
00127                         mrKalmanApproximate,
00128                         mrKernelDMV
00129                 };
00130 
00131                 /** Constructor
00132                   */
00133                 CGasConcentrationGridMap2D(
00134                         TMapRepresentation      mapType = mrAchim,
00135             float                               x_min = -2,
00136                         float                           x_max = 2,
00137                         float                           y_min = -2,
00138                         float                           y_max = 2,
00139                         float                           resolution = 0.1
00140                         );
00141 
00142                 /** Destructor */
00143                 virtual ~CGasConcentrationGridMap2D();
00144 
00145                  /** Returns true if the map is empty/no observation has been inserted.
00146                    */
00147                  bool  isEmpty() const;
00148 
00149 
00150                 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
00151                  *
00152                  * \param takenFrom The robot's pose the observation is supposed to be taken from.
00153                  * \param obs The observation.
00154                  * \return This method returns a likelihood in the range [0,1].
00155                  *
00156                  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
00157                  */
00158                  double  computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00159 
00160                 /** Save the current map as a graphical file (BMP,PNG,...).
00161                   * The file format will be derived from the file extension (see  CImage::saveToFile )
00162                   *  It depends on the map representation model:
00163                   *             mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$
00164                   *             mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell.
00165                   *             mrInformationFilter:  Id.
00166                   */
00167                 void  saveAsBitmapFile(const std::string  &filName) const;
00168 
00169 
00170                 /** Parameters related with inserting observations into the map:
00171                   */
00172                 struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions
00173                 {
00174                         TInsertionOptions();    //!< Default values loader
00175 
00176                         /** See utils::CLoadableOptions */
00177                         void  loadFromConfigFile(
00178                                 const mrpt::utils::CConfigFileBase  &source,
00179                                 const std::string &section);
00180 
00181                         void  dumpToTextStream(CStream  &out) const; //!< See utils::CLoadableOptions
00182 
00183                         /** @name For all mapping methods
00184                             @{ */
00185                         uint16_t sensorType;    //!< The sensor type for the gas concentration map (0x0000 ->mean of all installed sensors, 0x2600, 0x6810, ...)
00186                         /** @} */
00187 
00188                         /** @name Kernel methods (mrKernelDM, mrKernelDMV)
00189                             @{ */
00190                         float   sigma;  //!< The sigma of the "Parzen"-kernel Gaussian
00191                         float   cutoffRadius;   //!< The cutoff radius for updating cells.
00192             float       R_min,R_max;    //!< Limits for normalization of sensor readings.
00193                         double  dm_sigma_omega; //!< [DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; see CGasConcentrationGridMap2D) */
00194                         /** @} */
00195 
00196                         /** @name Kalman-filter methods (mrKalmanFilter, mrKalmanApproximate)
00197                             @{ */
00198                         float   KF_covSigma;    //!< The "sigma" for the initial covariance value between cells (in meters).
00199                         float   KF_initialCellStd;      //!< The initial standard deviation of each cell's concentration (will be stored both at each cell's structure and in the covariance matrix as variances in the diagonal) (in normalized concentration units).
00200                         float   KF_observationModelNoise;       //!< The sensor model noise (in normalized concentration units).
00201                         float   KF_defaultCellMeanValue;        //!< The default value for the mean of cells' concentration.
00202                         uint16_t        KF_W_size;      //!< [mrKalmanApproximate] The size of the window of neighbor cells.
00203                         /** @} */
00204 
00205                         /** @name Parameters of the "MOS model"
00206                             @{ */
00207                         bool useMOSmodel;       //!< If true use MOS model before map algorithm
00208 
00209                         float   tauR;   //!< Tau values for the rise sensor's phases.
00210                         float   tauD;   //!< Tau values for the decay (tauD) sensor's phases.
00211 
00212                         uint16_t lastObservations_size; //!< The number of observations to keep in m_lastObservations
00213                         size_t  winNoise_size;  //!< The number of observations used to reduce noise on signal.
00214                         uint16_t        decimate_value; //!< The decimate frecuency applied after noise filtering
00215 
00216                         vector_float calibrated_tauD_voltages;  //!< Measured values of K= 1/tauD for different volatile concentrations
00217                         vector_float calibrated_tauD_values;
00218 
00219                         vector_float calibrated_delay_RobotSpeeds;      //!< Measured values of the delay (memory effect) for different robot speeds
00220                         vector_float calibrated_delay_values;
00221 
00222                         uint16_t enose_id;      //!< id for the enose used to generate this map (must be < gasGrid_count)
00223                         bool save_maplog;       //!< If true save generated gas map as a log file
00224                         /** @} */ // end: Parameters of the "MOS model"
00225 
00226                 } insertionOptions;
00227 
00228                 /** Changes the size of the grid, maintaining previous contents.
00229                   * \sa setSize
00230                   */
00231                 void  resize(           float   new_x_min,
00232                                                                         float   new_x_max,
00233                                                                         float   new_y_min,
00234                                                                         float   new_y_max,
00235                                                                         const TGasConcentrationCell& defaultValueNewCells,
00236                                                                         float   additionalMarginMeters = 1.0f );
00237 
00238                 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
00239                  *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
00240                  * \param  otherMap                                       [IN] The other map to compute the matching with.
00241                  * \param  otherMapPose                           [IN] The 6D pose of the other map as seen from "this".
00242                  * \param  minDistForCorr                         [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
00243                  * \param  minMahaDistForCorr             [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
00244                  *
00245                  * \return The matching ratio [0,1]
00246                  * \sa computeMatchingWith2D
00247                  */
00248                 float  compute3DMatchingRatio(
00249                                 const CMetricMap                                                *otherMap,
00250                                 const CPose3D                                                   &otherMapPose,
00251                                 float                                                                   minDistForCorr = 0.10f,
00252                                 float                                                                   minMahaDistForCorr = 2.0f
00253                                 ) const;
00254 
00255 
00256                 /** The implementation in this class just calls all the corresponding method of the contained metric maps.
00257                   */
00258                 void  saveMetricMapRepresentationToFile(
00259                         const std::string       &filNamePrefix
00260                         ) const;
00261 
00262                 /** Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell.
00263                   *  This method can only be called in a KF map model.
00264                   */
00265                 void  saveAsMatlab3DGraph(const std::string  &filName) const;
00266 
00267                 /** Returns a 3D object representing the map.
00268                   */
00269                 void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;
00270 
00271                 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
00272                   *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
00273                   */
00274                 void  auxParticleFilterCleanUp();
00275 
00276                 /** Return the type of the gas distribution map, according to parameters passed on construction.
00277                   */
00278                 TMapRepresentation       getMapType();
00279 
00280                 /** Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form of the expected variance).
00281                   *  This methods is implemented differently for the different gas map types.
00282                   */
00283                 void predictMeasurement(
00284                         const double    &x,
00285                         const double    &y,
00286                         double                  &out_predict_response,
00287                         double                  &out_predict_response_variance );
00288 
00289                 /** Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based methods). */
00290                 void getMeanAndCov( vector_double &out_means, CMatrixDouble &out_cov) const;
00291 
00292         protected:
00293 
00294                 /** The content of each m_lastObservations in the estimation when using the option : MOS_MODEl (insertionOptions.useMOSmodel =1)
00295                         */
00296                 struct MAPS_IMPEXP TdataMap
00297                 {
00298                                 float                                           reading;
00299                                 mrpt::system::TTimeStamp        timestamp;
00300                                 float                                           k;
00301                                 CPose3D                                         sensorPose;
00302                                 float                                           estimation;
00303                                 float                                           reading_filtered;
00304                                 float                                           speed;
00305                 };
00306 
00307                 /** [useMOSmodel] The last N GasObservations, used for the MOS MODEL estimation. */
00308                 TdataMap m_new_Obs, m_new_ANS;
00309                 std::vector<TdataMap> m_lastObservations;
00310                 std::vector<TdataMap> m_antiNoise_window;
00311 
00312                 /** [useMOSmodel] Ofstream to save to file option "save_maplog"
00313                   */
00314                 std::ofstream                   *m_debug_dump;
00315 
00316                 /** [useMOSmodel] Decimate value for oversampled enose readings
00317                   */
00318                 uint16_t                                decimate_count;
00319 
00320                 /** [useMOSmodel] To force e-nose samples to have fixed time increments
00321                   */
00322                 double                                  fixed_incT;
00323                 bool                                    first_incT;
00324 
00325                 /** The map representation type of this map, as passed in the constructor */
00326                 TMapRepresentation      m_mapType;
00327 
00328                 CMatrixD                                m_cov;  //!< The whole covariance matrix, used for the Kalman Filter map representation.
00329 
00330                 /** The compressed band diagonal matrix for the KF2 implementation.
00331                   *   The format is a Nx(W^2+2W+1) matrix, one row per cell in the grid map with the
00332                   *    cross-covariances between each cell and half of the window around it in the grid.
00333                   */
00334                 CMatrixD                m_stackedCov;
00335 
00336                 mutable bool    m_hasToRecoverMeanAndCov;       //!< Only for the KF2 implementation.
00337 
00338                 /** @name Auxiliary vars for DM & DM+V methods
00339                     @{ */
00340                 float               m_DM_lastCutOff;
00341                 std::vector<float>      m_DM_gaussWindow;
00342                 double                          m_average_normreadings_mean, m_average_normreadings_var;
00343                 size_t              m_average_normreadings_count;
00344                 /** @} */
00345 
00346                 /** The implementation of "insertObservation" for Achim Lilienthal's map models DM & DM+V.
00347                   * \param normReading Is a [0,1] normalized concentration reading.
00348                   * \param sensorPose Is the sensor pose on the robot
00349                   * \param is_DMV = false -> map type is Kernel DM; true -> map type is DM+V
00350                   */
00351                 void  insertObservation_KernelDM_DMV(
00352                         float            normReading,
00353                         const CPose3D   &sensorPose,
00354                         bool             is_DMV );
00355 
00356                 /** The implementation of "insertObservation" for the (whole) Kalman Filter map model.
00357                   * \param normReading Is a [0,1] normalized concentration reading.
00358                   * \param sensorPose Is the sensor pose
00359                   */
00360                 void  insertObservation_KF(
00361                         float                   normReading,
00362                         const CPose3D   &sensorPose );
00363 
00364                 /** The implementation of "insertObservation" for the Efficient Kalman Filter map model.
00365                   * \param normReading Is a [0,1] normalized concentration reading.
00366                   * \param sensorPose Is the sensor pose
00367                   */
00368                 void  insertObservation_KF2(
00369                         float                   normReading,
00370                         const CPose3D   &sensorPose );
00371 
00372 
00373                 /** Estimates the gas concentration based on readings and sensor model
00374                   */
00375                 void CGasConcentration_estimation (
00376                         float                                                   reading,
00377                         const CPose3D                                   &sensorPose,
00378                         const mrpt::system::TTimeStamp  timestamp);
00379 
00380                 /** Reduce noise by averaging with a mobile window
00381                   */
00382                 void noise_filtering (
00383                         float   reading,
00384                         const   CPose3D &sensorPose,
00385                         const   mrpt::system::TTimeStamp timestamp );
00386 
00387                 /** Save the GAS_MAP generated into a log file for offline representation
00388                   */
00389                 void save_log_map(
00390                         const mrpt::system::TTimeStamp  timestamp,
00391                         const float                                             reading,
00392                         const float                                             estimation,
00393                         const float                                             k,
00394                         const double                                    yaw,
00395                         const float                                             speed);
00396 
00397                 /** Computes the average cell concentration, or the overall average value if it has never been observed  */
00398                 double computeMeanCellValue_DM_DMV (const TGasConcentrationCell *cell ) const;
00399 
00400                 /** Computes the estimated variance of the cell concentration, or the overall average variance if it has never been observed  */
00401                 double computeVarCellValue_DM_DMV (const TGasConcentrationCell *cell ) const;
00402 
00403 
00404                 /** In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std values.
00405                   * \sa m_hasToRecoverMeanAndCov
00406                   */
00407                 void  recoverMeanAndCov() const;
00408 
00409                  /** Erase all the contents of the map
00410                   */
00411                  virtual void  internal_clear();
00412 
00413                  /** Insert the observation information into this map. This method must be implemented
00414                   *    in derived classes.
00415                   * \param obs The observation
00416                   * \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)
00417                   *
00418                   * \sa CObservation::insertObservationInto
00419                   */
00420                  virtual bool  internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00421 
00422 
00423         };
00424 
00425 
00426         } // End of namespace
00427 
00428 
00429         // Specializations MUST occur at the same namespace:
00430         namespace utils
00431         {
00432                 template <>
00433                 struct TEnumTypeFiller<slam::CGasConcentrationGridMap2D::TMapRepresentation>
00434                 {
00435                         typedef slam::CGasConcentrationGridMap2D::TMapRepresentation enum_t;
00436                         static void fill(bimap<enum_t,std::string>  &m_map)
00437                         {
00438                                 m_map.insert(slam::CGasConcentrationGridMap2D::mrKernelDM,          "mrKernelDM");
00439                                 m_map.insert(slam::CGasConcentrationGridMap2D::mrKalmanFilter,      "mrKalmanFilter");
00440                                 m_map.insert(slam::CGasConcentrationGridMap2D::mrKalmanApproximate, "mrKalmanApproximate");
00441                                 m_map.insert(slam::CGasConcentrationGridMap2D::mrKernelDMV,         "mrKernelDMV");
00442                         }
00443                 };
00444         } // End of namespace
00445 } // End of namespace
00446 
00447 #endif



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