git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackingmsfgs.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 #define SCALE_BASE 1.1
  44 #define SCALE_RANGE 2
  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)))
  49
  50 void calcKernelEpanechnikov(CvMat* pK)
  51 {    /* Allocate kernel for histogramm creation: */
  52     int     x,y;
  53     int     w = pK->width;
  54     int     h = pK->height;
  55     float   x0 = 0.5f*(w-1);
  56     float   y0 = 0.5f*(h-1);
  57
  58     for(y=0; y<h; ++y) for(x=0; x<w; ++x)
  59     {
  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);
  63     }
  64 }   /* Allocate kernel for histogram creation. */
  65
  66 class CvBlobTrackerOneMSFGS:public CvBlobTrackerOne
  67 {
  68 private:
  69     /* Parameters: */
  70     float           m_FGWeight;
  71     float           m_Alpha;
  72     CvSize          m_ObjSize;
  73     CvMat*          m_KernelHistModel;
  74     CvMat*          m_KernelHistCandidate;
  75     CvSize          m_KernelMeanShiftSize;
  76     CvMat*          m_KernelMeanShiftK[SCALE_NUM];
  77     CvMat*          m_KernelMeanShiftG[SCALE_NUM];
  78     CvMat*          m_Weights;
  79     int             m_BinBit;
  80     int             m_ByteShift;
  81     int             m_BinNum;
  82     int             m_Dim;
  83     int             m_BinNumTotal;
  84     CvMat*          m_HistModel;
  85     float           m_HistModelVolume;
  86     CvMat*          m_HistCandidate;
  87     float           m_HistCandidateVolume;
  88     CvMat*          m_HistTemp;
  89     CvBlob          m_Blob;
  90
  91     void ReAllocHist(int Dim, int BinBit)
  92     {
  93         m_BinBit = BinBit;
  94         m_ByteShift = 8-BinBit;
  95         m_Dim = Dim;
  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);
 105         cvZero(m_HistModel);
 106         m_HistModelVolume = 0.0f;
 107         m_HistCandidateVolume = 0.0f;
 108     }
 109
 110     void ReAllocKernel(int  w, int h, float sigma=0.4)
 111     {
 112         double  ScaleToObj = sigma*1.39;
 113         int     kernel_width = cvRound(w/ScaleToObj);
 114         int     kernel_height = cvRound(h/ScaleToObj);
 115         int     x,y,s;
 116         assert(w>0);
 117         assert(h>0);
 118         m_ObjSize = cvSize(w,h);
 119         m_KernelMeanShiftSize = cvSize(kernel_width,kernel_height);
 120
 121
 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);
 129
 130         if(m_Weights) cvReleaseMat(&m_Weights);
 131         m_Weights = cvCreateMat(kernel_height, kernel_width, CV_32F);
 132
 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);
 144
 145             for(y=0; y<kernel_height; ++y)
 146             {
 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) );
 149
 150                 for(x=0; x<kernel_width; ++x)
 151                 {
 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));
 157                 }
 158             }   /* Next line. */
 159         }
 160     }   /* ReallocKernel */
 161
 162     inline double Gaussian2D(double x, double sigma2)
 163     {
 164         return (exp(-x/(2*sigma2)) / (2*3.1415926535897932384626433832795*sigma2) );
 165     }
 166
 167     void calcHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pKernel, CvMat* pHist, DefHistType* pHistVolume)
 168     {
 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;
 174         int         x,y;
 175
 176         //cvZero(pHist);
 177         cvSet(pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
 178         Volume = 1;
 179
 180         if(m_Dim == 3)
 181         {
 182             for(y=0; y<h; ++y)
 183             {
 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));
 192
 193                 for(x=0; x<w; ++x, pImgData+=3)
 194                 {
 195                     if((x0+x)>=pImg->width) continue;
 196                     if((x0+x)<0)continue;
 197
 198                     if(pMaskData==NULL || pMaskData[x]>128)
 199                     {
 200                         DefHistType K = pKernelData[x];
 201                         int index = HIST_INDEX(pImgData);
 202                         assert(index >= 0 && index < pHist->cols);
 203                         Volume += K;
 204                         ((DefHistType*)(pHist->data.ptr))[index] += K;
 205
 206                     }   /* Only masked pixels. */
 207                 }   /*  Next column. */
 208             }   /*  Next row. */
 209         }   /* if m_Dim == 3. */
 210
 211         if(pHistVolume)pHistVolume[0] = Volume;
 212
 213     }; /* calcHist */
 214
 215     double calcBhattacharyya()
 216     {
 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 */
 221
 222     void calcWeights(IplImage* pImg, IplImage* pImgFG, CvPoint Center)
 223     {
 224         cvZero(m_Weights);
 225
 226         /* Calculate new position: */
 227         if(m_Dim == 3)
 228         {
 229             int         x0 = Center.x - m_KernelMeanShiftSize.width/2;
 230             int         y0 = Center.y - m_KernelMeanShiftSize.height/2;
 231             int         x,y;
 232
 233             assert(m_Weights->width == m_KernelMeanShiftSize.width);
 234             assert(m_Weights->height == m_KernelMeanShiftSize.height);
 235
 236             /* Calcualte shift vector: */
 237             for(y=0; y<m_KernelMeanShiftSize.height; ++y)
 238             {
 239                 unsigned char* pImgData = NULL;
 240                 unsigned char* pMaskData = NULL;
 241                 float* pWData = NULL;
 242
 243                 if(y+y0 < 0 || y+y0 >= pImg->height) continue;
 244
 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));
 248
 249                 for(x=0; x<m_KernelMeanShiftSize.width; ++x, pImgData+=3)
 250                 {
 251                     double      V  = 0;
 252                     double      HM = 0;
 253                     double      HC = 0;
 254                     int         index;
 255                     if(x+x0 < 0 || x+x0 >= pImg->width) continue;
 256
 257                     index = HIST_INDEX(pImgData);
 258                     assert(index >= 0 && index < m_BinNumTotal);
 259
 260                     if(m_HistModelVolume>0)
 261                         HM = ((DefHistType*)m_HistModel->data.ptr)[index]/m_HistModelVolume;
 262
 263                     if(m_HistCandidateVolume>0)
 264                         HC = ((DefHistType*)m_HistCandidate->data.ptr)[index]/m_HistCandidateVolume;
 265
 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);
 269
 270                 }   /* Next column. */
 271             }   /*  Next row. */
 272         }   /*  if m_Dim == 3. */
 273     }   /*  calcWeights */
 274
 275 public:
 276     CvBlobTrackerOneMSFGS()
 277     {
 278         int i;
 279         m_FGWeight = 0;
 280         m_Alpha = 0.0;
 281
 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)");
 287
 288         m_BinBit=0;
 289         m_Dim = 0;
 290         m_HistModel = NULL;
 291         m_HistCandidate = NULL;
 292         m_HistTemp = NULL;
 293         m_KernelHistModel = NULL;
 294         m_KernelHistCandidate = NULL;
 295         m_Weights = NULL;
 296
 297         for(i=0; i<SCALE_NUM; ++i)
 298         {
 299             m_KernelMeanShiftK[i] = NULL;
 300             m_KernelMeanShiftG[i] = NULL;
 301         }
 302         ReAllocHist(3,5);   /* 3D hist, each dimension has 2^5 bins. */
 303     }
 304
 305     ~CvBlobTrackerOneMSFGS()
 306     {
 307         int i;
 308         if(m_HistModel) cvReleaseMat(&m_HistModel);
 309         if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
 310         if(m_HistTemp) cvReleaseMat(&m_HistTemp);
 311         if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
 312
 313         for(i=0; i<SCALE_NUM; ++i)
 314         {
 315             if(m_KernelMeanShiftK[i]) cvReleaseMat(&m_KernelMeanShiftK[i]);
 316             if(m_KernelMeanShiftG[i]) cvReleaseMat(&m_KernelMeanShiftG[i]);
 317         }
 318     }
 319
 320     /* Interface: */
 321     virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
 322     {
 323         int w = cvRound(CV_BLOB_WX(pBlobInit));
 324         int h = cvRound(CV_BLOB_WY(pBlobInit));
 325         if(w<3)w=3;
 326         if(h<3)h=3;
 327         if(w>pImg->width)w=pImg->width;
 328         if(h>pImg->height)h=pImg->height;
 329         ReAllocKernel(w,h);
 330         calcHist(pImg, pImgFG, cvPointFrom32f(CV_BLOB_CENTER(pBlobInit)), m_KernelHistModel, m_HistModel, &m_HistModelVolume);
 331         m_Blob = pBlobInit[0];
 332     };
 333
 334     virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
 335     {
 336         int     iter;
 337
 338         if(pBlobPrev)
 339         {
 340             m_Blob = pBlobPrev[0];
 341         }
 342
 343         for(iter=0; iter<10; ++iter)
 344         {
 345 //            float   newx=0,newy=0,sum=0;
 346             float   dx=0,dy=0,sum=0;
 347             int     x,y,si;
 348
 349             CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 350             CvSize  Size   = cvSize(cvRound(m_Blob.w),cvRound(m_Blob.h));
 351
 352             if(m_ObjSize.width != Size.width || m_ObjSize.height != Size.height)
 353             {   /* Reallocate kernels: */
 354                 ReAllocKernel(Size.width,Size.height);
 355             }   /* Reallocate kernels. */
 356
 357             /* Mean shift in coordinate space: */
 358             calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
 359             calcWeights(pImg, pImgFG, Center);
 360
 361             for(si=1; si<(SCALE_NUM-1); ++si)
 362             {
 363                 CvMat*  pKernel = m_KernelMeanShiftK[si];
 364                 float   sdx = 0, sdy=0, ssum=0;
 365                 int     s = si-SCALE_RANGE;
 366                 float   factor = (1.0f-( float(s)/float(SCALE_RANGE) )*( float(s)/float(SCALE_RANGE) ));
 367
 368                 for(y=0; y<m_KernelMeanShiftSize.height; ++y)
 369                 for(x=0; x<m_KernelMeanShiftSize.width;  ++x)
 370                 {
 371                     float W = *(float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,x,sizeof(float));
 372                     float K = *(float*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,x,sizeof(float));
 373                     float KW = K*W;
 374                     ssum += (float)fabs(KW);
 375                     sdx += KW*(x-m_KernelMeanShiftSize.width*0.5f);
 376                     sdy += KW*(y-m_KernelMeanShiftSize.height*0.5f);
 377                 }   /* Next pixel. */
 378
 379                 dx += sdx * factor;
 380                 dy += sdy * factor;
 381                 sum  += ssum * factor;
 382
 383             }   /* Next scale. */
 384
 385             if(sum > 0)
 386             {
 387                 dx /= sum;
 388                 dy /= sum;
 389             }
 390
 391             m_Blob.x += dx;
 392             m_Blob.y += dy;
 393
 394             {   /* Mean shift in scale space: */
 395                 float   news = 0;
 396                 float   sum = 0;
 397                 float   scale;
 398
 399                 Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 400                 calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
 401                 calcWeights(pImg, pImgFG, Center);
 402                 //cvSet(m_Weights,cvScalar(1));
 403
 404                 for(si=0; si<SCALE_NUM; si++)
 405                 {
 406                     double  W = cvDotProduct(m_Weights, m_KernelMeanShiftG[si]);;
 407                     int     s = si-SCALE_RANGE;
 408                     sum += (float)fabs(W);
 409                     news += (float)(s*W);
 410                 }
 411
 412                 if(sum>0)
 413                 {
 414                     news /= sum;
 415                 }
 416
 417                 scale = (float)pow((double)SCALE_BASE,(double)news);
 418                 m_Blob.w *= scale;
 419                 m_Blob.h *= scale;
 420             }   /* Mean shift in scale space. */
 421
 422             /* Check fo finish: */
 423             if(fabs(dx)<0.1 && fabs(dy)<0.1) break;
 424
 425         }   /* Next iteration. */
 426
 427         if(m_Alpha>0)
 428         {   /* Update histogram: */
 429             double  Vol, WM, WC;
 430             CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 431             calcHist(pImg, pImgFG, Center, m_KernelHistModel, m_HistCandidate, &m_HistCandidateVolume);
 432             Vol = 0.5*(m_HistModelVolume + m_HistCandidateVolume);
 433             WM = Vol*(1-m_Alpha)/m_HistModelVolume;
 434             WC = Vol*(m_Alpha)/m_HistCandidateVolume;
 435             cvAddWeighted(m_HistModel, WM, m_HistCandidate,WC,0,m_HistModel);
 436             m_HistModelVolume = (float)cvSum(m_HistModel).val[0];
 437         }   /* Update histogram. */
 438
 439         return &m_Blob;
 440
 441     };  /* Process */
 442
 443     virtual void Release(){delete this;};
 444 }; /*CvBlobTrackerOneMSFGS*/
 445
 446 CvBlobTrackerOne* cvCreateBlobTrackerOneMSFGS()
 447 {
 448     return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFGS;
 449 }
 450
 451 CvBlobTracker* cvCreateBlobTrackerMSFGS()
 452 {
 453     return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFGS);
 454 }
 455