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 CParticleFilterData_H
00029 #define CParticleFilterData_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/bayes/CProbabilityParticle.h>
00033
00034 #include <algorithm>
00035
00036 namespace mrpt
00037 {
00038 namespace bayes
00039 {
00055 template <class T>
00056 class CParticleFilterData
00057 {
00058 public:
00061 typedef CProbabilityParticle<T> CParticleData;
00062
00065 typedef std::deque<CParticleData> CParticleList;
00066
00069 CParticleList m_particles;
00070
00072 CParticleFilterData() : m_particles(0)
00073 { }
00074
00077 void clearParticles()
00078 {
00079 MRPT_TRY_START;
00080 typename CParticleList::iterator it;
00081 for (it=m_particles.begin();it!=m_particles.end();it++)
00082 if (it->d) delete it->d;
00083 m_particles.clear();
00084 MRPT_TRY_END;
00085 }
00086
00089 virtual ~CParticleFilterData()
00090 {
00091 MRPT_TRY_START;
00092 clearParticles();
00093 MRPT_TRY_END;
00094 }
00095
00099 void writeParticlesToStream( utils::CStream &out ) const
00100 {
00101 MRPT_TRY_START;
00102 uint32_t n = static_cast<uint32_t>(m_particles.size());
00103 out << n;
00104 typename CParticleList::const_iterator it;
00105 for (it=m_particles.begin();it!=m_particles.end();it++)
00106 out << it->log_w << (*it->d);
00107 MRPT_TRY_END;
00108 }
00109
00113 void readParticlesFromStream(utils::CStream &in)
00114 {
00115 MRPT_TRY_START;
00116 clearParticles();
00117 uint32_t n;
00118 in >> n;
00119 m_particles.resize(n);
00120 typename CParticleList::iterator it;
00121 for (it=m_particles.begin();it!=m_particles.end();it++)
00122 {
00123 in >> it->log_w;
00124 it->d = new T();
00125 in >> *it->d;
00126 }
00127 MRPT_TRY_END;
00128 }
00129
00130
00133 void getWeights( vector_double &out_logWeights ) const
00134 {
00135 MRPT_TRY_START;
00136 out_logWeights.resize(m_particles.size());
00137 vector_double::iterator it;
00138 typename CParticleList::const_iterator it2;
00139 for (it=out_logWeights.begin(),it2=m_particles.begin();it2!=m_particles.end();it++,it2++)
00140 *it = it2->log_w;
00141 MRPT_TRY_END;
00142 }
00143
00146 const CParticleData * getMostLikelyParticle() const
00147 {
00148 MRPT_TRY_START
00149 const CParticleData *ret = NULL;
00150 ASSERT_(m_particles.size()>0)
00151
00152 typename CParticleList::const_iterator it;
00153 for (it=m_particles.begin();it!=m_particles.end();it++)
00154 {
00155 if (ret==NULL || it->log_w > ret->log_w)
00156 ret = &(*it);
00157 }
00158 return ret;
00159 MRPT_TRY_END
00160 }
00161
00162
00163 };
00164
00174 #define IMPLEMENT_PARTICLE_FILTER_CAPABLE(T) \
00175 public: \
00176 virtual double getW(size_t i) const \
00177 { \
00178 MRPT_TRY_START; \
00179 if (i>=m_particles.size()) THROW_EXCEPTION_CUSTOM_MSG1("Index %i is out of range!",(int)i); \
00180 return m_particles[i].log_w; \
00181 MRPT_TRY_END; \
00182 } \
00183 virtual void setW(size_t i, double w) \
00184 { \
00185 MRPT_TRY_START; \
00186 if (i>=m_particles.size()) THROW_EXCEPTION_CUSTOM_MSG1("Index %i is out of range!",(int)i); \
00187 m_particles[i].log_w = w; \
00188 MRPT_TRY_END; \
00189 } \
00190 virtual size_t particlesCount() const { return m_particles.size(); } \
00191 virtual double normalizeWeights( double *out_max_log_w = NULL ) \
00192 { \
00193 MRPT_TRY_START; \
00194 CParticleList::iterator it;\
00195 \
00196 if (!m_particles.size()) return 0; \
00197 double minW,maxW; \
00198 minW = maxW = m_particles[0].log_w; \
00199 \
00200 for (it=m_particles.begin();it!=m_particles.end();it++) \
00201 { \
00202 maxW = std::max<double>( maxW, it->log_w ); \
00203 minW = std::min<double>( minW, it->log_w ); \
00204 } \
00205 \
00206 for (it=m_particles.begin();it!=m_particles.end();it++) \
00207 it->log_w -= maxW; \
00208 if (out_max_log_w) \
00209 *out_max_log_w = maxW; \
00210 \
00211 return exp(maxW-minW); \
00212 MRPT_TRY_END; \
00213 } \
00214 virtual double ESS() \
00215 { \
00216 MRPT_TRY_START; \
00217 CParticleList::iterator it; \
00218 double cum = 0; \
00219 \
00220 \
00221 double sumLinearWeights = 0; \
00222 for (it=m_particles.begin();it!=m_particles.end();it++) \
00223 sumLinearWeights += exp( it->log_w ); \
00224 \
00225 for (it=m_particles.begin();it!=m_particles.end();it++) \
00226 cum+= utils::square( exp( it->log_w ) / sumLinearWeights ); \
00227 \
00228 if (cum==0) \
00229 return 0; \
00230 else return 1.0/(m_particles.size()*cum); \
00231 MRPT_TRY_END; \
00232 } \
00233 \
00234 virtual void performSubstitution( const std::vector<size_t> &indx) \
00235 { \
00236 MRPT_TRY_START; \
00237 CParticleList parts; \
00238 CParticleList::iterator itDest,itSrc; \
00239 size_t M_old = m_particles.size(); \
00240 size_t i,j,lastIndxOld = 0; \
00241 std::vector<bool> oldParticlesReused(M_old,false); \
00242 std::vector<bool>::const_iterator oldPartIt; \
00243 std::vector<size_t> sorted_indx(indx); \
00244 std::vector<size_t>::iterator sort_idx_it; \
00245 \
00246 std::sort( sorted_indx.begin(), sorted_indx.end() ); \
00247 \
00248 parts.resize( sorted_indx.size() ); \
00249 for (i=0,itDest=parts.begin();itDest!=parts.end();i++,itDest++) \
00250 { \
00251 const size_t sorted_idx = sorted_indx[i]; \
00252 itDest->log_w = m_particles[ sorted_idx ].log_w; \
00253 \
00254 for (j=lastIndxOld;j<sorted_idx;j++) \
00255 { \
00256 if (!oldParticlesReused[j]) \
00257 { \
00258 delete m_particles[j].d; \
00259 m_particles[j].d = NULL; \
00260 } \
00261 } \
00262 \
00263 lastIndxOld = sorted_idx; \
00264 \
00265 \
00266 if (!oldParticlesReused[sorted_idx]) \
00267 { \
00268 \
00269 parts[i].d = m_particles[ sorted_idx ].d; \
00270 oldParticlesReused[sorted_idx]=true; \
00271 } \
00272 else \
00273 { \
00274 \
00275 ASSERT_( m_particles[ sorted_idx ].d != NULL); \
00276 parts[i].d = new T( *m_particles[ sorted_idx ].d ); \
00277 } \
00278 } \
00279 \
00280 for (itSrc=m_particles.begin(),oldPartIt=oldParticlesReused.begin();itSrc!=m_particles.end();itSrc++,oldPartIt++) \
00281 if (! *oldPartIt ) \
00282 { \
00283 delete itSrc->d; \
00284 itSrc->d = NULL; \
00285 } \
00286 \
00287 m_particles.resize( parts.size() ); \
00288 for (itSrc=parts.begin(),itDest=m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ ) \
00289 { \
00290 itDest->log_w = itSrc->log_w; \
00291 itDest->d = itSrc->d; \
00292 itSrc->d = NULL; \
00293 } \
00294 parts.clear(); \
00295 MRPT_TRY_END; \
00296 } \
00297
00298 }
00299 }
00300 #endif