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 )
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<CPose3D> robotPath;
00062 };
00063
00064
00065 DEFINE_SERIALIZABLE_PRE( CMultiMetricMapPDF )
00066
00067
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:
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
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
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:
00108 CMultiMetricMap averageMap;
00109 bool averageMapIsUpdated;
00110
00113 CSensFrameProbSequence SFs;
00114
00117 std::vector<uint32_t> SF2robotPath;
00118
00119
00122 float H(float p);
00123
00124 public:
00125
00130 struct MRPTDLLIMPEXP TPredictionParams : public utils::CLoadableOptions
00131 {
00134 TPredictionParams();
00135
00138 void loadFromConfigFile(
00139 const mrpt::utils::CConfigFileBase &source,
00140 const std::string §ion);
00141
00144 void dumpToTextStream(
00145 CStream &out);
00146
00155 int pfOptimalProposal_mapSelection;
00156
00157
00160 float ICPGlobalAlign_MinQuality;
00161
00164 COccupancyGridMap2D::TLikelihoodOptions update_gridMapLikelihoodOptions;
00165
00166 TKLDParams KLD_params;
00167
00168 } options;
00169
00172 CMultiMetricMapPDF(
00173 const bayes::CParticleFilter::TParticleFilterOptions &opts = bayes::CParticleFilter::TParticleFilterOptions(),
00174 const mrpt::slam::TSetOfMetricMapInitializers *mapsInitializers = NULL,
00175 const TPredictionParams *predictionOptions = NULL );
00176
00179 virtual ~CMultiMetricMapPDF();
00180
00182 void clear( const CPose2D &initialPose );
00183
00185 void clear( const CPose3D &initialPose );
00186
00190 void getEstimatedPosePDFAtTime(
00191 size_t timeStep,
00192 CPose3DPDFParticles &out_estimation ) const;
00193
00197 void getEstimatedPosePDF( CPose3DPDFParticles &out_estimation ) const;
00198
00201 CMultiMetricMap * getCurrentMetricMapEstimation( );
00202
00205 CMultiMetricMap * getCurrentMostLikelyMetricMap( );
00206
00211 void insertObservation(CSensoryFrame &sf, bool forceInsertion = true);
00212
00216 void getPath(size_t i, std::deque<CPose3D> &out_path) const;
00217
00220 double getCurrentEntropyOfPaths();
00221
00224 double getCurrentJointEntropy();
00225
00228 void updateSensoryFrameSequence();
00229
00232 void saveCurrentPathEstimationToTextFile( const std::string &fil );
00233
00236 float newInfoIndex;
00237
00238 private:
00241 const CPose3D * getLastPose(const size_t &i) const;
00242
00245 void rebuildAverageMap();
00246
00249 struct MRPTDLLIMPEXP TPathBin
00250 {
00251 TPathBin() : x(),y(),phi()
00252 { }
00253
00254 vector_int x,y,phi;
00255
00258 void dumpToStdOut() const;
00259 };
00260
00261
00264 void loadTPathBinFromPath(
00265 CMultiMetricMapPDF::TPathBin &outBin,
00266 std::deque<CPose3D> *path = NULL,
00267 CPose3D *newPose = NULL );
00268
00271 int findTPathBinIntoSet(
00272 TPathBin &desiredBin,
00273 std::deque<TPathBin> &theSet
00274 );
00275
00278 mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb;
00279
00282 mutable vector_double m_maxLikelihood;
00283
00284 CPoseRandomSampler m_movementDrawer;
00285
00288 mutable std::vector<CPose2D> m_movementDrawMaximumLikelihood;
00289
00292 static double particlesEvaluator_AuxPFOptimal(
00293 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00294 const CParticleFilterCapable *obj,
00295 size_t index,
00296 const void *action,
00297 const void *observation );
00298
00301 static double auxiliarComputeObservationLikelihood(
00302 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00303 const CParticleFilterCapable *obj,
00304 size_t particleIndexForMap,
00305 const CSensoryFrame *observation,
00306 const CPose3D *x );
00307
00308
00309
00310 };
00311
00312
00313 }
00314 }
00315
00316 #endif