Merge pull request #2 from nailbiter/optimPF

The implementation of particle filtering tracker

README.md

@@ -18,7 +18,7 @@ Here is the CMake command for you:
 
 ```
 $ cd <opencv_build_directory>
-$ cmake -DOPENCV_EXTRA_MODULES_PATH=<opencv_contrib> <opencv_source_directory>
+$ cmake -DOPENCV_EXTRA_MODULES_PATH=<opencv_contrib>/modules <opencv_source_directory>
 $ make -j5
 ```
 
@@ -27,5 +27,5 @@ modules from `opencv_contrib` repository. If you don't want all of the modules,
 use CMake's `BUILD_opencv_*` options. Like in this example:
 
 ```
-$ cmake -DOPENCV_EXTRA_MODULES_PATH=<opencv_contrib> -DBUILD_opencv_legacy=OFF <opencv_source_directory>
-```
\ No newline at end of file
+$ cmake -DOPENCV_EXTRA_MODULES_PATH=<opencv_contrib>/modules -DBUILD_opencv_legacy=OFF <opencv_source_directory>
+```

modules/tracking/CMakeLists.txt

 set(the_description "Tracking API")
-ocv_define_module(tracking opencv_imgproc)
+ocv_define_module(tracking opencv_imgproc opencv_optim)

modules/tracking/doc/tracker_algorithms.rst

@@ -3,7 +3,7 @@ Tracker Algorithms
 
 .. highlight:: cpp
 
-Two algorithms will be implemented soon, the first is MIL (Multiple Instance Learning) [MIL]_ and second is Online Boosting [OLB]_.
+The following algorithms are implemented at the moment.
 
 .. [MIL] B Babenko, M-H Yang, and S Belongie, Visual Tracking with Online Multiple Instance Learning, In CVPR, 2009
 
@@ -13,7 +13,8 @@ TrackerBoosting
 ---------------
 
 This is a real-time object tracking based on a novel on-line version of the AdaBoost algorithm.
-The classifier uses the surrounding background as negative examples in update step to avoid the drifting problem.
+The classifier uses the surrounding background as negative examples in update step to avoid the drifting problem. The implementation is based on
+[OLB]_.
 
 .. ocv:class:: TrackerBoosting
 
@@ -33,10 +34,39 @@ Implementation of TrackerBoosting from :ocv:class:`Tracker`::
 
    };
 
+TrackerBoosting::Params
+-----------------------------------------------------------------------
+
+.. ocv:struct:: TrackerBoosting::Params
+
+List of BOOSTING parameters::
+
+   struct CV_EXPORTS Params
+   {
+    Params();
+    int numClassifiers;  //the number of classifiers to use in a OnlineBoosting algorithm
+    float samplerOverlap;  //search region parameters to use in a OnlineBoosting algorithm
+    float samplerSearchFactor;  // search region parameters to use in a OnlineBoosting algorithm
+    int iterationInit;  //the initial iterations
+    int featureSetNumFeatures;  // #features
+
+    void read( const FileNode& fn );
+    void write( FileStorage& fs ) const;
+   };
+
+TrackerBoosting::TrackerBoosting
+-----------------------------------------------------------------------
+
+Constructor
+
+.. ocv:function:: bool TrackerBoosting::TrackerBoosting( const TrackerBoosting::Params &parameters = TrackerBoosting::Params() )
+
+    :param parameters: BOOSTING parameters :ocv:struct:`TrackerBoosting::Params`
+
 TrackerMIL
 ----------
 
-The MIL algorithm trains a classifier in an online manner to separate the object from the background. Multiple Instance Learning avoids the drift problem for a robust tracking.
+The MIL algorithm trains a classifier in an online manner to separate the object from the background. Multiple Instance Learning avoids the drift problem for a robust tracking. The implementation is based on [MIL]_.
 
 Original code can be found here http://vision.ucsd.edu/~bbabenko/project_miltrack.shtml
 
@@ -89,33 +119,3 @@ Constructor
 .. ocv:function:: bool TrackerMIL::TrackerMIL( const TrackerMIL::Params &parameters = TrackerMIL::Params() )
 
     :param parameters: MIL parameters :ocv:struct:`TrackerMIL::Params`
-
-
-TrackerBoosting::Params
-------------------
-
-.. ocv:struct:: TrackerBoosting::Params
-
-List of BOOSTING parameters::
-
-   struct CV_EXPORTS Params
-   {
-    Params();
-    int numClassifiers;  //the number of classifiers to use in a OnlineBoosting algorithm
-    float samplerOverlap;  //search region parameters to use in a OnlineBoosting algorithm
-    float samplerSearchFactor;  // search region parameters to use in a OnlineBoosting algorithm
-    int iterationInit;  //the initial iterations
-    int featureSetNumFeatures;  // #features
-
-    void read( const FileNode& fn );
-    void write( FileStorage& fs ) const;
-   };
-
-TrackerBoosting::TrackerBoosting
-----------------------
-
-Constructor
-
-.. ocv:function:: bool TrackerBoosting::TrackerBoosting( const TrackerBoosting::Params &parameters = TrackerBoosting::Params() )
-
-    :param parameters: BOOSTING parameters :ocv:struct:`TrackerBoosting::Params`

modules/tracking/include/opencv2/tracking/tracker.hpp

@@ -47,6 +47,7 @@
 #include "feature.hpp"
 #include "onlineMIL.hpp"
 #include "onlineBoosting.hpp"
+#include "opencv2/optim.hpp"
 #include <iostream>
 
 /*
@@ -796,6 +797,25 @@ class CV_EXPORTS_W TrackerSamplerCS : public TrackerSamplerAlgorithm
 
 };
 
+class CV_EXPORTS_W TrackerSamplerPF : public TrackerSamplerAlgorithm{
+public:
+  struct CV_EXPORTS Params
+  {
+    Params();
+    int iterationNum;
+    int particlesNum;
+    double alpha;
+    Mat_<double> std; 
+  };
+  TrackerSamplerPF(const Mat& chosenRect,const TrackerSamplerPF::Params &parameters = TrackerSamplerPF::Params());
+protected:
+  bool samplingImpl( const Mat& image, Rect boundingBox, std::vector<Mat>& sample );
+private:
+  Params params;
+  Ptr<optim::Solver> _solver;
+  Ptr<optim::Solver::Function> _function;
+};
+
 /************************************ Specific TrackerFeature Classes ************************************/
 
 /**
@@ -1015,7 +1035,6 @@ class CV_EXPORTS_W TrackerBoosting : public Tracker
   Params params;
   AlgorithmInfo* info() const;
 };
-
 } /* namespace cv */
 
 #endif

modules/tracking/src/PFSolver.hpp

+#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;
+    }
+}

modules/tracking/src/TrackingFunctionPF.hpp

+#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]));
+    }
+}

modules/tracking/src/trackerSamplerAlgorithm.cpp

@@ -41,6 +41,8 @@
 
 #include "precomp.hpp"
 #include <time.h>
+#include "PFSolver.hpp"
+#include "TrackingFunctionPF.hpp"
 
 #ifdef _WIN32
 #define TIME( arg ) (((double) clock()) / CLOCKS_PER_SEC)
@@ -379,4 +381,34 @@ std::vector<Mat> TrackerSamplerCS::patchesRegularScan( const Mat& image, Rect tr
   return sample;
 }
 
+TrackerSamplerPF::Params::Params(){
+    iterationNum=20;
+    particlesNum=100;
+    alpha=0.9;
+    std=(Mat_<double>(1,4)<<15.0,15.0,15.0,15.0); 
+}
+TrackerSamplerPF::TrackerSamplerPF(const Mat& chosenRect,const TrackerSamplerPF::Params &parameters):
+    params( parameters ),_function(new TrackingFunctionPF(chosenRect)){
+        className="PF";
+        _solver=createPFSolver(_function,parameters.std,TermCriteria(TermCriteria::MAX_ITER,parameters.iterationNum,0.0),
+        parameters.particlesNum,parameters.alpha);
+}
+bool TrackerSamplerPF::samplingImpl( const Mat& image, Rect boundingBox, std::vector<Mat>& sample ){
+    Ptr<TrackerTargetState> ptr;
+    Mat_<double> _last_guess=(Mat_<double>(1,4)<<(double)boundingBox.x,(double)boundingBox.y,
+    (double)boundingBox.x+boundingBox.width,(double)boundingBox.y+boundingBox.height);
+    PFSolver* promoted_solver=dynamic_cast<PFSolver*>(static_cast<optim::Solver*>(_solver));
+
+    promoted_solver->setParamsSTD(params.std);
+    promoted_solver->minimize(_last_guess);
+    dynamic_cast<TrackingFunctionPF*>(static_cast<optim::Solver::Function*>(promoted_solver->getFunction()))->update(image);
+    while(promoted_solver->iteration() <= promoted_solver->getTermCriteria().maxCount);
+    promoted_solver->getOptParam(_last_guess);
+
+    Rect res=Rect(Point_<int>((int)_last_guess(0,0),(int)_last_guess(0,1)),Point_<int>((int)_last_guess(0,2),(int)_last_guess(0,3)));
+    sample.clear();
+    sample.push_back(image(res));
+    return true;
+}
+
 } /* namespace cv */