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