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>
-```
+$ cmake -DOPENCV_EXTRA_MODULES_PATH=<opencv_contrib>/modules -DBUILD_opencv_legacy=OFF <opencv_source_directory>
+```

modules/tracking/CMakeLists.txt

@@ -1,2 +1,2 @@
 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

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