X-Git-Url: https://vcs.maemo.org/git/?a=blobdiff_plain;f=src%2Fcvaux%2Fvs%2Fblobtrackingmsfgs.cpp;fp=src%2Fcvaux%2Fvs%2Fblobtrackingmsfgs.cpp;h=36e835bfdcaf210a8828fe3557ad9e5d5879d6c4;hb=e4c14cdbdf2fe805e79cd96ded236f57e7b89060;hp=0000000000000000000000000000000000000000;hpb=454138ff8a20f6edb9b65a910101403d8b520643;p=opencv

diff --git a/src/cvaux/vs/blobtrackingmsfgs.cpp b/src/cvaux/vs/blobtrackingmsfgs.cpp
new file mode 100644
index 0000000..36e835b
--- /dev/null
+++ b/src/cvaux/vs/blobtrackingmsfgs.cpp
@@ -0,0 +1,457 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                        Intel License Agreement
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of Intel Corporation may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_cvaux.h"
+
+#define SCALE_BASE 1.1
+#define SCALE_RANGE 2
+#define SCALE_NUM (2*SCALE_RANGE+1)
+typedef float DefHistType;
+#define DefHistTypeMat CV_32F
+#define HIST_INDEX(_pData) (((_pData)[0]>>m_ByteShift) + (((_pData)[1]>>(m_ByteShift))<<m_BinBit)+((pImgData[2]>>m_ByteShift)<<(m_BinBit*2)))
+
+void calcKernelEpanechnikov(CvMat* pK)
+{    /* Allocate kernel for histogramm creation: */
+    int     x,y;
+    int     w = pK->width;
+    int     h = pK->height;
+    float   x0 = 0.5f*(w-1);
+    float   y0 = 0.5f*(h-1);
+
+    for(y=0; y<h; ++y) for(x=0; x<w; ++x)
+    {
+//                float   r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
+        float   r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((x0*x0)+(y0*y0));
+        CV_MAT_ELEM(pK[0],DefHistType, y, x) = (DefHistType)((r2<1)?(1-r2):0);
+    }
+}   /* Allocate kernel for histogram creation. */
+
+class CvBlobTrackerOneMSFGS:public CvBlobTrackerOne
+{
+private:
+    /* Parameters: */
+    float           m_FGWeight;
+    float           m_Alpha;
+    CvSize          m_ObjSize;
+    CvMat*          m_KernelHistModel;
+    CvMat*          m_KernelHistCandidate;
+    CvSize          m_KernelMeanShiftSize;
+    CvMat*          m_KernelMeanShiftK[SCALE_NUM];
+    CvMat*          m_KernelMeanShiftG[SCALE_NUM];
+    CvMat*          m_Weights;
+    int             m_BinBit;
+    int             m_ByteShift;
+    int             m_BinNum;
+    int             m_Dim;
+    int             m_BinNumTotal;
+    CvMat*          m_HistModel;
+    float           m_HistModelVolume;
+    CvMat*          m_HistCandidate;
+    float           m_HistCandidateVolume;
+    CvMat*          m_HistTemp;
+    CvBlob          m_Blob;
+
+    void ReAllocHist(int Dim, int BinBit)
+    {
+        m_BinBit = BinBit;
+        m_ByteShift = 8-BinBit;
+        m_Dim = Dim;
+        m_BinNum = (1<<BinBit);
+        m_BinNumTotal = cvRound(pow((double)m_BinNum,(double)m_Dim));
+        if(m_HistModel) cvReleaseMat(&m_HistModel);
+        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
+        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
+        m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
+        m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
+        m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
+        cvZero(m_HistCandidate);
+        cvZero(m_HistModel);
+        m_HistModelVolume = 0.0f;
+        m_HistCandidateVolume = 0.0f;
+    }
+
+    void ReAllocKernel(int  w, int h, float sigma=0.4)
+    {
+        double  ScaleToObj = sigma*1.39;
+        int     kernel_width = cvRound(w/ScaleToObj);
+        int     kernel_height = cvRound(h/ScaleToObj);
+        int     x,y,s;
+        assert(w>0);
+        assert(h>0);
+        m_ObjSize = cvSize(w,h);
+        m_KernelMeanShiftSize = cvSize(kernel_width,kernel_height);
+
+
+        /* Create kernels for histogram calculation: */
+        if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
+        m_KernelHistModel = cvCreateMat(h, w, DefHistTypeMat);
+        calcKernelEpanechnikov(m_KernelHistModel);
+        if(m_KernelHistCandidate) cvReleaseMat(&m_KernelHistCandidate);
+        m_KernelHistCandidate = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
+        calcKernelEpanechnikov(m_KernelHistCandidate);
+
+        if(m_Weights) cvReleaseMat(&m_Weights);
+        m_Weights = cvCreateMat(kernel_height, kernel_width, CV_32F);
+
+        for(s=-SCALE_RANGE; s<=SCALE_RANGE; ++s)
+        {   /* Allocate kernel for meanshifts in space and scale: */
+            int     si = s+SCALE_RANGE;
+            double  cur_sigma = sigma * pow(SCALE_BASE,s);
+            double  cur_sigma2 = cur_sigma*cur_sigma;
+            double  x0 = 0.5*(kernel_width-1);
+            double  y0 = 0.5*(kernel_height-1);
+            if(m_KernelMeanShiftK[si]) cvReleaseMat(&m_KernelMeanShiftK[si]);
+            if(m_KernelMeanShiftG[si]) cvReleaseMat(&m_KernelMeanShiftG[si]);
+            m_KernelMeanShiftK[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
+            m_KernelMeanShiftG[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
+
+            for(y=0; y<kernel_height; ++y)
+            {
+                DefHistType* pK = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftK[si][0], y, 0, sizeof(DefHistType) );
+                DefHistType* pG = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftG[si][0], y, 0, sizeof(DefHistType) );
+
+                for(x=0; x<kernel_width; ++x)
+                {
+                    double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
+                    double sigma12 = cur_sigma2 / 2.56;
+                    double sigma22 = cur_sigma2 * 2.56;
+                    pK[x] = (DefHistType)(Gaussian2D(r2, sigma12)/sigma12 - Gaussian2D(r2, sigma22)/sigma22);
+                    pG[x] = (DefHistType)(Gaussian2D(r2, cur_sigma2/1.6) - Gaussian2D(r2, cur_sigma2*1.6));
+                }
+            }   /* Next line. */
+        }
+    }   /* ReallocKernel */
+
+    inline double Gaussian2D(double x, double sigma2)
+    {
+        return (exp(-x/(2*sigma2)) / (2*3.1415926535897932384626433832795*sigma2) );
+    }
+
+    void calcHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pKernel, CvMat* pHist, DefHistType* pHistVolume)
+    {
+        int         w = pKernel->width;
+        int         h = pKernel->height;
+        DefHistType Volume = 0;
+        int         x0 = Center.x - w/2;
+        int         y0 = Center.y - h/2;
+        int         x,y;
+
+        //cvZero(pHist);
+        cvSet(pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
+        Volume = 1;
+
+        if(m_Dim == 3)
+        {
+            for(y=0; y<h; ++y)
+            {
+                unsigned char* pImgData = NULL;
+                unsigned char* pMaskData = NULL;
+                DefHistType* pKernelData = NULL;
+                if((y0+y)>=pImg->height) continue;
+                if((y0+y)<0)continue;
+                pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
+                pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
+                pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,0,sizeof(DefHistType));
+
+                for(x=0; x<w; ++x, pImgData+=3)
+                {
+                    if((x0+x)>=pImg->width) continue;
+                    if((x0+x)<0)continue;
+
+                    if(pMaskData==NULL || pMaskData[x]>128)
+                    {
+                        DefHistType K = pKernelData[x];
+                        int index = HIST_INDEX(pImgData);
+                        assert(index >= 0 && index < pHist->cols);
+                        Volume += K;
+                        ((DefHistType*)(pHist->data.ptr))[index] += K;
+
+                    }   /* Only masked pixels. */
+                }   /*  Next column. */
+            }   /*  Next row. */
+        }   /* if m_Dim == 3. */
+
+        if(pHistVolume)pHistVolume[0] = Volume;
+
+    }; /* calcHist */
+
+    double calcBhattacharyya()
+    {
+        cvMul(m_HistCandidate,m_HistModel,m_HistTemp);
+        cvPow(m_HistTemp,m_HistTemp,0.5);
+        return cvSum(m_HistTemp).val[0] / sqrt(m_HistCandidateVolume*m_HistModelVolume);
+    }   /* calcBhattacharyyaCoefficient */
+
+    void calcWeights(IplImage* pImg, IplImage* pImgFG, CvPoint Center)
+    {
+        cvZero(m_Weights);
+
+        /* Calculate new position: */
+        if(m_Dim == 3)
+        {
+            int         x0 = Center.x - m_KernelMeanShiftSize.width/2;
+            int         y0 = Center.y - m_KernelMeanShiftSize.height/2;
+            int         x,y;
+
+            assert(m_Weights->width == m_KernelMeanShiftSize.width);
+            assert(m_Weights->height == m_KernelMeanShiftSize.height);
+
+            /* Calcualte shift vector: */
+            for(y=0; y<m_KernelMeanShiftSize.height; ++y)
+            {
+                unsigned char* pImgData = NULL;
+                unsigned char* pMaskData = NULL;
+                float* pWData = NULL;
+
+                if(y+y0 < 0 || y+y0 >= pImg->height) continue;
+
+                pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
+                pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
+                pWData = (float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,0,sizeof(float));
+
+                for(x=0; x<m_KernelMeanShiftSize.width; ++x, pImgData+=3)
+                {
+                    double      V  = 0;
+                    double      HM = 0;
+                    double      HC = 0;
+                    int         index;
+                    if(x+x0 < 0 || x+x0 >= pImg->width) continue;
+
+                    index = HIST_INDEX(pImgData);
+                    assert(index >= 0 && index < m_BinNumTotal);
+
+                    if(m_HistModelVolume>0)
+                        HM = ((DefHistType*)m_HistModel->data.ptr)[index]/m_HistModelVolume;
+
+                    if(m_HistCandidateVolume>0)
+                        HC = ((DefHistType*)m_HistCandidate->data.ptr)[index]/m_HistCandidateVolume;
+
+                    V = (HC>0)?sqrt(HM / HC):0;
+                    V += m_FGWeight*(pMaskData?((pMaskData[x]/255.0f)):0);
+                    pWData[x] = (float)MIN(V,100000);
+
+                }   /* Next column. */
+            }   /*  Next row. */
+        }   /*  if m_Dim == 3. */
+    }   /*  calcWeights */
+
+public:
+    CvBlobTrackerOneMSFGS()
+    {
+        int i;
+        m_FGWeight = 0;
+        m_Alpha = 0.0;
+
+        /* Add several parameters for external use: */
+        AddParam("FGWeight", &m_FGWeight);
+        CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
+        AddParam("Alpha", &m_Alpha);
+        CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");
+
+        m_BinBit=0;
+        m_Dim = 0;
+        m_HistModel = NULL;
+        m_HistCandidate = NULL;
+        m_HistTemp = NULL;
+        m_KernelHistModel = NULL;
+        m_KernelHistCandidate = NULL;
+        m_Weights = NULL;
+
+        for(i=0; i<SCALE_NUM; ++i)
+        {
+            m_KernelMeanShiftK[i] = NULL;
+            m_KernelMeanShiftG[i] = NULL;
+        }
+        ReAllocHist(3,5);   /* 3D hist, each dimension has 2^5 bins. */
+
+        SetModuleName("MSFGS");
+    }
+
+    ~CvBlobTrackerOneMSFGS()
+    {
+        int i;
+        if(m_HistModel) cvReleaseMat(&m_HistModel);
+        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
+        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
+        if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
+
+        for(i=0; i<SCALE_NUM; ++i)
+        {
+            if(m_KernelMeanShiftK[i]) cvReleaseMat(&m_KernelMeanShiftK[i]);
+            if(m_KernelMeanShiftG[i]) cvReleaseMat(&m_KernelMeanShiftG[i]);
+        }
+    }
+
+    /* Interface: */
+    virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
+    {
+        int w = cvRound(CV_BLOB_WX(pBlobInit));
+        int h = cvRound(CV_BLOB_WY(pBlobInit));
+        if(w<3)w=3;
+        if(h<3)h=3;
+        if(w>pImg->width)w=pImg->width;
+        if(h>pImg->height)h=pImg->height;
+        ReAllocKernel(w,h);
+        calcHist(pImg, pImgFG, cvPointFrom32f(CV_BLOB_CENTER(pBlobInit)), m_KernelHistModel, m_HistModel, &m_HistModelVolume);
+        m_Blob = pBlobInit[0];
+    };
+
+    virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
+    {
+        int     iter;
+
+        if(pBlobPrev)
+        {
+            m_Blob = pBlobPrev[0];
+        }
+
+        for(iter=0; iter<10; ++iter)
+        {
+//            float   newx=0,newy=0,sum=0;
+            float   dx=0,dy=0,sum=0;
+            int     x,y,si;
+
+            CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
+            CvSize  Size   = cvSize(cvRound(m_Blob.w),cvRound(m_Blob.h));
+
+            if(m_ObjSize.width != Size.width || m_ObjSize.height != Size.height)
+            {   /* Reallocate kernels: */
+                ReAllocKernel(Size.width,Size.height);
+            }   /* Reallocate kernels. */
+
+            /* Mean shift in coordinate space: */
+            calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
+            calcWeights(pImg, pImgFG, Center);
+
+            for(si=1; si<(SCALE_NUM-1); ++si)
+            {
+                CvMat*  pKernel = m_KernelMeanShiftK[si];
+                float   sdx = 0, sdy=0, ssum=0;
+                int     s = si-SCALE_RANGE;
+                float   factor = (1.0f-( float(s)/float(SCALE_RANGE) )*( float(s)/float(SCALE_RANGE) ));
+
+                for(y=0; y<m_KernelMeanShiftSize.height; ++y)
+                for(x=0; x<m_KernelMeanShiftSize.width;  ++x)
+                {
+                    float W = *(float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,x,sizeof(float));
+                    float K = *(float*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,x,sizeof(float));
+                    float KW = K*W;
+                    ssum += (float)fabs(KW);
+                    sdx += KW*(x-m_KernelMeanShiftSize.width*0.5f);
+                    sdy += KW*(y-m_KernelMeanShiftSize.height*0.5f);
+                }   /* Next pixel. */
+
+                dx += sdx * factor;
+                dy += sdy * factor;
+                sum  += ssum * factor;
+
+            }   /* Next scale. */
+
+            if(sum > 0)
+            {
+                dx /= sum;
+                dy /= sum;
+            }
+
+            m_Blob.x += dx;
+            m_Blob.y += dy;
+
+            {   /* Mean shift in scale space: */
+                float   news = 0;
+                float   sum = 0;
+                float   scale;
+
+                Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
+                calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
+                calcWeights(pImg, pImgFG, Center);
+                //cvSet(m_Weights,cvScalar(1));
+
+                for(si=0; si<SCALE_NUM; si++)
+                {
+                    double  W = cvDotProduct(m_Weights, m_KernelMeanShiftG[si]);;
+                    int     s = si-SCALE_RANGE;
+                    sum += (float)fabs(W);
+                    news += (float)(s*W);
+                }
+
+                if(sum>0)
+                {
+                    news /= sum;
+                }
+
+                scale = (float)pow((double)SCALE_BASE,(double)news);
+                m_Blob.w *= scale;
+                m_Blob.h *= scale;
+            }   /* Mean shift in scale space. */
+
+            /* Check fo finish: */
+            if(fabs(dx)<0.1 && fabs(dy)<0.1) break;
+
+        }   /* Next iteration. */
+
+        if(m_Alpha>0)
+        {   /* Update histogram: */
+            double  Vol, WM, WC;
+            CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
+            calcHist(pImg, pImgFG, Center, m_KernelHistModel, m_HistCandidate, &m_HistCandidateVolume);
+            Vol = 0.5*(m_HistModelVolume + m_HistCandidateVolume);
+            WM = Vol*(1-m_Alpha)/m_HistModelVolume;
+            WC = Vol*(m_Alpha)/m_HistCandidateVolume;
+            cvAddWeighted(m_HistModel, WM, m_HistCandidate,WC,0,m_HistModel);
+            m_HistModelVolume = (float)cvSum(m_HistModel).val[0];
+        }   /* Update histogram. */
+
+        return &m_Blob;
+
+    };  /* Process */
+
+    virtual void Release(){delete this;};
+}; /*CvBlobTrackerOneMSFGS*/
+
+CvBlobTrackerOne* cvCreateBlobTrackerOneMSFGS()
+{
+    return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFGS;
+}
+
+CvBlobTracker* cvCreateBlobTrackerMSFGS()
+{
+    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFGS);
+}
+