X-Git-Url: http://vcs.maemo.org/git/?a=blobdiff_plain;f=src%2Fcvaux%2Fvs%2Fblobtrackingmsfg.cpp;fp=src%2Fcvaux%2Fvs%2Fblobtrackingmsfg.cpp;h=e275535a099ea5ed747a80cdf511d4797a2bff0c;hb=e4c14cdbdf2fe805e79cd96ded236f57e7b89060;hp=0000000000000000000000000000000000000000;hpb=454138ff8a20f6edb9b65a910101403d8b520643;p=opencv diff --git a/src/cvaux/vs/blobtrackingmsfg.cpp b/src/cvaux/vs/blobtrackingmsfg.cpp new file mode 100644 index 0000000..e275535 --- /dev/null +++ b/src/cvaux/vs/blobtrackingmsfg.cpp @@ -0,0 +1,1180 @@ +/*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_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; yw); + 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= 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(wpImg->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; iterwidth); + assert(m_ObjSize.height < pImg->height); + + /* Calculate shift vector: */ + for(y=0; yheight; ++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; xwidth; ++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= 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= 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; hx; + 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_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= 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;iwidth; + 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; p0) + { + 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