Merge pull request #2 from nailbiter/optimPF
The implementation of particle filtering trackerpull/6/head
Antonella Cascitelli
11 years ago
commit
dd1fd48e9e
7 changed files with 406 additions and 38 deletions
@ -1,2 +1,2 @@ |
||||
set(the_description "Tracking API") |
||||
ocv_define_module(tracking opencv_imgproc) |
||||
ocv_define_module(tracking opencv_imgproc opencv_optim) |
||||
|
||||
@ -0,0 +1,219 @@ |
||||
#include "opencv2/optim.hpp" |
||||
#include "opencv2/core/core_c.h" |
||||
#include <algorithm> |
||||
#include <typeinfo> |
||||
#include <cmath> |
||||
#define WEIGHTED |
||||
|
||||
namespace cv{ |
||||
|
||||
//!particle filtering class
|
||||
class PFSolver : public optim::Solver{ |
||||
public: |
||||
class Function : public optim::Solver::Function |
||||
{ |
||||
public: |
||||
//!if parameters have no sense due to some reason (e.g. lie outside of function domain), this function "corrects" them,
|
||||
//!that is brings to the function domain
|
||||
virtual void correctParams(double* /*optParams*/)const{} |
||||
//!is used when there is a dependence on the number of iterations done in calc(), note that levels are counted starting from 1
|
||||
virtual void setLevel(int /*level*/, int /*levelsNum*/){} |
||||
}; |
||||
PFSolver(); |
||||
void getOptParam(OutputArray params)const; |
||||
int iteration(); |
||||
double minimize(InputOutputArray x); |
||||
|
||||
void setParticlesNum(int num); |
||||
int getParticlesNum(); |
||||
void setAlpha(double AlphaM); |
||||
double getAlpha(); |
||||
void getParamsSTD(OutputArray std)const; |
||||
void setParamsSTD(InputArray std); |
||||
|
||||
Ptr<optim::Solver::Function> getFunction() const; |
||||
void setFunction(const Ptr<Solver::Function>& f); |
||||
TermCriteria getTermCriteria() const; |
||||
void setTermCriteria(const TermCriteria& termcrit); |
||||
private: |
||||
Mat_<double> _std,_particles,_logweight; |
||||
Ptr<Solver::Function> _Function; |
||||
PFSolver::Function* _real_function; |
||||
TermCriteria _termcrit; |
||||
int _maxItNum,_iter,_particlesNum; |
||||
double _alpha; |
||||
inline void normalize(Mat_<double>& row); |
||||
RNG rng; |
||||
}; |
||||
|
||||
CV_EXPORTS_W Ptr<PFSolver> createPFSolver(const Ptr<optim::Solver::Function>& f=Ptr<optim::Solver::Function>(),InputArray std=Mat(), |
||||
TermCriteria termcrit=TermCriteria(TermCriteria::MAX_ITER,5,0.0),int particlesNum=100,double alpha=0.6); |
||||
|
||||
PFSolver::PFSolver(){ |
||||
_Function=Ptr<Solver::Function>(); |
||||
_real_function=NULL; |
||||
_std=Mat_<double>(); |
||||
rng=RNG(getTickCount()); |
||||
} |
||||
void PFSolver::getOptParam(OutputArray params)const{ |
||||
params.create(1,_std.rows,CV_64FC1); |
||||
Mat mat(1,_std.rows,CV_64FC1); |
||||
#ifdef WEIGHTED |
||||
mat.setTo(0.0); |
||||
for(int i=0;i<_particles.rows;i++){ |
||||
mat+=_particles.row(i)/exp(-_logweight(0,i)); |
||||
} |
||||
_real_function->correctParams((double*)mat.data); |
||||
mat.copyTo(params); |
||||
#else |
||||
params.create(1,_std.rows,CV_64FC1); |
||||
Mat optimus=_particles.row(std::max_element(_logweight.begin(),_logweight.end())-_logweight.begin()); |
||||
_real_function->correctParams(optimus.data); |
||||
optimus.copyTo(params); |
||||
#endif |
||||
} |
||||
int PFSolver::iteration(){ |
||||
if(_iter>=_maxItNum){ |
||||
return _maxItNum+1; |
||||
} |
||||
|
||||
_real_function->setLevel(_iter+1,_maxItNum); |
||||
|
||||
//perturb
|
||||
for(int j=0;j<_particles.cols;j++){ |
||||
double sigma=_std(0,j); |
||||
for(int i=0;i<_particles.rows;i++){ |
||||
_particles(i,j)+=rng.gaussian(sigma); |
||||
} |
||||
} |
||||
|
||||
//measure
|
||||
for(int i=0;i<_particles.rows;i++){ |
||||
_real_function->correctParams((double*)_particles.row(i).data); |
||||
_logweight(0,i)=-(_real_function->calc((double*)_particles.row(i).data)); |
||||
} |
||||
//normalize
|
||||
normalize(_logweight); |
||||
//replicate
|
||||
Mat_<double> new_particles(_particlesNum,_std.cols); |
||||
int num_particles=0; |
||||
for(int i=0;i<_particles.rows;i++){ |
||||
int num_replicons=cvFloor(new_particles.rows/exp(-_logweight(0,i))); |
||||
for(int j=0;j<num_replicons;j++,num_particles++){ |
||||
_particles.row(i).copyTo(new_particles.row(num_particles)); |
||||
} |
||||
} |
||||
Mat_<double> maxrow=_particles.row(std::max_element(_logweight.begin(),_logweight.end())-_logweight.begin()); |
||||
for(;num_particles<new_particles.rows;num_particles++){ |
||||
maxrow.copyTo(new_particles.row(num_particles)); |
||||
} |
||||
|
||||
if(_particles.rows!=new_particles.rows){ |
||||
_particles=new_particles; |
||||
}else{ |
||||
new_particles.copyTo(_particles); |
||||
} |
||||
_std=_std*_alpha; |
||||
_iter++; |
||||
return _iter; |
||||
} |
||||
double PFSolver::minimize(InputOutputArray x){ |
||||
CV_Assert(_Function.empty()==false); |
||||
CV_Assert(_std.rows==1 && _std.cols>0); |
||||
Mat mat_x=x.getMat(); |
||||
CV_Assert(mat_x.type()==CV_64FC1 && MIN(mat_x.rows,mat_x.cols)==1 && MAX(mat_x.rows,mat_x.cols)==_std.cols); |
||||
|
||||
_iter=0; |
||||
_particles=Mat_<double>(_particlesNum,_std.cols); |
||||
if(mat_x.rows>1){ |
||||
mat_x=mat_x.t(); |
||||
} |
||||
for(int i=0;i<_particles.rows;i++){ |
||||
mat_x.copyTo(_particles.row(i)); |
||||
} |
||||
|
||||
_logweight.create(1,_particles.rows); |
||||
_logweight.setTo(-log(_particles.rows)); |
||||
return 0.0; |
||||
} |
||||
|
||||
void PFSolver::setParticlesNum(int num){ |
||||
CV_Assert(num>0); |
||||
_particlesNum=num; |
||||
} |
||||
int PFSolver::getParticlesNum(){ |
||||
return _particlesNum; |
||||
} |
||||
void PFSolver::setAlpha(double AlphaM){ |
||||
CV_Assert(0<AlphaM && AlphaM<=1); |
||||
_alpha=AlphaM; |
||||
} |
||||
double PFSolver::getAlpha(){ |
||||
return _alpha; |
||||
} |
||||
Ptr<optim::Solver::Function> PFSolver::getFunction() const{ |
||||
return _Function; |
||||
} |
||||
void PFSolver::setFunction(const Ptr<optim::Solver::Function>& f){ |
||||
CV_Assert(f.empty()==false); |
||||
|
||||
Ptr<Solver::Function> non_const_f(f); |
||||
Solver::Function* f_ptr=static_cast<Solver::Function*>(non_const_f); |
||||
|
||||
PFSolver::Function *pff=dynamic_cast<PFSolver::Function*>(f_ptr); |
||||
CV_Assert(pff!=NULL); |
||||
_Function=f; |
||||
_real_function=pff; |
||||
} |
||||
TermCriteria PFSolver::getTermCriteria() const{ |
||||
return TermCriteria(TermCriteria::MAX_ITER,_maxItNum,0.0); |
||||
} |
||||
void PFSolver::setTermCriteria(const TermCriteria& termcrit){ |
||||
CV_Assert(termcrit.type==TermCriteria::MAX_ITER && termcrit.maxCount>0); |
||||
_maxItNum=termcrit.maxCount; |
||||
} |
||||
void PFSolver::getParamsSTD(OutputArray std)const{ |
||||
std.create(1,_std.cols,CV_64FC1); |
||||
_std.copyTo(std); |
||||
} |
||||
void PFSolver::setParamsSTD(InputArray std){ |
||||
Mat m=std.getMat(); |
||||
CV_Assert(MIN(m.cols,m.rows)==1 && m.type()==CV_64FC1); |
||||
int ndim=MAX(m.cols,m.rows); |
||||
if(ndim!=_std.cols){ |
||||
_std=Mat_<double>(1,ndim); |
||||
} |
||||
if(m.rows==1){ |
||||
m.copyTo(_std); |
||||
}else{ |
||||
Mat std_t=Mat_<double>(ndim,1,(double*)_std.data); |
||||
m.copyTo(std_t); |
||||
} |
||||
} |
||||
|
||||
Ptr<PFSolver> createPFSolver(const Ptr<optim::Solver::Function>& f,InputArray std,TermCriteria termcrit,int particlesNum,double alpha){ |
||||
Ptr<PFSolver> ptr(new PFSolver()); |
||||
|
||||
if(f.empty()==false){ |
||||
ptr->setFunction(f); |
||||
} |
||||
Mat mystd=std.getMat(); |
||||
if(mystd.cols!=0 || mystd.rows!=0){ |
||||
ptr->setParamsSTD(std); |
||||
} |
||||
ptr->setTermCriteria(termcrit); |
||||
ptr->setParticlesNum(particlesNum); |
||||
ptr->setAlpha(alpha); |
||||
return ptr; |
||||
} |
||||
void PFSolver::normalize(Mat_<double>& row){ |
||||
double logsum=0.0; |
||||
double max=*(std::max_element(row.begin(),row.end())); |
||||
row-=max; |
||||
for(int i=0;i<row.cols;i++){ |
||||
logsum+=exp(row(0,i)); |
||||
} |
||||
logsum=log(logsum); |
||||
row-=logsum; |
||||
} |
||||
} |
||||
@ -0,0 +1,98 @@ |
||||
#include <cmath> |
||||
#include <opencv2/imgproc/imgproc_c.h> |
||||
#define CLIP(x,a,b) MIN(MAX((x),(a)),(b)) |
||||
#define HIST_SIZE 50 |
||||
|
||||
namespace cv{ |
||||
|
||||
class TrackingFunctionPF : public PFSolver::Function{ |
||||
public: |
||||
TrackingFunctionPF(const Mat& chosenRect); |
||||
void update(const Mat& image); |
||||
double calc(const double* x) const; |
||||
void correctParams(double* pt)const; |
||||
private: |
||||
Mat _image; |
||||
static inline Rect rectFromRow(const double* row); |
||||
const int _nh,_ns,_nv; |
||||
class TrackingHistogram{ |
||||
public: |
||||
TrackingHistogram(const Mat& img,int nh,int ns,int nv); |
||||
double dist(const TrackingHistogram& hist)const; |
||||
private: |
||||
Mat_<double> HShist, Vhist; |
||||
}; |
||||
TrackingHistogram _origHist; |
||||
}; |
||||
|
||||
TrackingFunctionPF::TrackingHistogram::TrackingHistogram(const Mat& img,int nh,int ns,int nv){ |
||||
|
||||
Mat hsv; |
||||
img.convertTo(hsv,CV_32F,1.0/255.0); |
||||
cvtColor(hsv,hsv,CV_BGR2HSV); |
||||
|
||||
HShist=Mat_<double>(nh,ns,0.0); |
||||
Vhist=Mat_<double>(1,nv,0.0); |
||||
|
||||
for(int i=0;i<img.rows;i++){ |
||||
for(int j=0;j<img.cols;j++){ |
||||
const Vec3f& pt=hsv.at<Vec3f>(i,j); |
||||
|
||||
if(pt.val[1]>0.1 && pt.val[2]>0.2){ |
||||
HShist(MIN(nh-1,(int)(nh*pt.val[0]/360.0)),MIN(ns-1,(int)(ns*pt.val[1])))++; |
||||
}else{ |
||||
Vhist(0,MIN(nv-1,(int)(nv*pt.val[2])))++; |
||||
} |
||||
}} |
||||
|
||||
double total=*(sum(HShist)+sum(Vhist)).val; |
||||
HShist/=total; |
||||
Vhist/=total; |
||||
} |
||||
double TrackingFunctionPF::TrackingHistogram::dist(const TrackingHistogram& hist)const{ |
||||
double res=1.0; |
||||
|
||||
for(int i=0;i<HShist.rows;i++){ |
||||
for(int j=0;j<HShist.cols;j++){ |
||||
res-=sqrt(HShist(i,j)*hist.HShist(i,j)); |
||||
}} |
||||
for(int j=0;j<Vhist.cols;j++){ |
||||
res-=sqrt(Vhist(0,j)*hist.Vhist(0,j)); |
||||
} |
||||
|
||||
return sqrt(res); |
||||
} |
||||
double TrackingFunctionPF::calc(const double* x) const{ |
||||
Rect rect=rectFromRow(x); |
||||
if(rect.area()==0){ |
||||
return 2.0; |
||||
} |
||||
return _origHist.dist(TrackingHistogram(_image(rect),_nh,_ns,_nv)); |
||||
} |
||||
TrackingFunctionPF::TrackingFunctionPF(const Mat& chosenRect):_nh(HIST_SIZE),_ns(HIST_SIZE),_nv(HIST_SIZE),_origHist(chosenRect,_nh,_ns,_nv){ |
||||
} |
||||
void TrackingFunctionPF::update(const Mat& image){ |
||||
_image=image; |
||||
|
||||
TrackingHistogram hist(image,_nh,_ns,_nv); |
||||
} |
||||
void TrackingFunctionPF::correctParams(double* pt)const{ |
||||
pt[0]=CLIP(pt[0],0.0,_image.cols+0.9); |
||||
pt[1]=CLIP(pt[1],0.0,_image.rows+0.9); |
||||
pt[2]=CLIP(pt[2],0.0,_image.cols+0.9); |
||||
pt[3]=CLIP(pt[3],0.0,_image.rows+0.9); |
||||
if(pt[0]>pt[2]){ |
||||
double tmp=pt[0]; |
||||
pt[0]=pt[2]; |
||||
pt[2]=tmp; |
||||
} |
||||
if(pt[1]>pt[3]){ |
||||
double tmp=pt[1]; |
||||
pt[1]=pt[3]; |
||||
pt[3]=tmp; |
||||
} |
||||
} |
||||
Rect TrackingFunctionPF::rectFromRow(const double* row){ |
||||
return Rect(Point_<int>((int)row[0],(int)row[1]),Point_<int>((int)row[2],(int)row[3])); |
||||
} |
||||
} |
||||
Loading…
Reference in new issue