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 §ion); 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.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |