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 #ifndef CMultiMetricMapPDF_H
00029 #define CMultiMetricMapPDF_H
00030
00031 #include <mrpt/slam/CMultiMetricMap.h>
00032 #include <mrpt/slam/CSensFrameProbSequence.h>
00033 #include <mrpt/poses/CPosePDFParticles.h>
00034 #include <mrpt/poses/CPose3DPDFParticles.h>
00035
00036 #include <mrpt/poses/CPoseRandomSampler.h>
00037
00038 #include <mrpt/bayes/CParticleFilterCapable.h>
00039 #include <mrpt/bayes/CParticleFilterData.h>
00040 #include <mrpt/utils/CLoadableOptions.h>
00041
00042 namespace mrpt
00043 {
00044 namespace slam
00045 {
00046 DEFINE_SERIALIZABLE_PRE( CRBPFParticleData )
00047
00048
00049 class MRPTDLLIMPEXP CRBPFParticleData : public mrpt::utils::CSerializable
00050 {
00051
00052 DEFINE_SERIALIZABLE( CRBPFParticleData )
00053 public:
00054 CRBPFParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
00055 mapTillNow( mapsInitializers ),
00056 robotPath()
00057 {
00058 }
00059
00060 CMultiMetricMap mapTillNow;
00061 std::deque<TPose3D> robotPath;
00062 };
00063
00064
00065 DEFINE_SERIALIZABLE_PRE( CMultiMetricMapPDF )
00066
00067
00068
00069
00070
00071
00072 class MRPTDLLIMPEXP CMultiMetricMapPDF : public mrpt::utils::CSerializable, public mrpt::bayes::CParticleFilterCapable, public mrpt::bayes::CParticleFilterData<CRBPFParticleData>
00073 {
00074 friend class CMetricMapBuilderRBPF;
00075
00076
00077 DEFINE_SERIALIZABLE( CMultiMetricMapPDF )
00078
00079
00080 IMPLEMENT_PARTICLE_FILTER_CAPABLE(CRBPFParticleData);
00081
00082 protected:
00083
00084
00085 void prediction_and_update_pfStandardProposal(
00086 const mrpt::slam::CActionCollection * action,
00087 const mrpt::slam::CSensoryFrame * observation,
00088 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00089
00090
00091
00092 void prediction_and_update_pfOptimalProposal(
00093 const mrpt::slam::CActionCollection * action,
00094 const mrpt::slam::CSensoryFrame * observation,
00095 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00096
00097
00098
00099 void prediction_and_update_pfAuxiliaryPFOptimal(
00100 const mrpt::slam::CActionCollection * action,
00101 const mrpt::slam::CSensoryFrame * observation,
00102 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00103
00104
00105 private:
00106
00107
00108 CMultiMetricMap averageMap;
00109 bool averageMapIsUpdated;
00110
00111
00112
00113 CSensFrameProbSequence SFs;
00114
00115
00116
00117 std::vector<uint32_t> SF2robotPath;
00118
00119
00120
00121
00122 float H(float p);
00123
00124 public:
00125
00126
00127
00128
00129
00130 struct MRPTDLLIMPEXP TPredictionParams : public utils::CLoadableOptions
00131 {
00132
00133
00134 TPredictionParams();
00135
00136
00137
00138 void loadFromConfigFile(
00139 const mrpt::utils::CConfigFileBase &source,
00140 const std::string §ion);
00141
00142
00143
00144 void dumpToTextStream(CStream &out) const;
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154 int pfOptimalProposal_mapSelection;
00155
00156
00157
00158
00159 float ICPGlobalAlign_MinQuality;
00160
00161
00162
00163 COccupancyGridMap2D::TLikelihoodOptions update_gridMapLikelihoodOptions;
00164
00165 TKLDParams KLD_params;
00166
00167 } options;
00168
00169
00170
00171 CMultiMetricMapPDF(
00172 const bayes::CParticleFilter::TParticleFilterOptions &opts = bayes::CParticleFilter::TParticleFilterOptions(),
00173 const mrpt::slam::TSetOfMetricMapInitializers *mapsInitializers = NULL,
00174 const TPredictionParams *predictionOptions = NULL );
00175
00176
00177
00178 virtual ~CMultiMetricMapPDF();
00179
00180
00181 void clear( const CPose2D &initialPose );
00182
00183
00184 void clear( const CPose3D &initialPose );
00185
00186
00187
00188
00189 void getEstimatedPosePDFAtTime(
00190 size_t timeStep,
00191 CPose3DPDFParticles &out_estimation ) const;
00192
00193
00194
00195
00196 void getEstimatedPosePDF( CPose3DPDFParticles &out_estimation ) const;
00197
00198
00199
00200 CMultiMetricMap * getCurrentMetricMapEstimation( );
00201
00202
00203
00204 CMultiMetricMap * getCurrentMostLikelyMetricMap( );
00205
00206
00207
00208
00209 void insertObservation(CSensoryFrame &sf);
00210
00211
00212
00213
00214 void getPath(size_t i, std::deque<math::TPose3D> &out_path) const;
00215
00216
00217
00218 double getCurrentEntropyOfPaths();
00219
00220
00221
00222 double getCurrentJointEntropy();
00223
00224
00225
00226 void updateSensoryFrameSequence();
00227
00228
00229
00230 void saveCurrentPathEstimationToTextFile( const std::string &fil );
00231
00232
00233
00234 float newInfoIndex;
00235
00236 private:
00237
00238
00239 const TPose3D * getLastPose(const size_t &i) const;
00240
00241
00242
00243 void rebuildAverageMap();
00244
00245
00246
00247 struct MRPTDLLIMPEXP TPathBin
00248 {
00249 TPathBin() : x(),y(),phi()
00250 { }
00251
00252 vector_int x,y,phi;
00253
00254
00255
00256 void dumpToStdOut() const;
00257 };
00258
00259
00260
00261
00262 void loadTPathBinFromPath(
00263 CMultiMetricMapPDF::TPathBin &outBin,
00264 std::deque<TPose3D> *path = NULL,
00265 CPose3D *newPose = NULL );
00266
00267
00268
00269 int findTPathBinIntoSet(
00270 TPathBin &desiredBin,
00271 std::deque<TPathBin> &theSet
00272 );
00273
00274
00275
00276 mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb;
00277
00278
00279
00280 mutable vector_double m_maxLikelihood;
00281
00282 CPoseRandomSampler m_movementDrawer;
00283
00284
00285
00286 mutable std::vector<CPose2D> m_movementDrawMaximumLikelihood;
00287
00288
00289
00290 static double particlesEvaluator_AuxPFOptimal(
00291 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00292 const CParticleFilterCapable *obj,
00293 size_t index,
00294 const void *action,
00295 const void *observation );
00296
00297
00298
00299 static double auxiliarComputeObservationLikelihood(
00300 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00301 const CParticleFilterCapable *obj,
00302 size_t particleIndexForMap,
00303 const CSensoryFrame *observation,
00304 const CPose3D *x );
00305
00306
00307
00308 };
00309
00310
00311 }
00312 }
00313
00314 #endif