|
| 1 | +#include "opencv2/optim.hpp" |
| 2 | +#include "opencv2/core/core_c.h" |
| 3 | +#include <algorithm> |
| 4 | +#include <typeinfo> |
| 5 | +#include <cmath> |
| 6 | +#define WEIGHTED |
| 7 | + |
| 8 | +namespace cv{ |
| 9 | + |
| 10 | + //!particle filtering class |
| 11 | + class PFSolver : public optim::Solver{ |
| 12 | + public: |
| 13 | + class Function : public optim::Solver::Function |
| 14 | + { |
| 15 | + public: |
| 16 | + //!if parameters have no sense due to some reason (e.g. lie outside of function domain), this function "corrects" them, |
| 17 | + //!that is brings to the function domain |
| 18 | + virtual void correctParams(double* /*optParams*/)const{} |
| 19 | + //!is used when there is a dependence on the number of iterations done in calc(), note that levels are counted starting from 1 |
| 20 | + virtual void setLevel(int /*level*/, int /*levelsNum*/){} |
| 21 | + }; |
| 22 | + PFSolver(); |
| 23 | + void getOptParam(OutputArray params)const; |
| 24 | + int iteration(); |
| 25 | + double minimize(InputOutputArray x); |
| 26 | + |
| 27 | + void setParticlesNum(int num); |
| 28 | + int getParticlesNum(); |
| 29 | + void setAlpha(double AlphaM); |
| 30 | + double getAlpha(); |
| 31 | + void getParamsSTD(OutputArray std)const; |
| 32 | + void setParamsSTD(InputArray std); |
| 33 | + |
| 34 | + Ptr<optim::Solver::Function> getFunction() const; |
| 35 | + void setFunction(const Ptr<Solver::Function>& f); |
| 36 | + TermCriteria getTermCriteria() const; |
| 37 | + void setTermCriteria(const TermCriteria& termcrit); |
| 38 | + private: |
| 39 | + Mat_<double> _std,_particles,_logweight; |
| 40 | + Ptr<Solver::Function> _Function; |
| 41 | + PFSolver::Function* _real_function; |
| 42 | + TermCriteria _termcrit; |
| 43 | + int _maxItNum,_iter,_particlesNum; |
| 44 | + double _alpha; |
| 45 | + inline void normalize(Mat_<double>& row); |
| 46 | + RNG rng; |
| 47 | + }; |
| 48 | + |
| 49 | + CV_EXPORTS_W Ptr<PFSolver> createPFSolver(const Ptr<optim::Solver::Function>& f=Ptr<optim::Solver::Function>(),InputArray std=Mat(), |
| 50 | + TermCriteria termcrit=TermCriteria(TermCriteria::MAX_ITER,5,0.0),int particlesNum=100,double alpha=0.6); |
| 51 | + |
| 52 | + PFSolver::PFSolver(){ |
| 53 | + _Function=Ptr<Solver::Function>(); |
| 54 | + _real_function=NULL; |
| 55 | + _std=Mat_<double>(); |
| 56 | + rng=RNG(getTickCount()); |
| 57 | + } |
| 58 | + void PFSolver::getOptParam(OutputArray params)const{ |
| 59 | + params.create(1,_std.rows,CV_64FC1); |
| 60 | + Mat mat(1,_std.rows,CV_64FC1); |
| 61 | +#ifdef WEIGHTED |
| 62 | + mat.setTo(0.0); |
| 63 | + for(int i=0;i<_particles.rows;i++){ |
| 64 | + mat+=_particles.row(i)/exp(-_logweight(0,i)); |
| 65 | + } |
| 66 | + _real_function->correctParams((double*)mat.data); |
| 67 | + mat.copyTo(params); |
| 68 | +#else |
| 69 | + params.create(1,_std.rows,CV_64FC1); |
| 70 | + Mat optimus=_particles.row(std::max_element(_logweight.begin(),_logweight.end())-_logweight.begin()); |
| 71 | + _real_function->correctParams(optimus.data); |
| 72 | + optimus.copyTo(params); |
| 73 | +#endif |
| 74 | + } |
| 75 | + int PFSolver::iteration(){ |
| 76 | + if(_iter>=_maxItNum){ |
| 77 | + return _maxItNum+1; |
| 78 | + } |
| 79 | + |
| 80 | + _real_function->setLevel(_iter+1,_maxItNum); |
| 81 | + |
| 82 | + //perturb |
| 83 | + for(int j=0;j<_particles.cols;j++){ |
| 84 | + double sigma=_std(0,j); |
| 85 | + for(int i=0;i<_particles.rows;i++){ |
| 86 | + _particles(i,j)+=rng.gaussian(sigma); |
| 87 | + } |
| 88 | + } |
| 89 | + |
| 90 | + //measure |
| 91 | + for(int i=0;i<_particles.rows;i++){ |
| 92 | + _real_function->correctParams((double*)_particles.row(i).data); |
| 93 | + _logweight(0,i)=-(_real_function->calc((double*)_particles.row(i).data)); |
| 94 | + } |
| 95 | + //normalize |
| 96 | + normalize(_logweight); |
| 97 | + //replicate |
| 98 | + Mat_<double> new_particles(_particlesNum,_std.cols); |
| 99 | + int num_particles=0; |
| 100 | + for(int i=0;i<_particles.rows;i++){ |
| 101 | + int num_replicons=cvFloor(new_particles.rows/exp(-_logweight(0,i))); |
| 102 | + for(int j=0;j<num_replicons;j++,num_particles++){ |
| 103 | + _particles.row(i).copyTo(new_particles.row(num_particles)); |
| 104 | + } |
| 105 | + } |
| 106 | + Mat_<double> maxrow=_particles.row(std::max_element(_logweight.begin(),_logweight.end())-_logweight.begin()); |
| 107 | + for(;num_particles<new_particles.rows;num_particles++){ |
| 108 | + maxrow.copyTo(new_particles.row(num_particles)); |
| 109 | + } |
| 110 | + |
| 111 | + if(_particles.rows!=new_particles.rows){ |
| 112 | + _particles=new_particles; |
| 113 | + }else{ |
| 114 | + new_particles.copyTo(_particles); |
| 115 | + } |
| 116 | + _std=_std*_alpha; |
| 117 | + _iter++; |
| 118 | + return _iter; |
| 119 | + } |
| 120 | + double PFSolver::minimize(InputOutputArray x){ |
| 121 | + CV_Assert(_Function.empty()==false); |
| 122 | + CV_Assert(_std.rows==1 && _std.cols>0); |
| 123 | + Mat mat_x=x.getMat(); |
| 124 | + CV_Assert(mat_x.type()==CV_64FC1 && MIN(mat_x.rows,mat_x.cols)==1 && MAX(mat_x.rows,mat_x.cols)==_std.cols); |
| 125 | + |
| 126 | + _iter=0; |
| 127 | + _particles=Mat_<double>(_particlesNum,_std.cols); |
| 128 | + if(mat_x.rows>1){ |
| 129 | + mat_x=mat_x.t(); |
| 130 | + } |
| 131 | + for(int i=0;i<_particles.rows;i++){ |
| 132 | + mat_x.copyTo(_particles.row(i)); |
| 133 | + } |
| 134 | + |
| 135 | + _logweight.create(1,_particles.rows); |
| 136 | + _logweight.setTo(-log(_particles.rows)); |
| 137 | + return 0.0; |
| 138 | + } |
| 139 | + |
| 140 | + void PFSolver::setParticlesNum(int num){ |
| 141 | + CV_Assert(num>0); |
| 142 | + _particlesNum=num; |
| 143 | + } |
| 144 | + int PFSolver::getParticlesNum(){ |
| 145 | + return _particlesNum; |
| 146 | + } |
| 147 | + void PFSolver::setAlpha(double AlphaM){ |
| 148 | + CV_Assert( 0<AlphaM && AlphaM<= 1); |
| 149 | + _alpha=AlphaM; |
| 150 | + } |
| 151 | + double PFSolver::getAlpha(){ |
| 152 | + return _alpha; |
| 153 | + } |
| 154 | + Ptr<optim::Solver::Function> PFSolver::getFunction() const{ |
| 155 | + return _Function; |
| 156 | + } |
| 157 | + void PFSolver::setFunction(const Ptr<optim::Solver::Function>& f){ |
| 158 | + CV_Assert(f.empty()==false); |
| 159 | + |
| 160 | + Ptr<Solver::Function> non_const_f(f); |
| 161 | + Solver::Function* f_ptr=static_cast<Solver::Function*>(non_const_f); |
| 162 | + |
| 163 | + PFSolver::Function *pff= dynamic_cast<PFSolver::Function*>(f_ptr); |
| 164 | + CV_Assert(pff!=NULL); |
| 165 | + _Function=f; |
| 166 | + _real_function=pff; |
| 167 | + } |
| 168 | + TermCriteria PFSolver::getTermCriteria() const{ |
| 169 | + return TermCriteria(TermCriteria::MAX_ITER,_maxItNum,0.0); |
| 170 | + } |
| 171 | + void PFSolver::setTermCriteria(const TermCriteria& termcrit){ |
| 172 | + CV_Assert(termcrit.type==TermCriteria::MAX_ITER && termcrit.maxCount>0); |
| 173 | + _maxItNum=termcrit.maxCount; |
| 174 | + } |
| 175 | + void PFSolver::getParamsSTD(OutputArray std)const{ |
| 176 | + std.create(1,_std.cols,CV_64FC1); |
| 177 | + _std.copyTo(std); |
| 178 | + } |
| 179 | + void PFSolver::setParamsSTD(InputArray std){ |
| 180 | + Mat m=std.getMat(); |
| 181 | + CV_Assert(MIN(m.cols,m.rows)==1 && m.type()==CV_64FC1); |
| 182 | + int ndim=MAX(m.cols,m.rows); |
| 183 | + if(ndim!=_std.cols){ |
| 184 | + _std=Mat_<double>(1,ndim); |
| 185 | + } |
| 186 | + if(m.rows==1){ |
| 187 | + m.copyTo(_std); |
| 188 | + }else{ |
| 189 | + Mat std_t=Mat_<double>(ndim,1,(double*)_std.data); |
| 190 | + m.copyTo(std_t); |
| 191 | + } |
| 192 | + } |
| 193 | + |
| 194 | + Ptr<PFSolver> createPFSolver(const Ptr<optim::Solver::Function>& f,InputArray std,TermCriteria termcrit,int particlesNum,double alpha){ |
| 195 | + Ptr<PFSolver> ptr(new PFSolver()); |
| 196 | + |
| 197 | + if(f.empty()==false){ |
| 198 | + ptr->setFunction(f); |
| 199 | + } |
| 200 | + Mat mystd=std.getMat(); |
| 201 | + if(mystd.cols!=0 || mystd.rows!=0){ |
| 202 | + ptr->setParamsSTD(std); |
| 203 | + } |
| 204 | + ptr->setTermCriteria(termcrit); |
| 205 | + ptr->setParticlesNum(particlesNum); |
| 206 | + ptr->setAlpha(alpha); |
| 207 | + return ptr; |
| 208 | + } |
| 209 | + void PFSolver::normalize(Mat_<double>& row){ |
| 210 | + double logsum=0.0; |
| 211 | + double max=*(std::max_element(row.begin(),row.end())); |
| 212 | + row-=max; |
| 213 | + for(int i=0;i<row.cols;i++){ |
| 214 | + logsum+=exp(row(0,i)); |
| 215 | + } |
| 216 | + logsum=log(logsum); |
| 217 | + row-=logsum; |
| 218 | + } |
| 219 | +} |
0 commit comments