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 PF_implementations_data_H
00029 #define PF_implementations_data_H
00030
00031 #include <mrpt/slam/CActionRobotMovement2D.h>
00032 #include <mrpt/bayes/CParticleFilterData.h>
00033 #include <mrpt/math/lightweight_geom_data.h>
00034 #include <mrpt/poses/CPose3D.h>
00035 #include <mrpt/poses/CPose3DPDFGaussian.h>
00036 #include <mrpt/poses/CPoseRandomSampler.h>
00037 #include <mrpt/slam/TKLDParams.h>
00038
00039 #include <mrpt/slam/link_pragmas.h>
00040
00041 namespace mrpt
00042 {
00043 namespace slam
00044 {
00045 using namespace std;
00046 using namespace mrpt::poses;
00047 using namespace mrpt::bayes;
00048 using namespace mrpt::math;
00049
00050
00051
00052 template <class PARTICLETYPE, class BINTYPE>
00053 void KLF_loadBinFromParticle(
00054 BINTYPE &outBin,
00055 const TKLDParams &opts,
00056 const PARTICLETYPE *currentParticleValue = NULL,
00057 const TPose3D *newPoseToBeInserted = NULL );
00058
00059
00060
00061
00062 template <class PARTICLE_TYPE, class MYSELF>
00063 class PF_implementation
00064 {
00065 public:
00066 PF_implementation() :
00067 m_accumRobotMovement2DIsValid(false),
00068 m_accumRobotMovement3DIsValid(false)
00069 {
00070 }
00071
00072
00073
00074
00075 protected:
00076
00077
00078
00079
00080
00081
00082
00083 CActionRobotMovement2D m_accumRobotMovement2D;
00084 bool m_accumRobotMovement2DIsValid;
00085 CPose3DPDFGaussian m_accumRobotMovement3D;
00086 bool m_accumRobotMovement3DIsValid;
00087
00088 CPoseRandomSampler m_movementDrawer;
00089 mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb;
00090 mutable vector_double m_pfAuxiliaryPFStandard_estimatedProb;
00091 mutable vector_double m_pfAuxiliaryPFOptimal_maxLikelihood;
00092 mutable std::vector<TPose3D> m_pfAuxiliaryPFOptimal_maxLikDrawnMovement;
00093 std::vector<bool> m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed;
00094
00095
00096
00097
00098
00099
00100
00101 template <class BINTYPE>
00102 static double PF_SLAM_particlesEvaluator_AuxPFStandard(
00103 const CParticleFilter::TParticleFilterOptions &PF_options,
00104 const CParticleFilterCapable *obj,
00105 size_t index,
00106 const void * action,
00107 const void * observation );
00108
00109 template <class BINTYPE>
00110 static double PF_SLAM_particlesEvaluator_AuxPFOptimal(
00111 const CParticleFilter::TParticleFilterOptions &PF_options,
00112 const CParticleFilterCapable *obj,
00113 size_t index,
00114 const void *action,
00115 const void *observation );
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 template <class BINTYPE>
00136 void PF_SLAM_implementation_pfAuxiliaryPFOptimal(
00137 const CActionCollection * actions,
00138 const CSensoryFrame * sf,
00139 const CParticleFilter::TParticleFilterOptions &PF_options,
00140 const TKLDParams &KLD_options);
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 template <class BINTYPE>
00153 void PF_SLAM_implementation_pfAuxiliaryPFStandard(
00154 const CActionCollection * actions,
00155 const CSensoryFrame * sf,
00156 const CParticleFilter::TParticleFilterOptions &PF_options,
00157 const TKLDParams &KLD_options);
00158
00159
00160
00161
00162
00163
00164
00165 template <class BINTYPE>
00166 void PF_SLAM_implementation_pfStandardProposal(
00167 const CActionCollection * actions,
00168 const CSensoryFrame * sf,
00169 const CParticleFilter::TParticleFilterOptions &PF_options,
00170 const TKLDParams &KLD_options);
00171
00172
00173
00174
00175 public:
00176
00177
00178
00179
00180 virtual const TPose3D * getLastPose(const size_t i) const = 0;
00181
00182 virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(
00183 PARTICLE_TYPE *particleData,
00184 const TPose3D &newPose) const = 0;
00185
00186
00187
00188
00189
00190
00191
00192 virtual void PF_SLAM_implementation_replaceByNewParticleSet(
00193 typename CParticleFilterData<PARTICLE_TYPE>::CParticleList &old_particles,
00194 const vector<TPose3D> &newParticles,
00195 const vector_double &newParticlesWeight,
00196 const vector<size_t> &newParticlesDerivedFromIdx ) const
00197 {
00198
00199
00200
00201
00202
00203 const size_t N = newParticles.size();
00204 typename MYSELF::CParticleList newParticlesArray(N);
00205
00206
00207
00208 std::vector<bool> oldParticleAlreadyCopied(old_particles.size(),false);
00209
00210 size_t i;
00211 typename MYSELF::CParticleList::iterator newPartIt;
00212 for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();newPartIt++,i++)
00213 {
00214
00215 newPartIt->log_w = newParticlesWeight[i];
00216
00217
00218 PARTICLE_TYPE *newPartData;
00219 if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
00220 {
00221
00222 newPartData = old_particles[ newParticlesDerivedFromIdx[i] ].d;
00223 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
00224 }
00225 else
00226 {
00227
00228 newPartData = new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
00229 }
00230
00231 newPartIt->d = newPartData;
00232 }
00233
00234
00235
00236
00237 for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
00238 PF_SLAM_implementation_custom_update_particle_with_new_pose( newPartIt->d, newParticles[i] );
00239
00240
00241 for (size_t i=0;i<old_particles.size();i++)
00242 if (!oldParticleAlreadyCopied[i])
00243 mrpt::utils::delete_safe( old_particles[ i ].d );
00244
00245
00246 old_particles.resize( newParticlesArray.size() );
00247 typename MYSELF::CParticleList::iterator trgPartIt;
00248 for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
00249 {
00250 trgPartIt->log_w = newPartIt->log_w;
00251 trgPartIt->d = newPartIt->d;
00252 }
00253 }
00254
00255
00256
00257 virtual bool PF_SLAM_implementation_doWeHaveValidObservations(
00258 const typename CParticleFilterData<PARTICLE_TYPE>::CParticleList &particles,
00259 const CSensoryFrame *sf) const
00260 {
00261 return true;
00262 }
00263
00264
00265 virtual bool PF_SLAM_implementation_skipRobotMovement() const
00266 {
00267 return false;
00268 }
00269
00270
00271 virtual double PF_SLAM_computeObservationLikelihoodForParticle(
00272 const CParticleFilter::TParticleFilterOptions &PF_options,
00273 const size_t particleIndexForMap,
00274 const CSensoryFrame &observation,
00275 const CPose3D &x ) const = 0;
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285 template <class BINTYPE>
00286 bool PF_SLAM_implementation_gatherActionsCheckBothActObs(
00287 const CActionCollection * actions,
00288 const CSensoryFrame * sf );
00289
00290 private:
00291
00292 template <class BINTYPE>
00293 void PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
00294 const CActionCollection * actions,
00295 const CSensoryFrame * sf,
00296 const CParticleFilter::TParticleFilterOptions &PF_options,
00297 const TKLDParams &KLD_options,
00298 const bool USE_OPTIMAL_SAMPLING );
00299
00300 template <class BINTYPE>
00301 void PF_SLAM_aux_perform_one_rejection_sampling_step(
00302 const bool USE_OPTIMAL_SAMPLING,
00303 const bool doResample,
00304 const double maxMeanLik,
00305 size_t k,
00306 const CSensoryFrame * sf,
00307 const CParticleFilter::TParticleFilterOptions &PF_options,
00308 CPose3D & out_newPose,
00309 double & out_newParticleLogWeight);
00310
00311
00312 };
00313 }
00314 }
00315
00316 #endif