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