00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 00038 namespace mrpt 00039 { 00040 namespace poses { class CPose2D; } 00041 00042 namespace slam 00043 { 00044 using namespace mrpt::utils; 00045 using namespace mrpt::poses; 00046 using namespace mrpt::math; 00047 class CObservation; 00048 00049 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CGasConcentrationGridMap2D , CMetricMap ) 00050 00051 /** The contents of each cell in a CGasConcentrationGridMap2D map. 00052 **/ 00053 struct MRPTDLLIMPEXP TGasConcentrationCell 00054 { 00055 /** Constructor 00056 */ 00057 TGasConcentrationCell( 00058 float Mean = 0.5f, 00059 float Std = 0, 00060 float W = 1e-20f, 00061 float Wr = 0 ) : mean(Mean), std(Std),w(W),wr(Wr) 00062 { 00063 } 00064 00065 /** The mean and std values of the gas concentration for this cell. 00066 */ 00067 float mean, std; 00068 00069 /** The accumulated weight and weighted-readings (See Achim's references) 00070 */ 00071 float w,wr; 00072 }; 00073 00074 /** CGasConcentrationGridMap2D represents a PDF of gas concentrations over a 2D area. 00075 **/ 00076 class MRPTDLLIMPEXP CGasConcentrationGridMap2D : public CMetricMap, public utils::CDynamicGrid<TGasConcentrationCell> 00077 { 00078 // This must be added to any CSerializable derived class: 00079 DEFINE_SERIALIZABLE( CGasConcentrationGridMap2D ) 00080 public: 00081 00082 float cell2float(const TGasConcentrationCell& c) const 00083 { 00084 return c.mean; 00085 } 00086 00087 00088 /** The type of map representation to be used. 00089 */ 00090 enum TMapRepresentation 00091 { 00092 mrAchim = 0, 00093 mrKalmanFilter, 00094 mrKalmanApproximate 00095 }; 00096 00097 00098 /** Constructor 00099 */ 00100 CGasConcentrationGridMap2D( 00101 TMapRepresentation mapType = mrAchim, 00102 float x_min = -2, 00103 float x_max = 2, 00104 float y_min = -2, 00105 float y_max = 2, 00106 float resolution = 0.1 00107 ); 00108 00109 /** Erase all the contents of the map 00110 */ 00111 void clear(); 00112 00113 /** Returns true if the map is empty/no observation has been inserted. 00114 */ 00115 bool isEmpty() const; 00116 00117 /** Insert the observation information into this map. This method must be implemented 00118 * in derived classes. 00119 * \param obs The observation 00120 * \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) 00121 * 00122 * \sa CObservation::insertObservationInto 00123 */ 00124 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00125 00126 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00127 * 00128 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00129 * \param obs The observation. 00130 * \return This method returns a likelihood in the range [0,1]. 00131 * 00132 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00133 */ 00134 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00135 00136 /** Save the current map as a graphical file (BMP,PNG,...). 00137 * The file format will be derived from the file extension (see CImage::saveToFile ) 00138 * It depends on the map representation model: 00139 * mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$ 00140 * mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell. 00141 * mrInformationFilter: Id. 00142 */ 00143 void saveAsBitmapFile(const std::string &filName) const; 00144 00145 /** Parameters related with inserting observations into the map: 00146 */ 00147 struct MRPTDLLIMPEXP 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 §ion); 00158 00159 /** See utils::CLoadableOptions 00160 */ 00161 void dumpToTextStream(CStream &out) const; 00162 00163 /** The sigma of the "Parzen"-kernel Gaussian 00164 */ 00165 float sigma; 00166 00167 /** The cutoff radius for updating cells. 00168 */ 00169 float cutoffRadius; 00170 00171 /** Limits for normalization of sensor readings. 00172 */ 00173 float R_min,R_max; 00174 00175 /** [KF model] The "sigma" for the initial covariance value between cells (in meters). 00176 */ 00177 float KF_covSigma; 00178 00179 /** [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). 00180 */ 00181 float KF_initialCellStd; 00182 00183 /** [KF model] The sensor model noise (in normalized concentration units). 00184 */ 00185 float KF_observationModelNoise; 00186 00187 /** [KF model] The default value for the mean of cells' concentration. 00188 */ 00189 float KF_defaultCellMeanValue; 00190 00191 /** [KF2 algorithm] The size of the window of neighbor cells. */ 00192 uint16_t KF_W_size; 00193 00194 } insertionOptions; 00195 00196 /** Changes the size of the grid, maintaining previous contents. 00197 * \sa setSize 00198 */ 00199 void resize( float new_x_min, 00200 float new_x_max, 00201 float new_y_min, 00202 float new_y_max, 00203 const TGasConcentrationCell& defaultValueNewCells, 00204 float additionalMarginMeters = 1.0f ); 00205 00206 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00207 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00208 * \param otherMap [IN] The other map to compute the matching with. 00209 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00210 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00211 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00212 * 00213 * \return The matching ratio [0,1] 00214 * \sa computeMatchingWith2D 00215 */ 00216 float compute3DMatchingRatio( 00217 const CMetricMap *otherMap, 00218 const CPose3D &otherMapPose, 00219 float minDistForCorr = 0.10f, 00220 float minMahaDistForCorr = 2.0f 00221 ) const; 00222 00223 00224 /** The implementation in this class just calls all the corresponding method of the contained metric maps. 00225 */ 00226 void saveMetricMapRepresentationToFile( 00227 const std::string &filNamePrefix 00228 ) const; 00229 00230 /** Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell. 00231 * This method can only be called in a KF map model. 00232 */ 00233 void saveAsMatlab3DGraph(const std::string &filName) const; 00234 00235 /** Returns a 3D object representing the map. 00236 */ 00237 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00238 00239 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00240 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00241 */ 00242 void auxParticleFilterCleanUp(); 00243 00244 /** Return the type of the gas distribution map, according to parameters passed on construction. 00245 */ 00246 TMapRepresentation getMapType(); 00247 00248 /** Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form of the expected variance). 00249 * This methods is implemented differently for the different gas map types. 00250 */ 00251 void predictMeasurement( 00252 const double &x, 00253 const double &y, 00254 double &out_predict_response, 00255 double &out_predict_response_variance ); 00256 00257 protected: 00258 00259 /** The map representation type of this map. 00260 */ 00261 TMapRepresentation m_mapType; 00262 00263 /** The whole covariance matrix, used for the Kalman Filter map representation. 00264 */ 00265 CMatrixD m_cov; 00266 00267 /** The implementation of "insertObservation" for the Achim's map model. 00268 * \param normReading Is a [0,1] normalized concentration reading. 00269 * \param sensorPose Is the sensor pose 00270 */ 00271 void insertObservation_Achim( 00272 float normReading, 00273 const CPose3D &sensorPose ); 00274 00275 /** The implementation of "insertObservation" for the (whole) Kalman Filter map model. 00276 * \param normReading Is a [0,1] normalized concentration reading. 00277 * \param sensorPose Is the sensor pose 00278 */ 00279 void insertObservation_KF( 00280 float normReading, 00281 const CPose3D &sensorPose ); 00282 00283 /** The implementation of "insertObservation" for the Efficient Kalman Filter map model. 00284 * \param normReading Is a [0,1] normalized concentration reading. 00285 * \param sensorPose Is the sensor pose 00286 */ 00287 void insertObservation_KF2( 00288 float normReading, 00289 const CPose3D &sensorPose ); 00290 00291 /** Computes the average cell concentration, or 0 if it has never been observed: 00292 */ 00293 static float computeAchimCellValue (const TGasConcentrationCell *cell ); 00294 00295 00296 /** The compressed band diagonal matrix for the KF2 implementation. 00297 * The format is a Nx(W^2+2W+1) matrix, one row per cell in the grid map with the 00298 * cross-covariances between each cell and half of the window around it in the grid. 00299 */ 00300 CMatrixD m_stackedCov; 00301 00302 mutable bool m_hasToRecoverMeanAndCov; //!< Only for the KF2 implementation. 00303 00304 /** In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std values. 00305 * \sa m_hasToRecoverMeanAndCov 00306 */ 00307 void recoverMeanAndCov() const; 00308 00309 }; 00310 00311 00312 } // End of namespace 00313 } // End of namespace 00314 00315 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |