--- /dev/null
+/*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"
+
+// Uncomment to trade flexibility for speed
+//#define CONST_HIST_SIZE
+
+// Uncomment to get some performance stats in stderr
+//#define REPORT_TICKS
+
+#ifdef CONST_HIST_SIZE
+#define m_BinBit 5
+#define m_ByteShift 3
+#endif
+
+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)))
+
+class DefHist
+{
+public:
+ CvMat* m_pHist;
+ DefHistType m_HistVolume;
+ DefHist(int BinNum=0)
+ {
+ m_pHist = NULL;
+ m_HistVolume = 0;
+ Resize(BinNum);
+ }
+
+ ~DefHist()
+ {
+ if(m_pHist)cvReleaseMat(&m_pHist);
+ }
+
+ void Resize(int BinNum)
+ {
+ if(m_pHist)cvReleaseMat(&m_pHist);
+ if(BinNum>0)
+ {
+ m_pHist = cvCreateMat(1, BinNum, DefHistTypeMat);
+ cvZero(m_pHist);
+ }
+ m_HistVolume = 0;
+ }
+
+ void Update(DefHist* pH, float W)
+ { /* Update histogram: */
+ double Vol, WM, WC;
+ Vol = 0.5*(m_HistVolume + pH->m_HistVolume);
+ WM = Vol*(1-W)/m_HistVolume;
+ WC = Vol*(W)/pH->m_HistVolume;
+ cvAddWeighted(m_pHist, WM, pH->m_pHist,WC,0,m_pHist);
+ m_HistVolume = (float)cvSum(m_pHist).val[0];
+ } /* Update histogram: */
+};
+
+class CvBlobTrackerOneMSFG:public CvBlobTrackerOne
+{
+protected:
+ int m_BinNumTotal; /* protected only for parralel MSPF */
+ CvSize m_ObjSize;
+
+ void ReAllocKernel(int w, int h)
+ {
+ int x,y;
+ float x0 = 0.5f*(w-1);
+ float y0 = 0.5f*(h-1);
+ assert(w>0);
+ assert(h>0);
+ m_ObjSize = cvSize(w,h);
+
+ if(m_KernelHist) cvReleaseMat(&m_KernelHist);
+ if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
+ m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
+ m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);
+
+ for(y=0; y<h; ++y) for(x=0; x<w; ++x)
+ {
+ double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
+// double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
+ CV_MAT_ELEM(m_KernelHist[0],DefHistType, y, x) = (DefHistType)GetKernelHist(r2);
+ CV_MAT_ELEM(m_KernelMeanShift[0],DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
+ }
+ }
+
+private:
+ /* Parameters: */
+ int m_IterNum;
+ float m_FGWeight;
+ float m_Alpha;
+ CvMat* m_KernelHist;
+ CvMat* m_KernelMeanShift;
+#ifndef CONST_HIST_SIZE
+ int m_BinBit;
+ int m_ByteShift;
+#endif
+ int m_BinNum;
+ int m_Dim;
+ /*
+ CvMat* m_HistModel;
+ float m_HistModelVolume;
+ CvMat* m_HistCandidate;
+ float m_HistCandidateVolume;
+ CvMat* m_HistTemp;
+ */
+ DefHist m_HistModel;
+ DefHist m_HistCandidate;
+ DefHist m_HistTemp;
+
+ CvBlob m_Blob;
+ int m_Collision;
+
+ void ReAllocHist(int Dim, int BinBit)
+ {
+#ifndef CONST_HIST_SIZE
+ m_BinBit = BinBit;
+ m_ByteShift = 8-BinBit;
+#endif
+ 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;
+ */
+ m_HistCandidate.Resize(m_BinNumTotal);
+ m_HistModel.Resize(m_BinNumTotal);
+ m_HistTemp.Resize(m_BinNumTotal);
+ }
+
+ double GetKernelHist(double r2)
+ {
+ return (r2 < 1) ? 1 - r2 : 0;
+ }
+
+ double GetKernelMeanShift(double r2)
+ {
+ return (r2<1) ? 1 : 0;
+ }
+
+// void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
+// void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)
+
+ void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
+ {
+ int UsePrecalculatedKernel = 0;
+ int BW = cvRound(pBlob->w);
+ int BH = cvRound(pBlob->h);
+ DefHistType Volume = 0;
+ int x0 = cvRound(pBlob->x - BW*0.5);
+ int y0 = cvRound(pBlob->y - BH*0.5);
+ int x,y;
+
+ UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;
+
+ //cvZero(pHist);
+ cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
+ Volume = 1;
+
+ assert(BW < pImg->width);
+ assert(BH < pImg->height);
+ if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
+ if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
+ if(x0<0){ x0=0;}
+ if(y0<0){ y0=0;}
+
+ if(m_Dim == 3)
+ {
+ for(y=0; y<BH; ++y)
+ {
+ unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
+ unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
+ DefHistType* pKernelData = NULL;
+
+ if(UsePrecalculatedKernel)
+ {
+ pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
+ }
+
+ for(x=0; x<BW; ++x, pImgData+=3)
+ {
+ DefHistType K;
+ int index = HIST_INDEX(pImgData);
+ assert(index >= 0 && index < pHist->m_pHist->cols);
+
+ if(UsePrecalculatedKernel)
+ {
+ K = pKernelData[x];
+ }
+ else
+ {
+ float dx = (x+x0-pBlob->x)/(pBlob->w*0.5f);
+ float dy = (y+y0-pBlob->y)/(pBlob->h*0.5f);
+ double r2 = dx*dx+dy*dy;
+ K = (float)GetKernelHist(r2);
+ }
+
+ if(pMaskData)
+ {
+ K *= pMaskData[x]*0.003921568627450980392156862745098f;
+ }
+ Volume += K;
+ ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;
+
+ } /* Next column. */
+ } /* Next row. */
+ } /* if m_Dim == 3. */
+
+ pHist->m_HistVolume = Volume;
+
+ }; /* CollectHist */
+
+ double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL)
+ {
+ if(pHM==NULL) pHM = &m_HistModel;
+ if(pHC==NULL) pHC = &m_HistCandidate;
+ if(pHT==NULL) pHT = &m_HistTemp;
+ if(pHC->m_HistVolume*pHM->m_HistVolume > 0)
+ {
+#if 0
+ // Use CV functions:
+ cvMul(pHM->m_pHist,pHC->m_pHist,pHT->m_pHist);
+ cvPow(pHT->m_pHist,pHT->m_pHist,0.5);
+ return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
+#else
+ // Do computations manually and let autovectorizer do the job:
+ DefHistType *hm, *hc, *ht;
+ double sum;
+ int size;
+ hm=(DefHistType *)(pHM->m_pHist->data.ptr);
+ hc=(DefHistType *)(pHC->m_pHist->data.ptr);
+ ht=(DefHistType *)(pHT->m_pHist->data.ptr);
+ size = pHM->m_pHist->width*pHM->m_pHist->height;
+ sum = 0.;
+ for(int i = 0; i < size; i++ )
+ {
+ sum += sqrt(hm[i]*hc[i]);
+ }
+ return sum / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
+#endif
+ }
+ return 0;
+ } /* calcBhattacharyyaCoefficient. */
+
+protected:
+ // double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
+ double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist=NULL, int /*thread_number*/ = 0)
+ {
+ if(pHist==NULL)pHist = &m_HistTemp;
+ CollectHist(pImg, pImgFG, pBlob, pHist);
+ return calcBhattacharyya(&m_HistModel, pHist, pHist);
+ }
+
+ void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob)
+ {
+ if(m_Alpha>0 && !m_Collision)
+ { /* Update histogram: */
+ CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
+ m_HistModel.Update(&m_HistCandidate, m_Alpha);
+ } /* Update histogram. */
+
+ } /* UpdateModelHist */
+
+public:
+ CvBlobTrackerOneMSFG()
+ {
+
+ /* Add several parameters for external use: */
+ m_FGWeight = 2;
+ AddParam("FGWeight", &m_FGWeight);
+ CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
+
+ m_Alpha = 0.01f;
+ AddParam("Alpha", &m_Alpha);
+ CommentParam("Alpha","Coefficient for model histogram updating (0 - hist is not upated)");
+
+ m_IterNum = 10;
+ AddParam("IterNum", &m_IterNum);
+ CommentParam("IterNum","Maximal number of iteration in meanshift operation");
+
+ /* Initialize internal data: */
+ m_Collision = 0;
+// m_BinBit=0;
+ m_Dim = 0;
+ /*
+ m_HistModel = NULL;
+ m_HistCandidate = NULL;
+ m_HistTemp = NULL;
+ */
+ m_KernelHist = NULL;
+ m_KernelMeanShift = NULL;
+ ReAllocHist(3,5); /* 3D hist, each dim has 2^5 bins*/
+
+ SetModuleName("MSFG");
+ }
+
+ ~CvBlobTrackerOneMSFG()
+ {
+ /*
+ if(m_HistModel) cvReleaseMat(&m_HistModel);
+ if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
+ if(m_HistTemp) cvReleaseMat(&m_HistTemp);
+ */
+ if(m_KernelHist) cvReleaseMat(&m_KernelHist);
+ if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
+ }
+
+ /* 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<CV_BLOB_MINW)w=CV_BLOB_MINW;
+ if(h<CV_BLOB_MINH)h=CV_BLOB_MINH;
+ if(pImg)
+ {
+ if(w>pImg->width)w=pImg->width;
+ if(h>pImg->height)h=pImg->height;
+ }
+ ReAllocKernel(w,h);
+ if(pImg)
+ CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
+ m_Blob = pBlobInit[0];
+ };
+
+ virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
+ {
+ int iter;
+
+ if(pBlobPrev)
+ {
+ m_Blob = pBlobPrev[0];
+ }
+
+ { /* Check blob size and realloc kernels if it is necessary: */
+ int w = cvRound(m_Blob.w);
+ int h = cvRound(m_Blob.h);
+ if( w != m_ObjSize.width || h!=m_ObjSize.height)
+ {
+ ReAllocKernel(w,h);
+ /* after this ( w != m_ObjSize.width || h!=m_ObjSize.height) shoiuld be false */
+ }
+ } /* Check blob size and realloc kernels if it is necessary: */
+
+
+ for(iter=0; iter<m_IterNum; ++iter)
+ {
+ float newx=0,newy=0,sum=0;
+ //int x,y;
+ double B0;
+
+ //CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
+ CollectHist(pImg, NULL, &m_Blob, &m_HistCandidate);
+ B0 = calcBhattacharyya();
+
+ if(m_Wnd)if(CV_BLOB_ID(pBlobPrev)==0 && iter == 0)
+ { /* Debug: */
+ IplImage* pW = cvCloneImage(pImgFG);
+ IplImage* pWFG = cvCloneImage(pImgFG);
+ int x,y;
+
+ cvZero(pW);
+ cvZero(pWFG);
+
+ assert(m_ObjSize.width < pImg->width);
+ assert(m_ObjSize.height < pImg->height);
+
+ /* Calculate shift vector: */
+ for(y=0; y<pImg->height; ++y)
+ {
+ unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y,0);
+ unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y,0)):NULL;
+
+ for(x=0; x<pImg->width; ++x, pImgData+=3)
+ {
+ int xk = cvRound(x-(m_Blob.x-m_Blob.w*0.5));
+ int yk = cvRound(y-(m_Blob.y-m_Blob.h*0.5));
+ double HM = 0;
+ double HC = 0;
+ double K;
+ int index = HIST_INDEX(pImgData);
+ assert(index >= 0 && index < m_BinNumTotal);
+
+ if(fabs(x-m_Blob.x)>m_Blob.w*0.6) continue;
+ if(fabs(y-m_Blob.y)>m_Blob.h*0.6) continue;
+
+ if(xk < 0 || xk >= m_KernelMeanShift->cols) continue;
+ if(yk < 0 || yk >= m_KernelMeanShift->rows) continue;
+
+ if(m_HistModel.m_HistVolume>0)
+ HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
+
+ if(m_HistCandidate.m_HistVolume>0)
+ HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
+
+ K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],yk,xk,sizeof(DefHistType));
+
+ if(HC>0)
+ {
+ double V = sqrt(HM / HC);
+ int Vi = cvRound(V * 64);
+ if(Vi < 0) Vi = 0;
+ if(Vi > 255) Vi = 255;
+ CV_IMAGE_ELEM(pW,uchar,y,x) = (uchar)Vi;
+
+ V += m_FGWeight*(pMaskData?(pMaskData[x]/255.0f):0);
+ V*=K;
+ Vi = cvRound(V * 64);
+ if(Vi < 0) Vi = 0;
+ if(Vi > 255) Vi = 255;
+ CV_IMAGE_ELEM(pWFG,uchar,y,x) = (uchar)Vi;
+ }
+
+ } /* Next column. */
+ } /* Next row. */
+
+ //cvNamedWindow("MSFG_W",0);
+ //cvShowImage("MSFG_W",pW);
+ //cvNamedWindow("MSFG_WFG",0);
+ //cvShowImage("MSFG_WFG",pWFG);
+ //cvNamedWindow("MSFG_FG",0);
+ //cvShowImage("MSFG_FG",pImgFG);
+
+ //cvSaveImage("MSFG_W.bmp",pW);
+ //cvSaveImage("MSFG_WFG.bmp",pWFG);
+ //cvSaveImage("MSFG_FG.bmp",pImgFG);
+
+ } /* Debug. */
+
+
+ /* Calculate new position by meanshift: */
+
+ /* Calculate new position: */
+ if(m_Dim == 3)
+ {
+ int x0 = cvRound(m_Blob.x - m_ObjSize.width*0.5);
+ int y0 = cvRound(m_Blob.y - m_ObjSize.height*0.5);
+ int x,y;
+
+ assert(m_ObjSize.width < pImg->width);
+ assert(m_ObjSize.height < pImg->height);
+
+ /* Crop blob bounds: */
+ if((x0+m_ObjSize.width)>=pImg->width) x0=pImg->width-m_ObjSize.width-1;
+ if((y0+m_ObjSize.height)>=pImg->height) y0=pImg->height-m_ObjSize.height-1;
+ if(x0<0){ x0=0;}
+ if(y0<0){ y0=0;}
+
+ /* Calculate shift vector: */
+ for(y=0; y<m_ObjSize.height; ++y)
+ {
+ unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
+ unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
+ DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],y,0,sizeof(DefHistType));
+
+ for(x=0; x<m_ObjSize.width; ++x, pImgData+=3)
+ {
+ DefHistType K = pKernelData[x];
+ double HM = 0;
+ double HC = 0;
+ int index = HIST_INDEX(pImgData);
+ assert(index >= 0 && index < m_BinNumTotal);
+
+ if(m_HistModel.m_HistVolume>0)
+ HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
+
+ if(m_HistCandidate.m_HistVolume>0)
+ HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
+
+ if(HC>0)
+ {
+ double V = sqrt(HM / HC);
+ if(!m_Collision && m_FGWeight>0 && pMaskData)
+ {
+ V += m_FGWeight*(pMaskData[x]/255.0f);
+ }
+ K *= (float)MIN(V,100000.);
+ }
+
+ sum += K;
+ newx += K*x;
+ newy += K*y;
+ } /* Next column. */
+ } /* Next row. */
+
+ if(sum > 0)
+ {
+ newx /= sum;
+ newy /= sum;
+ }
+ newx += x0;
+ newy += y0;
+
+ } /* if m_Dim == 3. */
+
+ /* Calculate new position by meanshift: */
+
+ for(;;)
+ { /* Iterate using bahattcharrya coefficient: */
+ double B1;
+ CvBlob B = m_Blob;
+// CvPoint NewCenter = cvPoint(cvRound(newx),cvRound(newy));
+ B.x = newx;
+ B.y = newy;
+ CollectHist(pImg, NULL, &B, &m_HistCandidate);
+ B1 = calcBhattacharyya();
+ if(B1 > B0) break;
+ newx = 0.5f*(newx+m_Blob.x);
+ newy = 0.5f*(newy+m_Blob.y);
+ if(fabs(newx-m_Blob.x)<0.1 && fabs(newy-m_Blob.y)<0.1) break;
+ } /* Iterate using bahattcharrya coefficient. */
+
+ if(fabs(newx-m_Blob.x)<0.5 && fabs(newy-m_Blob.y)<0.5) break;
+ m_Blob.x = newx;
+ m_Blob.y = newy;
+ } /* Next iteration. */
+
+ while(!m_Collision && m_FGWeight>0)
+ { /* Update size if no collision. */
+ float Alpha = 0.04f;
+ CvBlob NewBlob;
+ double M00,X,Y,XX,YY;
+ CvMoments m;
+ CvRect r;
+ CvMat mat;
+
+ r.width = cvRound(m_Blob.w*1.5+0.5);
+ r.height = cvRound(m_Blob.h*1.5+0.5);
+ r.x = cvRound(m_Blob.x - 0.5*r.width);
+ r.y = cvRound(m_Blob.y - 0.5*r.height);
+ if(r.x < 0) break;
+ if(r.y < 0) break;
+ if(r.x+r.width >= pImgFG->width) break;
+ if(r.y+r.height >= pImgFG->height) break;
+ if(r.height < 5 || r.width < 5) break;
+
+ cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
+ M00 = cvGetSpatialMoment( &m, 0, 0 );
+ if(M00 <= 0 ) break;
+ X = cvGetSpatialMoment( &m, 1, 0 )/M00;
+ Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
+ XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
+ YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
+ NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
+
+ NewBlob.w = Alpha*NewBlob.w+m_Blob.w*(1-Alpha);
+ NewBlob.h = Alpha*NewBlob.h+m_Blob.h*(1-Alpha);
+
+ m_Blob.w = MAX(NewBlob.w,5);
+ m_Blob.h = MAX(NewBlob.h,5);
+ break;
+
+ } /* Update size if no collision. */
+
+ return &m_Blob;
+
+ }; /* CvBlobTrackerOneMSFG::Process */
+
+ virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL)
+ {
+ double S = 0.2;
+ double B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
+ return exp((B-1)/(2*S));
+
+ }; /*CvBlobTrackerOneMSFG::*/
+
+ virtual void Update(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
+ { /* Update histogram: */
+ UpdateModelHist(pImg, pImgFG, pBlob?pBlob:&m_Blob);
+ } /*CvBlobTrackerOneMSFG::*/
+
+ virtual void Release(){delete this;};
+ virtual void SetCollision(int CollisionFlag)
+ {
+ m_Collision = CollisionFlag;
+ }
+ virtual void SaveState(CvFileStorage* fs)
+ {
+ cvWriteStruct(fs, "Blob", &m_Blob, "ffffi");
+ cvWriteInt(fs,"Collision", m_Collision);
+ cvWriteInt(fs,"HistVolume", cvRound(m_HistModel.m_HistVolume));
+ cvWrite(fs,"Hist", m_HistModel.m_pHist);
+ };
+ virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
+ {
+ CvMat* pM;
+ cvReadStructByName(fs, node, "Blob",&m_Blob, "ffffi");
+ m_Collision = cvReadIntByName(fs,node,"Collision",m_Collision);
+ pM = (CvMat*)cvRead(fs,cvGetFileNodeByName(fs,node,"Hist"));
+ if(pM)
+ {
+ m_HistModel.m_pHist = pM;
+ m_HistModel.m_HistVolume = (float)cvSum(pM).val[0];
+ }
+ };
+
+}; /*CvBlobTrackerOneMSFG*/
+
+#if 0
+void CvBlobTrackerOneMSFG::CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
+{
+ int UsePrecalculatedKernel = 0;
+ int BW = cvRound(pBlob->w);
+ int BH = cvRound(pBlob->h);
+ DefHistType Volume = 0;
+ int x0 = cvRound(pBlob->x - BW*0.5);
+ int y0 = cvRound(pBlob->y - BH*0.5);
+ int x,y;
+
+ UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;
+
+ //cvZero(pHist);
+ cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
+ Volume = 1;
+
+ assert(BW < pImg->width);
+ assert(BH < pImg->height);
+ if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
+ if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
+ if(x0<0){ x0=0;}
+ if(y0<0){ y0=0;}
+
+ if(m_Dim == 3)
+ {
+ for(y=0; y<BH; ++y)
+ {
+ unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
+ unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
+ DefHistType* pKernelData = NULL;
+
+ if(UsePrecalculatedKernel)
+ {
+ pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
+ }
+
+ for(x=0; x<BW; ++x, pImgData+=3)
+ {
+ DefHistType K;
+ int index = HIST_INDEX(pImgData);
+ assert(index >= 0 && index < pHist->m_pHist->cols);
+
+ if(UsePrecalculatedKernel)
+ {
+ K = pKernelData[x];
+ }
+ else
+ {
+ float dx = (x+x0-pBlob->x)/(pBlob->w*0.5);
+ float dy = (y+y0-pBlob->y)/(pBlob->h*0.5);
+ double r2 = dx*dx+dy*dy;
+ K = GetKernelHist(r2);
+ }
+
+ if(pMaskData)
+ {
+ K *= pMaskData[x]*0.003921568627450980392156862745098;
+ }
+ Volume += K;
+ ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;
+
+ } /* Next column. */
+ } /* Next row. */
+ } /* if m_Dim == 3. */
+
+ pHist->m_HistVolume = Volume;
+
+}; /* CollectHist */
+#endif
+
+CvBlobTrackerOne* cvCreateBlobTrackerOneMSFG()
+{
+ return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFG;
+}
+
+CvBlobTracker* cvCreateBlobTrackerMSFG()
+{
+ return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFG);
+}
+
+/* Create specific tracker without FG
+ * usin - just simple mean-shift tracker: */
+class CvBlobTrackerOneMS:public CvBlobTrackerOneMSFG
+{
+public:
+ CvBlobTrackerOneMS()
+ {
+ SetParam("FGWeight",0);
+ DelParam("FGWeight");
+ SetModuleName("MS");
+ };
+};
+
+CvBlobTrackerOne* cvCreateBlobTrackerOneMS()
+{
+ return (CvBlobTrackerOne*) new CvBlobTrackerOneMS;
+}
+
+CvBlobTracker* cvCreateBlobTrackerMS()
+{
+ return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMS);
+}
+
+typedef struct DefParticle
+{
+ CvBlob blob;
+ float Vx,Vy;
+ double W;
+} DefParticle;
+
+class CvBlobTrackerOneMSPF:public CvBlobTrackerOneMS
+{
+private:
+ /* parameters */
+ int m_ParticleNum;
+ float m_UseVel;
+ float m_SizeVar;
+ float m_PosVar;
+
+ CvSize m_ImgSize;
+ CvBlob m_Blob;
+ DefParticle* m_pParticlesPredicted;
+ DefParticle* m_pParticlesResampled;
+ CvRNG m_RNG;
+#ifdef _OPENMP
+ int m_ThreadNum;
+ DefHist* m_HistForParalel;
+#endif
+
+public:
+ virtual void SaveState(CvFileStorage* fs)
+ {
+ CvBlobTrackerOneMS::SaveState(fs);
+ cvWriteInt(fs,"ParticleNum",m_ParticleNum);
+ cvWriteStruct(fs,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd",m_ParticleNum);
+ cvWriteStruct(fs,"ParticlesResampled",m_pParticlesResampled,"ffffiffd",m_ParticleNum);
+ };
+
+ virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
+ {
+ //CvMat* pM;
+ CvBlobTrackerOneMS::LoadState(fs,node);
+ m_ParticleNum = cvReadIntByName(fs,node,"ParticleNum",m_ParticleNum);
+ if(m_ParticleNum>0)
+ {
+ Realloc();
+ printf("sizeof(DefParticle) is %d\n", (int)sizeof(DefParticle));
+ cvReadStructByName(fs,node,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd");
+ cvReadStructByName(fs,node,"ParticlesResampled",m_pParticlesResampled,"ffffiffd");
+ }
+ };
+ CvBlobTrackerOneMSPF()
+ {
+ m_pParticlesPredicted = NULL;
+ m_pParticlesResampled = NULL;
+ m_ParticleNum = 200;
+
+ AddParam("ParticleNum",&m_ParticleNum);
+ CommentParam("ParticleNum","Number of particles");
+ Realloc();
+
+ m_UseVel = 0;
+ AddParam("UseVel",&m_UseVel);
+ CommentParam("UseVel","Percent of particles which use velocity feature");
+
+ m_SizeVar = 0.05f;
+ AddParam("SizeVar",&m_SizeVar);
+ CommentParam("SizeVar","Size variation (in object size)");
+
+ m_PosVar = 0.2f;
+ AddParam("PosVar",&m_PosVar);
+ CommentParam("PosVar","Position variation (in object size)");
+
+ m_RNG = cvRNG(0);
+
+ SetModuleName("MSPF");
+
+#ifdef _OPENMP
+ {
+ m_ThreadNum = omp_get_num_procs();
+ m_HistForParalel = new DefHist[m_ThreadNum];
+ }
+#endif
+ }
+
+ ~CvBlobTrackerOneMSPF()
+ {
+ if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
+ if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
+#ifdef _OPENMP
+ if(m_HistForParalel) delete[] m_HistForParalel;
+#endif
+ }
+
+private:
+ void Realloc()
+ {
+ if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
+ if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
+ m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
+ m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
+ }; /* Realloc*/
+
+ void DrawDebug(IplImage* pImg, IplImage* /*pImgFG*/)
+ {
+ int k;
+ for(k=0; k<2; ++k)
+ {
+ DefParticle* pBP = k?m_pParticlesResampled:m_pParticlesPredicted;
+ //const char* name = k?"MSPF resampled particle":"MSPF Predicted particle";
+ IplImage* pI = cvCloneImage(pImg);
+ int h,hN = m_ParticleNum;
+ CvBlob C = cvBlob(0,0,0,0);
+ double WS = 0;
+ for(h=0; h<hN; ++h)
+ {
+ CvBlob B = pBP[h].blob;
+ int CW = cvRound(255*pBP[h].W);
+ CvBlob* pB = &B;
+ int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
+ CvSize s = cvSize(MAX(1,x), MAX(1,y));
+ double W = pBP[h].W;
+ C.x += pB->x;
+ C.y += pB->y;
+ C.w += pB->w;
+ C.h += pB->h;
+ WS+=W;
+
+ s = cvSize(1,1);
+ cvEllipse( pI,
+ cvPointFrom32f(CV_BLOB_CENTER(pB)),
+ s,
+ 0, 0, 360,
+ CV_RGB(CW,0,0), 1 );
+
+ } /* Next hypothesis. */
+
+ C.x /= hN;
+ C.y /= hN;
+ C.w /= hN;
+ C.h /= hN;
+
+ cvEllipse( pI,
+ cvPointFrom32f(CV_BLOB_CENTER(&C)),
+ cvSize(cvRound(C.w*0.5),cvRound(C.h*0.5)),
+ 0, 0, 360,
+ CV_RGB(0,0,255), 1 );
+
+ cvEllipse( pI,
+ cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
+ cvSize(cvRound(m_Blob.w*0.5),cvRound(m_Blob.h*0.5)),
+ 0, 0, 360,
+ CV_RGB(0,255,0), 1 );
+
+ //cvNamedWindow(name,0);
+ //cvShowImage(name,pI);
+ cvReleaseImage(&pI);
+ } /* */
+
+ //printf("Blob %d, point (%.1f,%.1f) size (%.1f,%.1f)\n",m_Blob.ID,m_Blob.x,m_Blob.y,m_Blob.w,m_Blob.h);
+ } /* ::DrawDebug */
+
+private:
+ void Prediction()
+ {
+ int p;
+ for(p=0; p<m_ParticleNum; ++p)
+ { /* "Prediction" of particle: */
+ //double t;
+ float r[5];
+ CvMat rm = cvMat(1,5,CV_32F,r);
+ cvRandArr(&m_RNG,&rm,CV_RAND_NORMAL,cvScalar(0),cvScalar(1));
+
+ m_pParticlesPredicted[p] = m_pParticlesResampled[p];
+
+ if(cvRandReal(&m_RNG)<0.5)
+ { /* Half of particles will predict based on external blob: */
+ m_pParticlesPredicted[p].blob = m_Blob;
+ }
+
+ if(cvRandReal(&m_RNG)<m_UseVel)
+ { /* Predict moving particle by usual way by using speed: */
+ m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
+ m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
+ }
+ else
+ { /* Stop several particles: */
+ m_pParticlesPredicted[p].Vx = 0;
+ m_pParticlesPredicted[p].Vy = 0;
+ }
+
+ { /* Update position: */
+ float S = (m_Blob.w + m_Blob.h)*0.5f;
+ m_pParticlesPredicted[p].blob.x += m_PosVar*S*r[0];
+ m_pParticlesPredicted[p].blob.y += m_PosVar*S*r[1];
+
+ /* Update velocity: */
+ m_pParticlesPredicted[p].Vx += (float)(m_PosVar*S*0.1*r[3]);
+ m_pParticlesPredicted[p].Vy += (float)(m_PosVar*S*0.1*r[4]);
+ }
+
+ /* Update size: */
+ m_pParticlesPredicted[p].blob.w *= (1+m_SizeVar*r[2]);
+ m_pParticlesPredicted[p].blob.h *= (1+m_SizeVar*r[2]);
+
+ /* Truncate size of particle: */
+ if(m_pParticlesPredicted[p].blob.w > m_ImgSize.width*0.5f)
+ {
+ m_pParticlesPredicted[p].blob.w = m_ImgSize.width*0.5f;
+ }
+
+ if(m_pParticlesPredicted[p].blob.h > m_ImgSize.height*0.5f)
+ {
+ m_pParticlesPredicted[p].blob.h = m_ImgSize.height*0.5f;
+ }
+
+ if(m_pParticlesPredicted[p].blob.w < 1 )
+ {
+ m_pParticlesPredicted[p].blob.w = 1;
+ }
+
+ if(m_pParticlesPredicted[p].blob.h < 1)
+ {
+ m_pParticlesPredicted[p].blob.h = 1;
+ }
+ } /* "Prediction" of particle. */
+ } /* Prediction */
+
+ void UpdateWeightsMS(IplImage* pImg, IplImage* /*pImgFG*/)
+ {
+ int p;
+#ifdef _OPENMP
+ if( m_HistForParalel[0].m_pHist==NULL || m_HistForParalel[0].m_pHist->cols != m_BinNumTotal)
+ {
+ int t;
+ for(t=0; t<m_ThreadNum; ++t)
+ m_HistForParalel[t].Resize(m_BinNumTotal);
+ }
+#endif
+
+#ifdef _OPENMP
+#pragma omp parallel for num_threads(m_ThreadNum) schedule(runtime)
+#endif
+ for(p=0;p<m_ParticleNum;++p)
+ { /* Calculate weights for particles: */
+ double S = 0.2;
+ double B = 0;
+#ifdef _OPENMP
+ assert(omp_get_thread_num()<m_ThreadNum);
+#endif
+
+ B = GetBhattacharyya(
+ pImg, NULL,
+ &(m_pParticlesPredicted[p].blob)
+#ifdef _OPENMP
+ ,&(m_HistForParalel[omp_get_thread_num()])
+#endif
+ );
+ m_pParticlesPredicted[p].W *= exp((B-1)/(2*S));
+
+ } /* Calculate weights for particles. */
+ }
+
+ void UpdateWeightsCC(IplImage* /*pImg*/, IplImage* /*pImgFG*/)
+ {
+ int p;
+#ifdef _OPENMP
+#pragma omp parallel for
+#endif
+ for(p=0; p<m_ParticleNum; ++p)
+ { /* Calculate weights for particles: */
+ double W = 1;
+ m_pParticlesPredicted[p].W *= W;
+ } /* Calculate weights for particles. */
+ }
+
+ void Resample()
+ { /* Resample particle: */
+ int p;
+ double Sum = 0;
+
+ for(p=0; p<m_ParticleNum; ++p)
+ {
+ Sum += m_pParticlesPredicted[p].W;
+ }
+
+ for(p=0; p<m_ParticleNum; ++p)
+ {
+ double T = Sum * cvRandReal(&m_RNG); /* Set current random threshold for cululative weight. */
+ int p2;
+ double Sum2 = 0;
+
+ for(p2=0; p2<m_ParticleNum; ++p2)
+ {
+ Sum2 += m_pParticlesPredicted[p2].W;
+ if(Sum2 >= T)break;
+ }
+
+ if(p2>=m_ParticleNum)p2=m_ParticleNum-1;
+ m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
+ m_pParticlesResampled[p].W = 1;
+
+ } /* Find next particle. */
+ } /* Resample particle. */
+
+
+public:
+ virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
+ {
+ int i;
+ CvBlobTrackerOneMSFG::Init(pBlobInit, pImg, pImgFG);
+ DefParticle PP;
+ PP.W = 1;
+ PP.Vx = 0;
+ PP.Vy = 0;
+ PP.blob = pBlobInit[0];
+ for(i=0;i<m_ParticleNum;++i)
+ {
+ m_pParticlesPredicted[i] = PP;
+ m_pParticlesResampled[i] = PP;
+ }
+ m_Blob = pBlobInit[0];
+
+ } /* CvBlobTrackerOneMSPF::Init*/
+
+ virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
+ {
+ int p;
+
+ m_ImgSize.width = pImg->width;
+ m_ImgSize.height = pImg->height;
+
+
+ m_Blob = pBlobPrev[0];
+
+ { /* Check blob size and realloc kernels if it is necessary: */
+ int w = cvRound(m_Blob.w);
+ int h = cvRound(m_Blob.h);
+ if( w != m_ObjSize.width || h!=m_ObjSize.height)
+ {
+ ReAllocKernel(w,h);
+ /* After this ( w != m_ObjSize.width || h!=m_ObjSize.height) should be false. */
+ }
+ } /* Check blob size and realloc kernels if it is necessary. */
+
+ Prediction();
+
+#ifdef REPORT_TICKS
+ int64 ticks = cvGetTickCount();
+#endif
+
+ UpdateWeightsMS(pImg, pImgFG);
+
+#ifdef REPORT_TICKS
+ ticks = cvGetTickCount() - ticks;
+ fprintf(stderr, "PF UpdateWeights, %d ticks\n", (int)ticks);
+ ticks = cvGetTickCount();
+#endif
+
+ Resample();
+
+#ifdef REPORT_TICKS
+ ticks = cvGetTickCount() - ticks;
+ fprintf(stderr, "PF Resampling, %d ticks\n", (int)ticks);
+#endif
+
+ { /* Find average result: */
+ float x = 0;
+ float y = 0;
+ float w = 0;
+ float h = 0;
+ float Sum = 0;
+
+ DefParticle* pP = m_pParticlesResampled;
+
+ for(p=0; p<m_ParticleNum; ++p)
+ {
+ float W = (float)pP[p].W;
+ x += W*pP[p].blob.x;
+ y += W*pP[p].blob.y;
+ w += W*pP[p].blob.w;
+ h += W*pP[p].blob.h;
+ Sum += W;
+ }
+
+ if(Sum>0)
+ {
+ m_Blob.x = x / Sum;
+ m_Blob.y = y / Sum;
+ m_Blob.w = w / Sum;
+ m_Blob.h = h / Sum;
+ }
+ } /* Find average result. */
+
+ if(m_Wnd)
+ {
+ DrawDebug(pImg, pImgFG);
+ }
+
+ return &m_Blob;
+
+ } /* CvBlobTrackerOneMSPF::Process */
+
+ virtual void SkipProcess(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
+ {
+ int p;
+ for(p=0; p<m_ParticleNum; ++p)
+ {
+ m_pParticlesResampled[p].blob = pBlob[0];
+ m_pParticlesResampled[p].Vx = 0;
+ m_pParticlesResampled[p].Vy = 0;
+ m_pParticlesResampled[p].W = 1;
+ }
+ }
+
+ virtual void Release(){delete this;};
+ virtual void ParamUpdate()
+ {
+ Realloc();
+ }
+
+}; /* CvBlobTrackerOneMSPF */
+
+CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF()
+{
+ return (CvBlobTrackerOne*) new CvBlobTrackerOneMSPF;
+}
+
+CvBlobTracker* cvCreateBlobTrackerMSPF()
+{
+ return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSPF);
+}
+
projects / opencv / blobdiff