1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
12 // Copyright (C) 2000, Intel Corporation, all rights reserved.
13 // Third party copyrights are property of their respective owners.
15 // Redistribution and use in source and binary forms, with or without modification,
16 // are permitted provided that the following conditions are met:
18 // * Redistribution's of source code must retain the above copyright notice,
19 // this list of conditions and the following disclaimer.
21 // * Redistribution's in binary form must reproduce the above copyright notice,
22 // this list of conditions and the following disclaimer in the documentation
23 // and/or other materials provided with the distribution.
25 // * The name of Intel Corporation may not be used to endorse or promote products
26 // derived from this software without specific prior written permission.
28 // This software is provided by the copyright holders and contributors "as is" and
29 // any express or implied warranties, including, but not limited to, the implied
30 // warranties of merchantability and fitness for a particular purpose are disclaimed.
31 // In no event shall the Intel Corporation or contributors be liable for any direct,
32 // indirect, incidental, special, exemplary, or consequential damages
33 // (including, but not limited to, procurement of substitute goods or services;
34 // loss of use, data, or profits; or business interruption) however caused
35 // and on any theory of liability, whether in contract, strict liability,
36 // or tort (including negligence or otherwise) arising in any way out of
37 // the use of this software, even if advised of the possibility of such damage.
41 #include "precomp.hpp"
43 #define SCALE_BASE 1.1
45 #define SCALE_NUM (2*SCALE_RANGE+1)
46 typedef float DefHistType;
47 #define DefHistTypeMat CV_32F
48 #define HIST_INDEX(_pData) (((_pData)[0]>>m_ByteShift) + (((_pData)[1]>>(m_ByteShift))<<m_BinBit)+((pImgData[2]>>m_ByteShift)<<(m_BinBit*2)))
50 static void calcKernelEpanechnikov(CvMat* pK)
51 { /* Allocate kernel for histogramm creation: */
55 float x0 = 0.5f*(w-1);
56 float y0 = 0.5f*(h-1);
58 for(y=0; y<h; ++y) for(x=0; x<w; ++x)
60 // float r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
61 float r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((x0*x0)+(y0*y0));
62 CV_MAT_ELEM(pK[0],DefHistType, y, x) = (DefHistType)((r2<1)?(1-r2):0);
64 } /* Allocate kernel for histogram creation. */
66 class CvBlobTrackerOneMSFGS:public CvBlobTrackerOne
73 CvMat* m_KernelHistModel;
74 CvMat* m_KernelHistCandidate;
75 CvSize m_KernelMeanShiftSize;
76 CvMat* m_KernelMeanShiftK[SCALE_NUM];
77 CvMat* m_KernelMeanShiftG[SCALE_NUM];
85 float m_HistModelVolume;
86 CvMat* m_HistCandidate;
87 float m_HistCandidateVolume;
91 void ReAllocHist(int Dim, int BinBit)
94 m_ByteShift = 8-BinBit;
96 m_BinNum = (1<<BinBit);
97 m_BinNumTotal = cvRound(pow((double)m_BinNum,(double)m_Dim));
98 if(m_HistModel) cvReleaseMat(&m_HistModel);
99 if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
100 if(m_HistTemp) cvReleaseMat(&m_HistTemp);
101 m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
102 m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
103 m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
104 cvZero(m_HistCandidate);
106 m_HistModelVolume = 0.0f;
107 m_HistCandidateVolume = 0.0f;
110 void ReAllocKernel(int w, int h, float sigma=0.4)
112 double ScaleToObj = sigma*1.39;
113 int kernel_width = cvRound(w/ScaleToObj);
114 int kernel_height = cvRound(h/ScaleToObj);
118 m_ObjSize = cvSize(w,h);
119 m_KernelMeanShiftSize = cvSize(kernel_width,kernel_height);
122 /* Create kernels for histogram calculation: */
123 if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
124 m_KernelHistModel = cvCreateMat(h, w, DefHistTypeMat);
125 calcKernelEpanechnikov(m_KernelHistModel);
126 if(m_KernelHistCandidate) cvReleaseMat(&m_KernelHistCandidate);
127 m_KernelHistCandidate = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
128 calcKernelEpanechnikov(m_KernelHistCandidate);
130 if(m_Weights) cvReleaseMat(&m_Weights);
131 m_Weights = cvCreateMat(kernel_height, kernel_width, CV_32F);
133 for(s=-SCALE_RANGE; s<=SCALE_RANGE; ++s)
134 { /* Allocate kernel for meanshifts in space and scale: */
135 int si = s+SCALE_RANGE;
136 double cur_sigma = sigma * pow(SCALE_BASE,s);
137 double cur_sigma2 = cur_sigma*cur_sigma;
138 double x0 = 0.5*(kernel_width-1);
139 double y0 = 0.5*(kernel_height-1);
140 if(m_KernelMeanShiftK[si]) cvReleaseMat(&m_KernelMeanShiftK[si]);
141 if(m_KernelMeanShiftG[si]) cvReleaseMat(&m_KernelMeanShiftG[si]);
142 m_KernelMeanShiftK[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
143 m_KernelMeanShiftG[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
145 for(y=0; y<kernel_height; ++y)
147 DefHistType* pK = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftK[si][0], y, 0, sizeof(DefHistType) );
148 DefHistType* pG = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftG[si][0], y, 0, sizeof(DefHistType) );
150 for(x=0; x<kernel_width; ++x)
152 double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
153 double sigma12 = cur_sigma2 / 2.56;
154 double sigma22 = cur_sigma2 * 2.56;
155 pK[x] = (DefHistType)(Gaussian2D(r2, sigma12)/sigma12 - Gaussian2D(r2, sigma22)/sigma22);
156 pG[x] = (DefHistType)(Gaussian2D(r2, cur_sigma2/1.6) - Gaussian2D(r2, cur_sigma2*1.6));
160 } /* ReallocKernel */
162 inline double Gaussian2D(double x, double sigma2)
164 return (exp(-x/(2*sigma2)) / (2*3.1415926535897932384626433832795*sigma2) );
167 void calcHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pKernel, CvMat* pHist, DefHistType* pHistVolume)
169 int w = pKernel->width;
170 int h = pKernel->height;
171 DefHistType Volume = 0;
172 int x0 = Center.x - w/2;
173 int y0 = Center.y - h/2;
177 cvSet(pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
184 unsigned char* pImgData = NULL;
185 unsigned char* pMaskData = NULL;
186 DefHistType* pKernelData = NULL;
187 if((y0+y)>=pImg->height) continue;
188 if((y0+y)<0)continue;
189 pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
190 pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
191 pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,0,sizeof(DefHistType));
193 for(x=0; x<w; ++x, pImgData+=3)
195 if((x0+x)>=pImg->width) continue;
196 if((x0+x)<0)continue;
198 if(pMaskData==NULL || pMaskData[x]>128)
200 DefHistType K = pKernelData[x];
201 int index = HIST_INDEX(pImgData);
202 assert(index >= 0 && index < pHist->cols);
204 ((DefHistType*)(pHist->data.ptr))[index] += K;
206 } /* Only masked pixels. */
209 } /* if m_Dim == 3. */
211 if(pHistVolume)pHistVolume[0] = Volume;
215 double calcBhattacharyya()
217 cvMul(m_HistCandidate,m_HistModel,m_HistTemp);
218 cvPow(m_HistTemp,m_HistTemp,0.5);
219 return cvSum(m_HistTemp).val[0] / sqrt(m_HistCandidateVolume*m_HistModelVolume);
220 } /* calcBhattacharyyaCoefficient */
222 void calcWeights(IplImage* pImg, IplImage* pImgFG, CvPoint Center)
226 /* Calculate new position: */
229 int x0 = Center.x - m_KernelMeanShiftSize.width/2;
230 int y0 = Center.y - m_KernelMeanShiftSize.height/2;
233 assert(m_Weights->width == m_KernelMeanShiftSize.width);
234 assert(m_Weights->height == m_KernelMeanShiftSize.height);
236 /* Calcualte shift vector: */
237 for(y=0; y<m_KernelMeanShiftSize.height; ++y)
239 unsigned char* pImgData = NULL;
240 unsigned char* pMaskData = NULL;
241 float* pWData = NULL;
243 if(y+y0 < 0 || y+y0 >= pImg->height) continue;
245 pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
246 pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
247 pWData = (float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,0,sizeof(float));
249 for(x=0; x<m_KernelMeanShiftSize.width; ++x, pImgData+=3)
255 if(x+x0 < 0 || x+x0 >= pImg->width) continue;
257 index = HIST_INDEX(pImgData);
258 assert(index >= 0 && index < m_BinNumTotal);
260 if(m_HistModelVolume>0)
261 HM = ((DefHistType*)m_HistModel->data.ptr)[index]/m_HistModelVolume;
263 if(m_HistCandidateVolume>0)
264 HC = ((DefHistType*)m_HistCandidate->data.ptr)[index]/m_HistCandidateVolume;
266 V = (HC>0)?sqrt(HM / HC):0;
267 V += m_FGWeight*(pMaskData?((pMaskData[x]/255.0f)):0);
268 pWData[x] = (float)MIN(V,100000);
272 } /* if m_Dim == 3. */
276 CvBlobTrackerOneMSFGS()
282 /* Add several parameters for external use: */
283 AddParam("FGWeight", &m_FGWeight);
284 CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
285 AddParam("Alpha", &m_Alpha);
286 CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");
291 m_HistCandidate = NULL;
293 m_KernelHistModel = NULL;
294 m_KernelHistCandidate = NULL;
297 for(i=0; i<SCALE_NUM; ++i)
299 m_KernelMeanShiftK[i] = NULL;
300 m_KernelMeanShiftG[i] = NULL;
302 ReAllocHist(3,5); /* 3D hist, each dimension has 2^5 bins. */
304 SetModuleName("MSFGS");
307 ~CvBlobTrackerOneMSFGS()
310 if(m_HistModel) cvReleaseMat(&m_HistModel);
311 if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
312 if(m_HistTemp) cvReleaseMat(&m_HistTemp);
313 if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
315 for(i=0; i<SCALE_NUM; ++i)
317 if(m_KernelMeanShiftK[i]) cvReleaseMat(&m_KernelMeanShiftK[i]);
318 if(m_KernelMeanShiftG[i]) cvReleaseMat(&m_KernelMeanShiftG[i]);
323 virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
325 int w = cvRound(CV_BLOB_WX(pBlobInit));
326 int h = cvRound(CV_BLOB_WY(pBlobInit));
329 if(w>pImg->width)w=pImg->width;
330 if(h>pImg->height)h=pImg->height;
332 calcHist(pImg, pImgFG, cvPointFrom32f(CV_BLOB_CENTER(pBlobInit)), m_KernelHistModel, m_HistModel, &m_HistModelVolume);
333 m_Blob = pBlobInit[0];
336 virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
342 m_Blob = pBlobPrev[0];
345 for(iter=0; iter<10; ++iter)
347 // float newx=0,newy=0,sum=0;
348 float dx=0,dy=0,sum=0;
351 CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
352 CvSize Size = cvSize(cvRound(m_Blob.w),cvRound(m_Blob.h));
354 if(m_ObjSize.width != Size.width || m_ObjSize.height != Size.height)
355 { /* Reallocate kernels: */
356 ReAllocKernel(Size.width,Size.height);
357 } /* Reallocate kernels. */
359 /* Mean shift in coordinate space: */
360 calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
361 calcWeights(pImg, pImgFG, Center);
363 for(si=1; si<(SCALE_NUM-1); ++si)
365 CvMat* pKernel = m_KernelMeanShiftK[si];
366 float sdx = 0, sdy=0, ssum=0;
367 int s = si-SCALE_RANGE;
368 float factor = (1.0f-( float(s)/float(SCALE_RANGE) )*( float(s)/float(SCALE_RANGE) ));
370 for(y=0; y<m_KernelMeanShiftSize.height; ++y)
371 for(x=0; x<m_KernelMeanShiftSize.width; ++x)
373 float W = *(float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,x,sizeof(float));
374 float K = *(float*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,x,sizeof(float));
376 ssum += (float)fabs(KW);
377 sdx += KW*(x-m_KernelMeanShiftSize.width*0.5f);
378 sdy += KW*(y-m_KernelMeanShiftSize.height*0.5f);
383 sum += ssum * factor;
396 { /* Mean shift in scale space: */
401 Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
402 calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
403 calcWeights(pImg, pImgFG, Center);
404 //cvSet(m_Weights,cvScalar(1));
406 for(si=0; si<SCALE_NUM; si++)
408 double W = cvDotProduct(m_Weights, m_KernelMeanShiftG[si]);;
409 int s = si-SCALE_RANGE;
410 sum1 += (float)fabs(W);
411 news += (float)(s*W);
419 scale = (float)pow((double)SCALE_BASE,(double)news);
422 } /* Mean shift in scale space. */
424 /* Check fo finish: */
425 if(fabs(dx)<0.1 && fabs(dy)<0.1) break;
427 } /* Next iteration. */
430 { /* Update histogram: */
432 CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
433 calcHist(pImg, pImgFG, Center, m_KernelHistModel, m_HistCandidate, &m_HistCandidateVolume);
434 Vol = 0.5*(m_HistModelVolume + m_HistCandidateVolume);
435 WM = Vol*(1-m_Alpha)/m_HistModelVolume;
436 WC = Vol*(m_Alpha)/m_HistCandidateVolume;
437 cvAddWeighted(m_HistModel, WM, m_HistCandidate,WC,0,m_HistModel);
438 m_HistModelVolume = (float)cvSum(m_HistModel).val[0];
439 } /* Update histogram. */
445 virtual void Release(){delete this;}
446 }; /*CvBlobTrackerOneMSFGS*/
448 static CvBlobTrackerOne* cvCreateBlobTrackerOneMSFGS()
450 return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFGS;
453 CvBlobTracker* cvCreateBlobTrackerMSFGS()
455 return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFGS);