git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackingmsfg.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   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.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //
  12 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  13 // Third party copyrights are property of their respective owners.
  14 //
  15 // Redistribution and use in source and binary forms, with or without modification,
  16 // are permitted provided that the following conditions are met:
  17 //
  18 //   * Redistribution's of source code must retain the above copyright notice,
  19 //     this list of conditions and the following disclaimer.
  20 //
  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.
  24 //
  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.
  27 //
  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.
  38 //
  39 //M*/
  40
  41 #include "_cvaux.h"
  42
  43 // Uncomment to trade flexibility for speed
  44 //#define CONST_HIST_SIZE
  45
  46 // Uncomment to get some performance stats in stderr
  47 //#define REPORT_TICKS
  48
  49 #ifdef CONST_HIST_SIZE
  50 #define m_BinBit 5
  51 #define m_ByteShift 3
  52 #endif
  53
  54 typedef float DefHistType;
  55 #define DefHistTypeMat CV_32F
  56 #define HIST_INDEX(_pData) (((_pData)[0]>>m_ByteShift) + (((_pData)[1]>>(m_ByteShift))<<m_BinBit)+((pImgData[2]>>m_ByteShift)<<(m_BinBit*2)))
  57
  58 class DefHist
  59 {
  60 public:
  61     CvMat*          m_pHist;
  62     DefHistType     m_HistVolume;
  63     DefHist(int BinNum=0)
  64     {
  65         m_pHist = NULL;
  66         m_HistVolume = 0;
  67         Resize(BinNum);
  68     }
  69     ~DefHist()
  70     {
  71         if(m_pHist)cvReleaseMat(&m_pHist);
  72     }
  73     void Resize(int BinNum)
  74     {
  75         if(m_pHist)cvReleaseMat(&m_pHist);
  76         if(BinNum>0)
  77         {
  78             m_pHist = cvCreateMat(1, BinNum, DefHistTypeMat);
  79             cvZero(m_pHist);
  80         }
  81         m_HistVolume = 0;
  82     }
  83     void Update(DefHist* pH, float W)
  84     {/* update hist */
  85         double  Vol, WM, WC;
  86         Vol = 0.5*(m_HistVolume + pH->m_HistVolume);
  87         WM = Vol*(1-W)/m_HistVolume;
  88         WC = Vol*(W)/pH->m_HistVolume;
  89         cvAddWeighted(m_pHist, WM, pH->m_pHist,WC,0,m_pHist);
  90         m_HistVolume = (float)cvSum(m_pHist).val[0];
  91     }/* update hist */
  92 };
  93
  94 class CvBlobTrackerOneMSFG:public CvBlobTrackerOne
  95 {
  96 protected:
  97     int             m_BinNumTotal; /* protected only for parralel MSPF */
  98     CvSize          m_ObjSize;
  99     void ReAllocKernel(int  w, int h)
 100     {
 101         int     x,y;
 102         float   x0 = 0.5f*(w-1);
 103         float   y0 = 0.5f*(h-1);
 104         assert(w>0);
 105         assert(h>0);
 106         m_ObjSize = cvSize(w,h);
 107
 108         if(m_KernelHist) cvReleaseMat(&m_KernelHist);
 109         if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
 110         m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
 111         m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);
 112         for(y=0;y<h;++y)for(x=0;x<w;++x)
 113         {
 114             double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
 115 //            double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
 116             CV_MAT_ELEM(m_KernelHist[0],DefHistType, y, x) = (DefHistType)GetKernelHist(r2);
 117             CV_MAT_ELEM(m_KernelMeanShift[0],DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
 118         }
 119     }
 120 private:
 121     /* parameters */
 122     int             m_IterNum;
 123     float           m_FGWeight;
 124     float           m_Alpha;
 125     CvMat*          m_KernelHist;
 126     CvMat*          m_KernelMeanShift;
 127 #ifndef CONST_HIST_SIZE
 128     int             m_BinBit;
 129     int             m_ByteShift;
 130 #endif
 131     int             m_BinNum;
 132     int             m_Dim;
 133     /*
 134     CvMat*          m_HistModel;
 135     float           m_HistModelVolume;
 136     CvMat*          m_HistCandidate;
 137     float           m_HistCandidateVolume;
 138     CvMat*          m_HistTemp;
 139     */
 140     DefHist         m_HistModel;
 141     DefHist         m_HistCandidate;
 142     DefHist         m_HistTemp;
 143
 144     CvBlob          m_Blob;
 145     int             m_Collision;
 146
 147     void ReAllocHist(int Dim, int BinBit)
 148     {
 149 #ifndef CONST_HIST_SIZE
 150         m_BinBit = BinBit;
 151         m_ByteShift = 8-BinBit;
 152 #endif
 153         m_Dim = Dim;
 154         m_BinNum = (1<<BinBit);
 155         m_BinNumTotal = cvRound(pow((double)m_BinNum,(double)m_Dim));
 156         /*
 157         if(m_HistModel) cvReleaseMat(&m_HistModel);
 158         if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
 159         if(m_HistTemp) cvReleaseMat(&m_HistTemp);
 160         m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
 161         m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
 162         m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
 163         cvZero(m_HistCandidate);
 164         cvZero(m_HistModel);
 165         m_HistModelVolume = 0.0f;
 166         m_HistCandidateVolume = 0.0f;
 167         */
 168         m_HistCandidate.Resize(m_BinNumTotal);
 169         m_HistModel.Resize(m_BinNumTotal);
 170         m_HistTemp.Resize(m_BinNumTotal);
 171     }
 172     double GetKernelHist(double r2)
 173     {
 174         return (r2 < 1) ? 1 -  r2 : 0;
 175     }
 176     double GetKernelMeanShift(double r2)
 177     {
 178         return (r2<1) ? 1 : 0;
 179     }
 180 //    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
 181 //    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)
 182     void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
 183     {
 184         int UsePrecalculatedKernel = 0;
 185         int BW = cvRound(pBlob->w);
 186         int BH = cvRound(pBlob->h);
 187         DefHistType Volume = 0;
 188         int         x0 = cvRound(pBlob->x - BW*0.5);
 189         int         y0 = cvRound(pBlob->y - BH*0.5);
 190         int         x,y;
 191
 192         UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;
 193
 194         //cvZero(pHist);
 195         cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
 196         Volume = 1;
 197
 198         assert(BW < pImg->width);
 199         assert(BH < pImg->height);
 200         if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
 201         if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
 202         if(x0<0){ x0=0;}
 203         if(y0<0){ y0=0;}
 204
 205         if(m_Dim == 3)
 206         {
 207             for(y=0;y<BH;++y)
 208             {
 209                 unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
 210                 unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
 211                 DefHistType* pKernelData = NULL;
 212                 if(UsePrecalculatedKernel)
 213                 {
 214                     pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
 215                 }
 216                 for(x=0;x<BW;++x,pImgData+=3)
 217                 {
 218                     DefHistType K;
 219                     int index = HIST_INDEX(pImgData);
 220                     assert(index >= 0 && index < pHist->m_pHist->cols);
 221                     if(UsePrecalculatedKernel)
 222                     {
 223                         K = pKernelData[x];
 224                     }
 225                     else
 226                     {
 227                         float dx = (x+x0-pBlob->x)/(pBlob->w*0.5f);
 228                         float dy = (y+y0-pBlob->y)/(pBlob->h*0.5f);
 229                         double r2 = dx*dx+dy*dy;
 230                         K = (float)GetKernelHist(r2);
 231                     }
 232
 233                     if(pMaskData)
 234                     {
 235                         K *= pMaskData[x]*0.003921568627450980392156862745098f;
 236                     }
 237                     Volume += K;
 238                     ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;
 239                 }/* next column */
 240             }/* next row */
 241         }/* if m_Dim == 3 */
 242         pHist->m_HistVolume = Volume;
 243     };/*CollectHist*/
 244     double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL)
 245     {
 246         if(pHM==NULL) pHM = &m_HistModel;
 247         if(pHC==NULL) pHC = &m_HistCandidate;
 248         if(pHT==NULL) pHT = &m_HistTemp;
 249         if(pHC->m_HistVolume*pHM->m_HistVolume > 0)
 250         {
 251 #if 0
 252             // use CV functions
 253             cvMul(pHM->m_pHist,pHC->m_pHist,pHT->m_pHist);
 254             cvPow(pHT->m_pHist,pHT->m_pHist,0.5);
 255             return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
 256 #else
 257             // Do computations manually and let autovectorizer do the job
 258             DefHistType *hm, *hc, *ht;
 259             double sum;
 260             int     size;
 261             hm=(DefHistType *)(pHM->m_pHist->data.ptr);
 262             hc=(DefHistType *)(pHC->m_pHist->data.ptr);
 263             ht=(DefHistType *)(pHT->m_pHist->data.ptr);
 264             size = pHM->m_pHist->width*pHM->m_pHist->height;
 265             sum = 0.;
 266             for(int i = 0; i < size; i++ )
 267             {
 268                 sum += sqrt(hm[i]*hc[i]);
 269             }
 270             return sum / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
 271 #endif
 272         }
 273         return 0;
 274     } /* calcBhattacharyyaCoefficient */
 275 protected:
 276     //double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
 277     double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist=NULL, int /*thread_number*/ = 0)
 278     {
 279         if(pHist==NULL)pHist = &m_HistTemp;
 280         CollectHist(pImg, pImgFG, pBlob, pHist);
 281         return calcBhattacharyya(&m_HistModel, pHist, pHist);
 282     }
 283     void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob)
 284     {
 285         if(m_Alpha>0 && !m_Collision)
 286         {/* update hist */
 287             CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
 288             m_HistModel.Update(&m_HistCandidate, m_Alpha);
 289         }/* update hist */
 290     }/* UpdateModelHist*/
 291
 292 public:
 293     CvBlobTrackerOneMSFG()
 294     {
 295
 296         /* add several parameters for external use */
 297         m_FGWeight = 2;
 298         AddParam("FGWeight", &m_FGWeight);
 299         CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
 300
 301         m_Alpha = 0.01f;
 302         AddParam("Alpha", &m_Alpha);
 303         CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");
 304
 305         m_IterNum = 10;
 306         AddParam("IterNum", &m_IterNum);
 307         CommentParam("IterNum","Maximal number of iteration in meanshift operation");
 308
 309         /* init internal data */
 310         m_Collision = 0;
 311 //        m_BinBit=0;
 312         m_Dim = 0;
 313         /*
 314         m_HistModel = NULL;
 315         m_HistCandidate = NULL;
 316         m_HistTemp = NULL;
 317         */
 318         m_KernelHist = NULL;
 319         m_KernelMeanShift = NULL;
 320         ReAllocHist(3,5);   /* 3D hist, each dim has 2^5 bins*/
 321     }
 322     ~CvBlobTrackerOneMSFG()
 323     {
 324         /*
 325         if(m_HistModel) cvReleaseMat(&m_HistModel);
 326         if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
 327         if(m_HistTemp) cvReleaseMat(&m_HistTemp);
 328         */
 329         if(m_KernelHist) cvReleaseMat(&m_KernelHist);
 330         if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
 331     }
 332     /* interface */
 333     virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
 334     {
 335         int w = cvRound(CV_BLOB_WX(pBlobInit));
 336         int h = cvRound(CV_BLOB_WY(pBlobInit));
 337         if(w<CV_BLOB_MINW)w=CV_BLOB_MINW;
 338         if(h<CV_BLOB_MINH)h=CV_BLOB_MINH;
 339         if(pImg)
 340         {
 341             if(w>pImg->width)w=pImg->width;
 342             if(h>pImg->height)h=pImg->height;
 343         }
 344         ReAllocKernel(w,h);
 345         if(pImg)
 346             CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
 347         m_Blob = pBlobInit[0];
 348     };
 349     virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
 350     {
 351         int     iter;
 352
 353         if(pBlobPrev)
 354         {
 355             m_Blob = pBlobPrev[0];
 356         }
 357
 358         {/* check blob size and realloc kernels if it is necessary */
 359             int w = cvRound(m_Blob.w);
 360             int h = cvRound(m_Blob.h);
 361             if( w != m_ObjSize.width || h!=m_ObjSize.height)
 362             {
 363                 ReAllocKernel(w,h);
 364                 /* after this ( w != m_ObjSize.width || h!=m_ObjSize.height) shoiuld be false */
 365             }
 366         }/* check blob size and realloc kernels if it is necessary */
 367
 368
 369         for(iter=0;iter<m_IterNum;++iter)
 370         {
 371             float   newx=0,newy=0,sum=0;
 372             //int     x,y;
 373             double  B0;
 374
 375             //CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 376             CollectHist(pImg, NULL, &m_Blob, &m_HistCandidate);
 377             B0 = calcBhattacharyya();
 378
 379             if(m_Wnd)if(CV_BLOB_ID(pBlobPrev)==0 && iter == 0)
 380             {/* debug */
 381                 IplImage*   pW = cvCloneImage(pImgFG);
 382                 IplImage*   pWFG = cvCloneImage(pImgFG);
 383                 int         x,y;
 384
 385                 cvZero(pW);
 386                 cvZero(pWFG);
 387
 388                 assert(m_ObjSize.width < pImg->width);
 389                 assert(m_ObjSize.height < pImg->height);
 390                 /*calc shift vector */
 391                 for(y=0;y<pImg->height;++y)
 392                 {
 393                     unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y,0);
 394                     unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y,0)):NULL;
 395                     for(x=0;x<pImg->width;++x,pImgData+=3)
 396                     {
 397                         int         xk = cvRound(x-(m_Blob.x-m_Blob.w*0.5));
 398                         int         yk = cvRound(y-(m_Blob.y-m_Blob.h*0.5));
 399                         double      HM = 0;
 400                         double      HC = 0;
 401                         double      K;
 402                         int index = HIST_INDEX(pImgData);
 403                         assert(index >= 0 && index < m_BinNumTotal);
 404
 405                         if(fabs(x-m_Blob.x)>m_Blob.w*0.6) continue;
 406                         if(fabs(y-m_Blob.y)>m_Blob.h*0.6) continue;
 407
 408                         if(xk < 0 || xk >= m_KernelMeanShift->cols) continue;
 409                         if(yk < 0 || yk >= m_KernelMeanShift->rows) continue;
 410
 411                         if(m_HistModel.m_HistVolume>0)
 412                             HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
 413                         if(m_HistCandidate.m_HistVolume>0)
 414                             HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
 415                         K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],yk,xk,sizeof(DefHistType));
 416
 417                         if(HC>0)
 418                         {
 419                             double  V = sqrt(HM / HC);
 420                             int     Vi = cvRound(V * 64);
 421                             if(Vi < 0) Vi = 0;
 422                             if(Vi > 255) Vi = 255;
 423                             CV_IMAGE_ELEM(pW,uchar,y,x) = (uchar)Vi;
 424
 425                             V += m_FGWeight*(pMaskData?(pMaskData[x]/255.0f):0);
 426                             V*=K;
 427                             Vi = cvRound(V * 64);
 428                             if(Vi < 0) Vi = 0;
 429                             if(Vi > 255) Vi = 255;
 430                             CV_IMAGE_ELEM(pWFG,uchar,y,x) = (uchar)Vi;
 431                         }
 432
 433                     }/* next column */
 434                 }/* next row */
 435                 //cvNamedWindow("MSFG_W",0);
 436                 //cvShowImage("MSFG_W",pW);
 437                 //cvNamedWindow("MSFG_WFG",0);
 438                 //cvShowImage("MSFG_WFG",pWFG);
 439                 //cvNamedWindow("MSFG_FG",0);
 440                 //cvShowImage("MSFG_FG",pImgFG);
 441
 442                 //cvSaveImage("MSFG_W.bmp",pW);
 443                 //cvSaveImage("MSFG_WFG.bmp",pWFG);
 444                 //cvSaveImage("MSFG_FG.bmp",pImgFG);
 445             }/* debug */
 446
 447
 448             /* calc new pos by meanshift */
 449             /* calc new pos */
 450             if(m_Dim == 3)
 451             {
 452                 int         x0 = cvRound(m_Blob.x - m_ObjSize.width*0.5);
 453                 int         y0 = cvRound(m_Blob.y - m_ObjSize.height*0.5);
 454                 int         x,y;
 455
 456                 assert(m_ObjSize.width < pImg->width);
 457                 assert(m_ObjSize.height < pImg->height);
 458
 459                 /* crop blob bounds */
 460                 if((x0+m_ObjSize.width)>=pImg->width) x0=pImg->width-m_ObjSize.width-1;
 461                 if((y0+m_ObjSize.height)>=pImg->height) y0=pImg->height-m_ObjSize.height-1;
 462                 if(x0<0){ x0=0;}
 463                 if(y0<0){ y0=0;}
 464                 /*calc shift vector */
 465                 for(y=0;y<m_ObjSize.height;++y)
 466                 {
 467                     unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
 468                     unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
 469                     DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],y,0,sizeof(DefHistType));
 470                     for(x=0;x<m_ObjSize.width;++x,pImgData+=3)
 471                     {
 472                         DefHistType K = pKernelData[x];
 473                         double      HM = 0;
 474                         double      HC = 0;
 475                         int index = HIST_INDEX(pImgData);
 476                         assert(index >= 0 && index < m_BinNumTotal);
 477
 478                         if(m_HistModel.m_HistVolume>0)
 479                             HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
 480                         if(m_HistCandidate.m_HistVolume>0)
 481                             HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
 482
 483                         if(HC>0)
 484                         {
 485                             double V = sqrt(HM / HC);
 486                             if(!m_Collision && m_FGWeight>0 && pMaskData)
 487                             {
 488                                 V += m_FGWeight*(pMaskData[x]/255.0f);
 489                             }
 490                             K *= (float)MIN(V,100000.);
 491                         }
 492
 493                         sum += K;
 494                         newx += K*x;
 495                         newy += K*y;
 496                     }/* next column */
 497                 }/* next row */
 498
 499                 if(sum > 0)
 500                 {
 501                     newx /= sum;
 502                     newy /= sum;
 503                 }
 504                 newx += x0;
 505                 newy += y0;
 506
 507             }/* if m_Dim == 3 */
 508             /* calc new pos be meanshift */
 509
 510             for(;;)
 511             {/* iterate using bahattcharrya coefficient */
 512                 double  B1;
 513                 CvBlob  B = m_Blob;
 514 //                CvPoint NewCenter = cvPoint(cvRound(newx),cvRound(newy));
 515                 B.x = newx;
 516                 B.y = newy;
 517                 CollectHist(pImg, NULL, &B, &m_HistCandidate);
 518                 B1 = calcBhattacharyya();
 519                 if(B1 > B0) break;
 520                 newx = 0.5f*(newx+m_Blob.x);
 521                 newy = 0.5f*(newy+m_Blob.y);
 522                 if(fabs(newx-m_Blob.x)<0.1 && fabs(newy-m_Blob.y)<0.1) break;
 523             }/* iterate using bahattcharrya coefficient */
 524
 525             if(fabs(newx-m_Blob.x)<0.5 && fabs(newy-m_Blob.y)<0.5) break;
 526             m_Blob.x = newx;
 527             m_Blob.y = newy;
 528         }/* next iteration */
 529
 530         while(!m_Collision && m_FGWeight>0)
 531         { /* update size if no collision */
 532             float       Alpha = 0.04f;
 533             CvBlob      NewBlob;
 534             double      M00,X,Y,XX,YY;
 535             CvMoments   m;
 536             CvRect      r;
 537             CvMat       mat;
 538
 539             r.width = cvRound(m_Blob.w*1.5+0.5);
 540             r.height = cvRound(m_Blob.h*1.5+0.5);
 541             r.x = cvRound(m_Blob.x - 0.5*r.width);
 542             r.y = cvRound(m_Blob.y - 0.5*r.height);
 543             if(r.x < 0) break;
 544             if(r.y < 0) break;
 545             if(r.x+r.width >= pImgFG->width) break;
 546             if(r.y+r.height >= pImgFG->height) break;
 547             if(r.height < 5 || r.width < 5) break;
 548
 549             cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
 550             M00 = cvGetSpatialMoment( &m, 0, 0 );
 551             if(M00 <= 0 ) break;
 552             X = cvGetSpatialMoment( &m, 1, 0 )/M00;
 553             Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
 554             XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
 555             YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
 556             NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
 557
 558             NewBlob.w = Alpha*NewBlob.w+m_Blob.w*(1-Alpha);
 559             NewBlob.h = Alpha*NewBlob.h+m_Blob.h*(1-Alpha);
 560
 561             m_Blob.w = MAX(NewBlob.w,5);
 562             m_Blob.h = MAX(NewBlob.h,5);
 563             break;
 564         }/* update size if no collision */
 565
 566         return &m_Blob;
 567     }; /* CvBlobTrackerOneMSFG::Process */
 568     virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL)
 569     {
 570         double  S = 0.2;
 571         double  B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
 572         return exp((B-1)/(2*S));
 573     };/*CvBlobTrackerOneMSFG::*/
 574
 575     virtual void Update(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
 576     {   /* update hist */
 577         UpdateModelHist(pImg, pImgFG, pBlob?pBlob:&m_Blob);
 578     }/*CvBlobTrackerOneMSFG::*/
 579
 580     virtual void Release(){delete this;};
 581     virtual void SetCollision(int CollisionFlag)
 582     {
 583         m_Collision = CollisionFlag;
 584     }
 585     virtual void SaveState(CvFileStorage* fs)
 586     {
 587         cvWriteStruct(fs, "Blob", &m_Blob, "ffffi");
 588         cvWriteInt(fs,"Collision", m_Collision);
 589         cvWriteInt(fs,"HistVolume", cvRound(m_HistModel.m_HistVolume));
 590         cvWrite(fs,"Hist", m_HistModel.m_pHist);
 591     };
 592     virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
 593     {
 594         CvMat* pM;
 595         cvReadStructByName(fs, node, "Blob",&m_Blob, "ffffi");
 596         m_Collision = cvReadIntByName(fs,node,"Collision",m_Collision);
 597         pM = (CvMat*)cvRead(fs,cvGetFileNodeByName(fs,node,"Hist"));
 598         if(pM)
 599         {
 600             m_HistModel.m_pHist = pM;
 601             m_HistModel.m_HistVolume = (float)cvSum(pM).val[0];
 602         }
 603     };
 604
 605 }; /*CvBlobTrackerOneMSFG*/
 606
 607 #if 0
 608 void CvBlobTrackerOneMSFG::CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
 609 {
 610     int UsePrecalculatedKernel = 0;
 611     int BW = cvRound(pBlob->w);
 612     int BH = cvRound(pBlob->h);
 613     DefHistType Volume = 0;
 614     int         x0 = cvRound(pBlob->x - BW*0.5);
 615     int         y0 = cvRound(pBlob->y - BH*0.5);
 616     int         x,y;
 617
 618     UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;
 619
 620     //cvZero(pHist);
 621     cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
 622     Volume = 1;
 623
 624     assert(BW < pImg->width);
 625     assert(BH < pImg->height);
 626     if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
 627     if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
 628     if(x0<0){ x0=0;}
 629     if(y0<0){ y0=0;}
 630
 631     if(m_Dim == 3)
 632     {
 633         for(y=0;y<BH;++y)
 634         {
 635             unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
 636             unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
 637             DefHistType* pKernelData = NULL;
 638             if(UsePrecalculatedKernel)
 639             {
 640                 pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
 641             }
 642             for(x=0;x<BW;++x,pImgData+=3)
 643             {
 644                 DefHistType K;
 645                 int index = HIST_INDEX(pImgData);
 646                 assert(index >= 0 && index < pHist->m_pHist->cols);
 647                 if(UsePrecalculatedKernel)
 648                 {
 649                     K = pKernelData[x];
 650                 }
 651                 else
 652                 {
 653                     float dx = (x+x0-pBlob->x)/(pBlob->w*0.5);
 654                     float dy = (y+y0-pBlob->y)/(pBlob->h*0.5);
 655                     double r2 = dx*dx+dy*dy;
 656                     K = GetKernelHist(r2);
 657                 }
 658
 659                 if(pMaskData)
 660                 {
 661                     K *= pMaskData[x]*0.003921568627450980392156862745098;
 662                 }
 663                 Volume += K;
 664                 ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;
 665             }/* next column */
 666         }/* next row */
 667     }/* if m_Dim == 3 */
 668     pHist->m_HistVolume = Volume;
 669 };/*CollectHist*/
 670 #endif
 671
 672 CvBlobTrackerOne* cvCreateBlobTrackerOneMSFG()
 673 {
 674     return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFG;
 675 }
 676 CvBlobTracker* cvCreateBlobTrackerMSFG()
 677 {
 678     return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFG);
 679 }
 680
 681 /* Create specific tracker without FG usin - just simple mean-shift tracker*/
 682 class CvBlobTrackerOneMS:public CvBlobTrackerOneMSFG
 683 {
 684 public:
 685     CvBlobTrackerOneMS()
 686     {
 687         SetParam("FGWeight",0);
 688         DelParam("FGWeight");
 689     };
 690 };
 691
 692 CvBlobTrackerOne* cvCreateBlobTrackerOneMS()
 693 {
 694     return (CvBlobTrackerOne*) new CvBlobTrackerOneMS;
 695 }
 696 CvBlobTracker* cvCreateBlobTrackerMS()
 697 {
 698     return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMS);
 699 }
 700
 701 typedef struct DefParticle
 702 {
 703     CvBlob  blob;
 704     float   Vx,Vy;
 705     double  W;
 706 }DefParticle;
 707
 708 class CvBlobTrackerOneMSPF:public CvBlobTrackerOneMS
 709 {
 710 private:
 711     /* parameters */
 712     int             m_ParticleNum;
 713     float           m_UseVel;
 714     float           m_SizeVar;
 715     float           m_PosVar;
 716
 717     CvSize          m_ImgSize;
 718     CvBlob          m_Blob;
 719     DefParticle*    m_pParticlesPredicted;
 720     DefParticle*    m_pParticlesResampled;
 721     CvRNG           m_RNG;
 722 #ifdef _OPENMP
 723     int             m_ThreadNum;
 724     DefHist*        m_HistForParalel;
 725 #endif
 726
 727 public:
 728     virtual void SaveState(CvFileStorage* fs)
 729     {
 730         CvBlobTrackerOneMS::SaveState(fs);
 731         cvWriteInt(fs,"ParticleNum",m_ParticleNum);
 732         cvWriteStruct(fs,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd",m_ParticleNum);
 733         cvWriteStruct(fs,"ParticlesResampled",m_pParticlesResampled,"ffffiffd",m_ParticleNum);
 734     };
 735     virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
 736     {
 737         //CvMat* pM;
 738         CvBlobTrackerOneMS::LoadState(fs,node);
 739         m_ParticleNum = cvReadIntByName(fs,node,"ParticleNum",m_ParticleNum);
 740         if(m_ParticleNum>0)
 741         {
 742             Realloc();
 743             printf("sizeof(DefParticle) is %d\n", sizeof(DefParticle));
 744             cvReadStructByName(fs,node,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd");
 745             cvReadStructByName(fs,node,"ParticlesResampled",m_pParticlesResampled,"ffffiffd");
 746         }
 747     };
 748     CvBlobTrackerOneMSPF()
 749     {
 750         m_pParticlesPredicted = NULL;
 751         m_pParticlesResampled = NULL;
 752         m_ParticleNum = 200;
 753
 754         AddParam("ParticleNum",&m_ParticleNum);
 755         CommentParam("ParticleNum","Number of particles");
 756         Realloc();
 757
 758         m_UseVel = 0;
 759         AddParam("UseVel",&m_UseVel);
 760         CommentParam("UseVel","Percent of particles which use velocity feature");
 761
 762         m_SizeVar = 0.05f;
 763         AddParam("SizeVar",&m_SizeVar);
 764         CommentParam("SizeVar","Size variation (in object size)");
 765
 766         m_PosVar = 0.2f;
 767         AddParam("PosVar",&m_PosVar);
 768         CommentParam("PosVar","Position variation (in object size)");
 769
 770         m_RNG = cvRNG(0);
 771 #ifdef _OPENMP
 772         {
 773             m_ThreadNum = omp_get_num_procs();
 774             m_HistForParalel = new DefHist[m_ThreadNum];
 775         }
 776 #endif
 777     }
 778     ~CvBlobTrackerOneMSPF()
 779     {
 780         if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
 781         if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
 782 #ifdef _OPENMP
 783         if(m_HistForParalel) delete[] m_HistForParalel;
 784 #endif
 785     }
 786 private:
 787     void Realloc()
 788     {
 789         if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
 790         if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
 791         m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
 792         m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
 793     };/* Realloc*/
 794     void DrawDebug(IplImage* pImg, IplImage* /*pImgFG*/)
 795     {
 796         int k;
 797         for(k=0;k<2;++k)
 798         {
 799             DefParticle*    pBP = k?m_pParticlesResampled:m_pParticlesPredicted;
 800             //const char*           name = k?"MSPF resampled particle":"MSPF Predicted particle";
 801             IplImage*       pI = cvCloneImage(pImg);
 802             int             h,hN = m_ParticleNum;
 803             CvBlob          C = cvBlob(0,0,0,0);
 804             double          WS = 0;
 805             for(h=0;h<hN;++h)
 806             {
 807                 CvBlob  B = pBP[h].blob;
 808                 int     CW = cvRound(255*pBP[h].W);
 809                 CvBlob* pB = &B;
 810                 int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
 811                 CvSize  s = cvSize(MAX(1,x), MAX(1,y));
 812                 double  W = pBP[h].W;
 813                 C.x += pB->x;
 814                 C.y += pB->y;
 815                 C.w += pB->w;
 816                 C.h += pB->h;
 817                 WS+=W;
 818
 819                 s = cvSize(1,1);
 820                 cvEllipse( pI,
 821                     cvPointFrom32f(CV_BLOB_CENTER(pB)),
 822                     s,
 823                     0, 0, 360,
 824                     CV_RGB(CW,0,0), 1 );
 825             }/* next hypothetis */
 826             C.x /= hN;
 827             C.y /= hN;
 828             C.w /= hN;
 829             C.h /= hN;
 830             cvEllipse( pI,
 831                 cvPointFrom32f(CV_BLOB_CENTER(&C)),
 832                 cvSize(cvRound(C.w*0.5),cvRound(C.h*0.5)),
 833                 0, 0, 360,
 834                 CV_RGB(0,0,255), 1 );
 835             cvEllipse( pI,
 836                 cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
 837                 cvSize(cvRound(m_Blob.w*0.5),cvRound(m_Blob.h*0.5)),
 838                 0, 0, 360,
 839                 CV_RGB(0,255,0), 1 );
 840             //cvNamedWindow(name,0);
 841             //cvShowImage(name,pI);
 842             cvReleaseImage(&pI);
 843         }/*  */
 844
 845         //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);
 846     }/* ::DrawDebug */
 847 private:
 848     void Prediction()
 849     {
 850         int p;
 851         for(p=0;p<m_ParticleNum;++p)
 852         {/* "prediction" of particle */
 853             //double  t;
 854             float   r[5];
 855             CvMat   rm = cvMat(1,5,CV_32F,r);
 856             cvRandArr(&m_RNG,&rm,CV_RAND_NORMAL,cvScalar(0),cvScalar(1));
 857
 858             m_pParticlesPredicted[p] = m_pParticlesResampled[p];
 859
 860             if(cvRandReal(&m_RNG)<0.5)
 861             {/* half of particles will predict based on external blob */
 862                 m_pParticlesPredicted[p].blob = m_Blob;
 863             }
 864
 865             if(cvRandReal(&m_RNG)<m_UseVel)
 866             {/* predict moving particle by usual way by using speed */
 867                 m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
 868                 m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
 869             }
 870             else
 871             {/* stop several particles */
 872                 m_pParticlesPredicted[p].Vx = 0;
 873                 m_pParticlesPredicted[p].Vy = 0;
 874             }
 875
 876             {/* update position */
 877                 float S = (m_Blob.w + m_Blob.h)*0.5f;
 878                 m_pParticlesPredicted[p].blob.x += m_PosVar*S*r[0];
 879                 m_pParticlesPredicted[p].blob.y += m_PosVar*S*r[1];
 880                 /* update velocity */
 881                 m_pParticlesPredicted[p].Vx += (float)(m_PosVar*S*0.1*r[3]);
 882                 m_pParticlesPredicted[p].Vy += (float)(m_PosVar*S*0.1*r[4]);
 883             }
 884
 885             /* update size */
 886             m_pParticlesPredicted[p].blob.w *= (1+m_SizeVar*r[2]);
 887             m_pParticlesPredicted[p].blob.h *= (1+m_SizeVar*r[2]);
 888
 889             /* truncate size of particle */
 890             if(m_pParticlesPredicted[p].blob.w > m_ImgSize.width*0.5f)
 891             {
 892                 m_pParticlesPredicted[p].blob.w = m_ImgSize.width*0.5f;
 893             }
 894             if(m_pParticlesPredicted[p].blob.h > m_ImgSize.height*0.5f)
 895             {
 896                 m_pParticlesPredicted[p].blob.h = m_ImgSize.height*0.5f;
 897             }
 898             if(m_pParticlesPredicted[p].blob.w < 1 )
 899             {
 900                 m_pParticlesPredicted[p].blob.w = 1;
 901             }
 902             if(m_pParticlesPredicted[p].blob.h < 1)
 903             {
 904                 m_pParticlesPredicted[p].blob.h = 1;
 905             }
 906         }/* "prediction" of particle */
 907     }/* Prediction */
 908
 909     void UpdateWeightsMS(IplImage* pImg, IplImage* /*pImgFG*/)
 910     {
 911         int p;
 912 #ifdef _OPENMP
 913         if( m_HistForParalel[0].m_pHist==NULL || m_HistForParalel[0].m_pHist->cols != m_BinNumTotal)
 914         {
 915             int t;
 916             for(t=0;t<m_ThreadNum;++t)
 917                 m_HistForParalel[t].Resize(m_BinNumTotal);
 918         }
 919 #endif
 920
 921 #ifdef _OPENMP
 922 #pragma omp parallel for num_threads(m_ThreadNum),schedule(runtime)
 923 #endif
 924         for(p=0;p<m_ParticleNum;++p)
 925         {/* calc weights for particles */
 926             double  S = 0.2;
 927             double  B = 0;
 928 #ifdef _OPENMP
 929             assert(omp_get_thread_num()<m_ThreadNum);
 930 #endif
 931
 932             B = GetBhattacharyya(
 933                 pImg, NULL,
 934                 &(m_pParticlesPredicted[p].blob)
 935 #ifdef _OPENMP
 936                 ,&(m_HistForParalel[omp_get_thread_num()])
 937 #endif
 938                 );
 939             m_pParticlesPredicted[p].W *= exp((B-1)/(2*S));
 940         }/* calc weights for particles */
 941     }
 942     void UpdateWeightsCC(IplImage* /*pImg*/, IplImage* /*pImgFG*/)
 943     {
 944         int p;
 945 #ifdef _OPENMP
 946 #pragma omp parallel for
 947 #endif
 948         for(p=0;p<m_ParticleNum;++p)
 949         {/* calc weights for particles */
 950             double W = 1;
 951             m_pParticlesPredicted[p].W *= W;
 952         }/* calc weights for particles */
 953     }
 954     void Resample()
 955     {/* resample particle */
 956         int         p;
 957         double      Sum = 0;
 958         for(p=0;p<m_ParticleNum;++p)
 959         {
 960             Sum += m_pParticlesPredicted[p].W;
 961         }
 962         for(p=0;p<m_ParticleNum;++p)
 963         {
 964             double  T = Sum * cvRandReal(&m_RNG); /* set current random threshold for cululative weight */
 965             int     p2;
 966             double  Sum2 = 0;
 967             for(p2=0;p2<m_ParticleNum;++p2)
 968             {
 969                 Sum2 += m_pParticlesPredicted[p2].W;
 970                 if(Sum2 >= T)break;
 971             }
 972             if(p2>=m_ParticleNum)p2=m_ParticleNum-1;
 973             m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
 974             m_pParticlesResampled[p].W = 1;
 975         }/* find next particle */
 976     } /* resample particle */
 977
 978
 979 public:
 980     virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
 981     {
 982         int i;
 983         CvBlobTrackerOneMSFG::Init(pBlobInit, pImg, pImgFG);
 984         DefParticle PP;
 985         PP.W = 1;
 986         PP.Vx = 0;
 987         PP.Vy = 0;
 988         PP.blob = pBlobInit[0];
 989         for(i=0;i<m_ParticleNum;++i)
 990         {
 991             m_pParticlesPredicted[i] = PP;
 992             m_pParticlesResampled[i] = PP;
 993         }
 994         m_Blob = pBlobInit[0];
 995     } /* CvBlobTrackerOneMSPF::Init*/
 996     virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
 997     {
 998         int p;
 999
1000         m_ImgSize.width = pImg->width;
1001         m_ImgSize.height = pImg->height;
1002
1003
1004         m_Blob = pBlobPrev[0];
1005
1006         {/* check blob size and realloc kernels if it is necessary */
1007             int w = cvRound(m_Blob.w);
1008             int h = cvRound(m_Blob.h);
1009             if( w != m_ObjSize.width || h!=m_ObjSize.height)
1010             {
1011                 ReAllocKernel(w,h);
1012                 /* after this ( w != m_ObjSize.width || h!=m_ObjSize.height) shoiuld be false */
1013             }
1014         }/* check blob size and realloc kernels if it is necessary */
1015
1016         Prediction();
1017
1018 #ifdef REPORT_TICKS
1019         int64 ticks = cvGetTickCount();
1020 #endif
1021
1022         UpdateWeightsMS(pImg, pImgFG);
1023
1024 #ifdef REPORT_TICKS
1025         ticks = cvGetTickCount() - ticks;
1026         fprintf(stderr, "PF UpdateWeights, %d ticks\n",  (int)ticks);
1027         ticks = cvGetTickCount();
1028 #endif
1029
1030         Resample();
1031
1032 #ifdef REPORT_TICKS
1033         ticks = cvGetTickCount() - ticks;
1034         fprintf(stderr, "PF Resampling, %d ticks\n",  (int)ticks);
1035 #endif
1036
1037         {/* find average result */
1038             float   x = 0;
1039             float   y = 0;
1040             float   w = 0;
1041             float   h = 0;
1042             float   Sum = 0;
1043             DefParticle* pP = m_pParticlesResampled;
1044             for(p=0;p<m_ParticleNum;++p)
1045             {
1046                 float W = (float)pP[p].W;
1047                 x += W*pP[p].blob.x;
1048                 y += W*pP[p].blob.y;
1049                 w += W*pP[p].blob.w;
1050                 h += W*pP[p].blob.h;
1051                 Sum += W;
1052             }
1053             if(Sum>0)
1054             {
1055                 m_Blob.x = x / Sum;
1056                 m_Blob.y = y / Sum;
1057                 m_Blob.w = w / Sum;
1058                 m_Blob.h = h / Sum;
1059             }
1060         }/* find average result */
1061
1062         if(m_Wnd)
1063         {
1064             DrawDebug(pImg, pImgFG);
1065         }
1066
1067         return &m_Blob;
1068     }/* CvBlobTrackerOneMSPF::Process */
1069     virtual void SkipProcess(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
1070     {
1071         int p;
1072         for(p=0;p<m_ParticleNum;++p)
1073         {
1074             m_pParticlesResampled[p].blob = pBlob[0];
1075             m_pParticlesResampled[p].Vx = 0;
1076             m_pParticlesResampled[p].Vy = 0;
1077             m_pParticlesResampled[p].W = 1;
1078         }
1079     }
1080
1081     virtual void Release(){delete this;};
1082     virtual void ParamUpdate()
1083     {
1084         Realloc();
1085     }
1086 }; /* CvBlobTrackerOneMSPF */
1087
1088 CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF()
1089 {
1090     return (CvBlobTrackerOne*) new CvBlobTrackerOneMSPF;
1091 }
1092 CvBlobTracker* cvCreateBlobTrackerMSPF()
1093 {
1094     return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSPF);
1095 }
1096