00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00053 struct MRPTDLLIMPEXP TGasConcentrationCell
00054 {
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
00067 float mean, std;
00068
00071 float w,wr;
00072 };
00073
00076 class MRPTDLLIMPEXP CGasConcentrationGridMap2D : public CMetricMap, public utils::CDynamicGrid<TGasConcentrationCell>
00077 {
00078
00079 DEFINE_SERIALIZABLE( CGasConcentrationGridMap2D )
00080 public:
00081
00082 float cell2float(const TGasConcentrationCell& c) const
00083 {
00084 return c.mean;
00085 }
00086
00087
00090 enum TMapRepresentation
00091 {
00092 mrAchim = 0,
00093 mrKalmanFilter,
00094 mrKalmanApproximate
00095 };
00096
00097
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
00111 void clear();
00112
00115 bool isEmpty() const;
00116
00124 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00125
00134 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00135
00143 void saveAsBitmapFile(const std::string &filName) const;
00144
00147 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions
00148 {
00151 TInsertionOptions();
00152
00155 void loadFromConfigFile(
00156 const mrpt::utils::CConfigFileBase &source,
00157 const std::string §ion);
00158
00161 void dumpToTextStream(utils::CStream &out);
00162
00165 float sigma;
00166
00169 float cutoffRadius;
00170
00173 float R_min,R_max;
00174
00177 float KF_covSigma;
00178
00181 float KF_initialCellStd;
00182
00185 float KF_observationModelNoise;
00186
00189 float KF_defaultCellMeanValue;
00190
00192 uint16_t KF_W_size;
00193
00194 } insertionOptions;
00195
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
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
00226 void saveMetricMapRepresentationToFile(
00227 const std::string &filNamePrefix
00228 ) const;
00229
00233 void saveAsMatlab3DGraph(const std::string &filName) const;
00234
00237 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00238
00242 void auxParticleFilterCleanUp();
00243
00246 TMapRepresentation getMapType();
00247
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
00261 TMapRepresentation m_mapType;
00262
00265 CMatrixD m_cov;
00266
00271 void insertObservation_Achim(
00272 float normReading,
00273 const CPose3D &sensorPose );
00274
00279 void insertObservation_KF(
00280 float normReading,
00281 const CPose3D &sensorPose );
00282
00287 void insertObservation_KF2(
00288 float normReading,
00289 const CPose3D &sensorPose );
00290
00293 static float computeAchimCellValue (const TGasConcentrationCell *cell );
00294
00295
00300 CMatrixD m_stackedCov;
00301
00302 mutable bool m_hasToRecoverMeanAndCov;
00303
00307 void recoverMeanAndCov() const;
00308
00309 };
00310
00311
00312 }
00313 }
00314
00315 #endif