00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 poses { class CPose2D; } 00044 00045 namespace slam 00046 { 00047 using namespace mrpt::utils; 00048 using namespace mrpt::poses; 00049 using namespace mrpt::math; 00050 class CObservation; 00051 00052 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CGasConcentrationGridMap2D , CMetricMap, MAPS_IMPEXP ) 00053 00054 /** The contents of each cell in a CGasConcentrationGridMap2D map. 00055 **/ 00056 struct MAPS_IMPEXP TGasConcentrationCell 00057 { 00058 /** Constructor 00059 */ 00060 TGasConcentrationCell( 00061 float Mean = 0.5f, 00062 float Std = 0, 00063 float W = 1e-20f, 00064 float Wr = 0 ) : mean(Mean), std(Std),w(W),wr(Wr) 00065 { 00066 } 00067 00068 /** The mean and std values of the gas concentration for this cell. 00069 */ 00070 float mean, std; 00071 00072 /** The accumulated weight and weighted-readings (See Achim's references) 00073 */ 00074 float w,wr; 00075 }; 00076 00077 /** The content of each m_lastObservations in KF3_deconv estimation 00078 */ 00079 struct MAPS_IMPEXP TdataMap 00080 { 00081 float reading; 00082 mrpt::system::TTimeStamp timestamp; 00083 float k; 00084 CPose3D sensorPose; 00085 float estimation; 00086 float reading_filtered; 00087 float speed; 00088 }; 00089 00090 /** CGasConcentrationGridMap2D represents a PDF of gas concentrations over a 2D area. 00091 **/ 00092 class MAPS_IMPEXP CGasConcentrationGridMap2D : public CMetricMap, public utils::CDynamicGrid<TGasConcentrationCell> 00093 { 00094 typedef utils::CDynamicGrid<TGasConcentrationCell> BASE; 00095 00096 // This must be added to any CSerializable derived class: 00097 DEFINE_SERIALIZABLE( CGasConcentrationGridMap2D ) 00098 public: 00099 00100 /** Calls the base CMetricMap::clear 00101 * Declared here to avoid ambiguity between the two clear() in both base classes. 00102 */ 00103 inline void clear() { CMetricMap::clear(); } 00104 00105 float cell2float(const TGasConcentrationCell& c) const 00106 { 00107 return c.mean; 00108 } 00109 00110 00111 /** The type of map representation to be used. 00112 */ 00113 enum TMapRepresentation 00114 { 00115 mrAchim = 0, 00116 mrKalmanFilter, 00117 mrKalmanApproximate, 00118 mrKalmanApproximate_deconv 00119 }; 00120 00121 00122 /** Constructor 00123 */ 00124 CGasConcentrationGridMap2D( 00125 TMapRepresentation mapType = mrAchim, 00126 float x_min = -2, 00127 float x_max = 2, 00128 float y_min = -2, 00129 float y_max = 2, 00130 float resolution = 0.1 00131 00132 ); 00133 00134 /** Destructor */ 00135 virtual ~CGasConcentrationGridMap2D(); 00136 00137 /** Returns true if the map is empty/no observation has been inserted. 00138 */ 00139 bool isEmpty() const; 00140 00141 00142 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00143 * 00144 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00145 * \param obs The observation. 00146 * \return This method returns a likelihood in the range [0,1]. 00147 * 00148 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00149 */ 00150 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00151 00152 /** Save the current map as a graphical file (BMP,PNG,...). 00153 * The file format will be derived from the file extension (see CImage::saveToFile ) 00154 * It depends on the map representation model: 00155 * mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$ 00156 * mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell. 00157 * mrInformationFilter: Id. 00158 */ 00159 void saveAsBitmapFile(const std::string &filName) const; 00160 00161 00162 /** Parameters related with inserting observations into the map: 00163 */ 00164 struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions 00165 { 00166 /** Default values loader: 00167 */ 00168 TInsertionOptions(); 00169 00170 /** See utils::CLoadableOptions 00171 */ 00172 void loadFromConfigFile( 00173 const mrpt::utils::CConfigFileBase &source, 00174 const std::string §ion); 00175 00176 /** See utils::CLoadableOptions 00177 */ 00178 void dumpToTextStream(CStream &out) const; 00179 00180 /** The sigma of the "Parzen"-kernel Gaussian 00181 */ 00182 float sigma; 00183 00184 /** The cutoff radius for updating cells. 00185 */ 00186 float cutoffRadius; 00187 00188 /** Limits for normalization of sensor readings. 00189 */ 00190 float R_min,R_max; 00191 00192 /** [KF model] The "sigma" for the initial covariance value between cells (in meters). 00193 */ 00194 float KF_covSigma; 00195 00196 /** [KF model] 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). 00197 */ 00198 float KF_initialCellStd; 00199 00200 /** [KF model] The sensor model noise (in normalized concentration units). 00201 */ 00202 float KF_observationModelNoise; 00203 00204 /** [KF model] The default value for the mean of cells' concentration. 00205 */ 00206 float KF_defaultCellMeanValue; 00207 00208 /** [KF2 algorithm] The size of the window of neighbor cells. */ 00209 uint16_t KF_W_size; 00210 00211 /** [KF3 algortihm] The sensor type for the gas concentration map (0x0000 ->mean of all installed sensors, 0x2600, 0x6810, ...) 00212 */ 00213 uint16_t KF_sensorType; 00214 00215 /** Tau values for the rise (tauR) and decay (tauD) sensor's phases. 00216 */ 00217 float tauR; 00218 float tauD; 00219 00220 /** [KF3 algortihm] The last N GasObservations, used for the Kalman Filter with deconvolution estimation. 00221 */ 00222 TdataMap new_Obs, new_ANS; 00223 std::vector<TdataMap> m_lastObservations; 00224 std::vector<TdataMap> antiNoise_window; 00225 00226 /** [KF3 algortihm] The number of observations to keep in m_lastObservations 00227 */ 00228 uint16_t lastObservations_size; 00229 00230 /** [KF3 algortihm] The number of observations used to reduce noise on signal. 00231 */ 00232 uint16_t winNoise_size; 00233 00234 /** [KF3 algortihm] The decimate frecuency applied after noise filtering 00235 */ 00236 uint16_t decimate_value; 00237 00238 /** [KF3 algortihm] Measured values of K= 1/tauD for different volatile concentrations 00239 */ 00240 vector_float tauD_concentration; 00241 vector_float tauD_value; 00242 00243 /** [KF3 algortihm] Measured values of the delay (memory effect) for different robot speeds 00244 */ 00245 vector_float memory_speed; 00246 vector_float memory_delay; 00247 00248 /** [KF3 algortihm] id for the enose used to generate this map (must be < gasGrid_count) 00249 */ 00250 uint16_t enose_id; 00251 00252 /** [KF3 algortihm] true / 1= Save generated gas map as a log file 00253 */ 00254 bool save_maplog; 00255 00256 00257 } insertionOptions; 00258 00259 /** Changes the size of the grid, maintaining previous contents. 00260 * \sa setSize 00261 */ 00262 void resize( float new_x_min, 00263 float new_x_max, 00264 float new_y_min, 00265 float new_y_max, 00266 const TGasConcentrationCell& defaultValueNewCells, 00267 float additionalMarginMeters = 1.0f ); 00268 00269 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00270 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00271 * \param otherMap [IN] The other map to compute the matching with. 00272 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00273 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00274 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00275 * 00276 * \return The matching ratio [0,1] 00277 * \sa computeMatchingWith2D 00278 */ 00279 float compute3DMatchingRatio( 00280 const CMetricMap *otherMap, 00281 const CPose3D &otherMapPose, 00282 float minDistForCorr = 0.10f, 00283 float minMahaDistForCorr = 2.0f 00284 ) const; 00285 00286 00287 /** The implementation in this class just calls all the corresponding method of the contained metric maps. 00288 */ 00289 void saveMetricMapRepresentationToFile( 00290 const std::string &filNamePrefix 00291 ) const; 00292 00293 /** Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell. 00294 * This method can only be called in a KF map model. 00295 */ 00296 void saveAsMatlab3DGraph(const std::string &filName) const; 00297 00298 /** Returns a 3D object representing the map. 00299 */ 00300 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00301 00302 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00303 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00304 */ 00305 void auxParticleFilterCleanUp(); 00306 00307 /** Return the type of the gas distribution map, according to parameters passed on construction. 00308 */ 00309 TMapRepresentation getMapType(); 00310 00311 /** Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form of the expected variance). 00312 * This methods is implemented differently for the different gas map types. 00313 */ 00314 void predictMeasurement( 00315 const double &x, 00316 const double &y, 00317 double &out_predict_response, 00318 double &out_predict_response_variance ); 00319 00320 /** Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based methods). */ 00321 void getMeanAndCov( vector_double &out_means, CMatrixDouble &out_cov) const; 00322 00323 protected: 00324 00325 /** [KF3] Ofstream to save to file option "save_maplog" 00326 */ 00327 std::ofstream *m_debug_dump; 00328 00329 /** [KF3] Decimate value for oversampled enose readings 00330 */ 00331 uint16_t decimate_count; 00332 00333 /** [KF3] To force e-nose samples to have fixed time increments 00334 */ 00335 double fixed_incT; 00336 bool first_incT; 00337 00338 /** The map representation type of this map. 00339 */ 00340 TMapRepresentation m_mapType; 00341 00342 /** The whole covariance matrix, used for the Kalman Filter map representation. 00343 */ 00344 CMatrixD m_cov; 00345 00346 00347 /** The implementation of "insertObservation" for the Achim's map model. 00348 * \param normReading Is a [0,1] normalized concentration reading. 00349 * \param sensorPose Is the sensor pose 00350 */ 00351 void insertObservation_Achim( 00352 float normReading, 00353 const CPose3D &sensorPose ); 00354 00355 /** The implementation of "insertObservation" for the (whole) Kalman Filter map model. 00356 * \param normReading Is a [0,1] normalized concentration reading. 00357 * \param sensorPose Is the sensor pose 00358 */ 00359 void insertObservation_KF( 00360 float normReading, 00361 const CPose3D &sensorPose ); 00362 00363 /** The implementation of "insertObservation" for the Efficient Kalman Filter map model. 00364 * \param normReading Is a [0,1] normalized concentration reading. 00365 * \param sensorPose Is the sensor pose 00366 */ 00367 void insertObservation_KF2( 00368 float normReading, 00369 const CPose3D &sensorPose ); 00370 00371 /** The implementation of "insertObservation" for the Efficient Kalman Filter map model with estimation of real gas concentration using sensor model. 00372 * \param normReading Is a [0,1] normalized concentration reading. 00373 * \param sensorPose Is the sensor pose 00374 */ 00375 void insertObservation_KF3( 00376 float normReading, 00377 CPose3D &sensorPose, 00378 mrpt::system::TTimeStamp timestamp); 00379 00380 /** Estimates the gas concentration based on readings and sensor model 00381 */ 00382 void CGasConcentration_estimation ( 00383 float reading, 00384 const CPose3D &sensorPose, 00385 const mrpt::system::TTimeStamp timestamp); 00386 00387 /** Reduce noise by averaging with a mobile window 00388 */ 00389 void noise_filtering ( 00390 float reading, 00391 const CPose3D &sensorPose, 00392 const mrpt::system::TTimeStamp timestamp ); 00393 00394 /** Save the GAS_MAP generated into a log file for offline representation 00395 */ 00396 void save_log_map( 00397 const mrpt::system::TTimeStamp timestamp, 00398 const float reading, 00399 const float estimation, 00400 const float k, 00401 const double yaw, 00402 const float speed); 00403 00404 /** Computes the average cell concentration, or 0 if it has never been observed: 00405 */ 00406 static float computeAchimCellValue (const TGasConcentrationCell *cell ); 00407 00408 00409 /** The compressed band diagonal matrix for the KF2 implementation. 00410 * The format is a Nx(W^2+2W+1) matrix, one row per cell in the grid map with the 00411 * cross-covariances between each cell and half of the window around it in the grid. 00412 */ 00413 CMatrixD m_stackedCov; 00414 00415 mutable bool m_hasToRecoverMeanAndCov; //!< Only for the KF2 implementation. 00416 00417 /** In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std values. 00418 * \sa m_hasToRecoverMeanAndCov 00419 */ 00420 void recoverMeanAndCov() const; 00421 00422 /** Erase all the contents of the map 00423 */ 00424 virtual void internal_clear(); 00425 00426 /** Insert the observation information into this map. This method must be implemented 00427 * in derived classes. 00428 * \param obs The observation 00429 * \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) 00430 * 00431 * \sa CObservation::insertObservationInto 00432 */ 00433 virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00434 00435 00436 }; 00437 00438 00439 } // End of namespace 00440 } // End of namespace 00441 00442 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
