vcs.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     for(y=0;y<h;++y)for(x=0;x<w;++x)
  58     {
  59 //                float   r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
  60         float   r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((x0*x0)+(y0*y0));
  61         CV_MAT_ELEM(pK[0],DefHistType, y, x) = (DefHistType)((r2<1)?(1-r2):0);
  62     }
  63 }/* allocate kernel for histogramm creation */
  64
  65 class CvBlobTrackerOneMSFGS:public CvBlobTrackerOne
  66 {
  67 private:
  68     /* parameters */
  69     float           m_FGWeight;
  70     float           m_Alpha;
  71     CvSize          m_ObjSize;
  72     CvMat*          m_KernelHistModel;
  73     CvMat*          m_KernelHistCandidate;
  74     CvSize          m_KernelMeanShiftSize;
  75     CvMat*          m_KernelMeanShiftK[SCALE_NUM];
  76     CvMat*          m_KernelMeanShiftG[SCALE_NUM];
  77     CvMat*          m_Weights;
  78     int             m_BinBit;
  79     int             m_ByteShift;
  80     int             m_BinNum;
  81     int             m_Dim;
  82     int             m_BinNumTotal;
  83     CvMat*          m_HistModel;
  84     float           m_HistModelVolume;
  85     CvMat*          m_HistCandidate;
  86     float           m_HistCandidateVolume;
  87     CvMat*          m_HistTemp;
  88     CvBlob          m_Blob;
  89     void ReAllocHist(int Dim, int BinBit)
  90     {
  91         m_BinBit = BinBit;
  92         m_ByteShift = 8-BinBit;
  93         m_Dim = Dim;
  94         m_BinNum = (1<<BinBit);
  95         m_BinNumTotal = cvRound(pow((double)m_BinNum,(double)m_Dim));
  96         if(m_HistModel) cvReleaseMat(&m_HistModel);
  97         if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
  98         if(m_HistTemp) cvReleaseMat(&m_HistTemp);
  99         m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
 100         m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
 101         m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
 102         cvZero(m_HistCandidate);
 103         cvZero(m_HistModel);
 104         m_HistModelVolume = 0.0f;
 105         m_HistCandidateVolume = 0.0f;
 106     }
 107     void ReAllocKernel(int  w, int h, float sigma=0.4)
 108     {
 109         double  ScaleToObj = sigma*1.39;
 110         int     kernel_width = cvRound(w/ScaleToObj);
 111         int     kernel_height = cvRound(h/ScaleToObj);
 112         int     x,y,s;
 113         assert(w>0);
 114         assert(h>0);
 115         m_ObjSize = cvSize(w,h);
 116         m_KernelMeanShiftSize = cvSize(kernel_width,kernel_height);
 117
 118
 119         /* create kernels for histogramm calculation */
 120         if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
 121         m_KernelHistModel = cvCreateMat(h, w, DefHistTypeMat);
 122         calcKernelEpanechnikov(m_KernelHistModel);
 123         if(m_KernelHistCandidate) cvReleaseMat(&m_KernelHistCandidate);
 124         m_KernelHistCandidate = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
 125         calcKernelEpanechnikov(m_KernelHistCandidate);
 126
 127         if(m_Weights) cvReleaseMat(&m_Weights);
 128         m_Weights = cvCreateMat(kernel_height, kernel_width, CV_32F);
 129
 130         for(s=-SCALE_RANGE;s<=SCALE_RANGE;++s)
 131         {/* allocate kernwl for meanshifts in space and scale */
 132             int     si = s+SCALE_RANGE;
 133             double  cur_sigma = sigma * pow(SCALE_BASE,s);
 134             double  cur_sigma2 = cur_sigma*cur_sigma;
 135             double  x0 = 0.5*(kernel_width-1);
 136             double  y0 = 0.5*(kernel_height-1);
 137             if(m_KernelMeanShiftK[si]) cvReleaseMat(&m_KernelMeanShiftK[si]);
 138             if(m_KernelMeanShiftG[si]) cvReleaseMat(&m_KernelMeanShiftG[si]);
 139             m_KernelMeanShiftK[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
 140             m_KernelMeanShiftG[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
 141             for(y=0;y<kernel_height;++y)
 142             {
 143                 DefHistType* pK = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftK[si][0], y, 0, sizeof(DefHistType) );
 144                 DefHistType* pG = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftG[si][0], y, 0, sizeof(DefHistType) );
 145                 for(x=0;x<kernel_width;++x)
 146                 {
 147                     double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
 148                     double sigma12 = cur_sigma2 / 2.56;
 149                     double sigma22 = cur_sigma2 * 2.56;
 150                     pK[x] = (DefHistType)(Gaussian2D(r2, sigma12)/sigma12 - Gaussian2D(r2, sigma22)/sigma22);
 151                     pG[x] = (DefHistType)(Gaussian2D(r2, cur_sigma2/1.6) - Gaussian2D(r2, cur_sigma2*1.6));
 152                 }
 153             }/* next line */
 154         }
 155     }/* ReallocKernel*/
 156     inline double Gaussian2D(double x, double sigma2)
 157     {
 158         return (exp(-x/(2*sigma2)) / (2*3.1415926535897932384626433832795*sigma2) );
 159     }
 160     void calcHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pKernel, CvMat* pHist, DefHistType* pHistVolume)
 161     {
 162         int         w = pKernel->width;
 163         int         h = pKernel->height;
 164         DefHistType Volume = 0;
 165         int         x0 = Center.x - w/2;
 166         int         y0 = Center.y - h/2;
 167         int         x,y;
 168
 169         //cvZero(pHist);
 170         cvSet(pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
 171         Volume = 1;
 172
 173         if(m_Dim == 3)
 174         {
 175             for(y=0;y<h;++y)
 176             {
 177                 unsigned char* pImgData = NULL;
 178                 unsigned char* pMaskData = NULL;
 179                 DefHistType* pKernelData = NULL;
 180                 if((y0+y)>=pImg->height) continue;
 181                 if((y0+y)<0)continue;
 182                 pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
 183                 pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
 184                 pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,0,sizeof(DefHistType));
 185                 for(x=0;x<w;++x,pImgData+=3)
 186                 {
 187                     if((x0+x)>=pImg->width) continue;
 188                     if((x0+x)<0)continue;
 189                     if(pMaskData==NULL || pMaskData[x]>128)
 190                     {
 191                         DefHistType K = pKernelData[x];
 192                         int index = HIST_INDEX(pImgData);
 193                         assert(index >= 0 && index < pHist->cols);
 194                         Volume += K;
 195                         ((DefHistType*)(pHist->data.ptr))[index] += K;
 196                     }/* only masked pixels */
 197                 }/* next column */
 198             }/* next row */
 199         }/* if m_Dim == 3 */
 200         if(pHistVolume)pHistVolume[0] = Volume;
 201     };/*calcHist*/
 202     double calcBhattacharyya()
 203     {
 204         cvMul(m_HistCandidate,m_HistModel,m_HistTemp);
 205         cvPow(m_HistTemp,m_HistTemp,0.5);
 206         return cvSum(m_HistTemp).val[0] / sqrt(m_HistCandidateVolume*m_HistModelVolume);
 207     } /* calcBhattacharyyaCoefficient */
 208     void calcWeights(IplImage* pImg, IplImage* pImgFG, CvPoint Center)
 209     {
 210         cvZero(m_Weights);
 211         /* calc new pos */
 212         if(m_Dim == 3)
 213         {
 214             int         x0 = Center.x - m_KernelMeanShiftSize.width/2;
 215             int         y0 = Center.y - m_KernelMeanShiftSize.height/2;
 216             int         x,y;
 217
 218             assert(m_Weights->width == m_KernelMeanShiftSize.width);
 219             assert(m_Weights->height == m_KernelMeanShiftSize.height);
 220
 221             /*calc shift vector */
 222             for(y=0;y<m_KernelMeanShiftSize.height;++y)
 223             {
 224                 unsigned char* pImgData = NULL;
 225                 unsigned char* pMaskData = NULL;
 226                 float* pWData = NULL;
 227
 228                 if(y+y0 < 0 || y+y0 >= pImg->height) continue;
 229
 230                 pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
 231                 pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
 232                 pWData = (float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,0,sizeof(float));
 233
 234                 for(x=0;x<m_KernelMeanShiftSize.width;++x,pImgData+=3)
 235                 {
 236                     double      V  = 0;
 237                     double      HM = 0;
 238                     double      HC = 0;
 239                     int         index;
 240                     if(x+x0 < 0 || x+x0 >= pImg->width) continue;
 241
 242                     index = HIST_INDEX(pImgData);
 243                     assert(index >= 0 && index < m_BinNumTotal);
 244
 245                     if(m_HistModelVolume>0)
 246                         HM = ((DefHistType*)m_HistModel->data.ptr)[index]/m_HistModelVolume;
 247                     if(m_HistCandidateVolume>0)
 248                         HC = ((DefHistType*)m_HistCandidate->data.ptr)[index]/m_HistCandidateVolume;
 249
 250                     V = (HC>0)?sqrt(HM / HC):0;
 251                     V += m_FGWeight*(pMaskData?((pMaskData[x]/255.0f)):0);
 252                     pWData[x] = (float)MIN(V,100000);
 253                 }/* next column */
 254             }/* next row */
 255         }/* if m_Dim == 3 */
 256     }/* calcWeights */
 257
 258 public:
 259     CvBlobTrackerOneMSFGS()
 260     {
 261         int i;
 262         m_FGWeight = 0;
 263         m_Alpha = 0.0;
 264
 265         /* add several parameters for external use */
 266         AddParam("FGWeight", &m_FGWeight);
 267         CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
 268         AddParam("Alpha", &m_Alpha);
 269         CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");
 270
 271         m_BinBit=0;
 272         m_Dim = 0;
 273         m_HistModel = NULL;
 274         m_HistCandidate = NULL;
 275         m_HistTemp = NULL;
 276         m_KernelHistModel = NULL;
 277         m_KernelHistCandidate = NULL;
 278         m_Weights = NULL;
 279         for(i=0;i<SCALE_NUM;++i)
 280         {
 281             m_KernelMeanShiftK[i] = NULL;
 282             m_KernelMeanShiftG[i] = NULL;
 283         }
 284         ReAllocHist(3,5);   /* 3D hist, each dim has 2^5 bins*/
 285     }
 286     ~CvBlobTrackerOneMSFGS()
 287     {
 288         int i;
 289         if(m_HistModel) cvReleaseMat(&m_HistModel);
 290         if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
 291         if(m_HistTemp) cvReleaseMat(&m_HistTemp);
 292         if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
 293         for(i=0;i<SCALE_NUM;++i)
 294         {
 295             if(m_KernelMeanShiftK[i]) cvReleaseMat(&m_KernelMeanShiftK[i]);
 296             if(m_KernelMeanShiftG[i]) cvReleaseMat(&m_KernelMeanShiftG[i]);
 297         }
 298     }
 299     /* interface */
 300     virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
 301     {
 302         int w = cvRound(CV_BLOB_WX(pBlobInit));
 303         int h = cvRound(CV_BLOB_WY(pBlobInit));
 304         if(w<3)w=3;
 305         if(h<3)h=3;
 306         if(w>pImg->width)w=pImg->width;
 307         if(h>pImg->height)h=pImg->height;
 308         ReAllocKernel(w,h);
 309         calcHist(pImg, pImgFG, cvPointFrom32f(CV_BLOB_CENTER(pBlobInit)), m_KernelHistModel, m_HistModel, &m_HistModelVolume);
 310         m_Blob = pBlobInit[0];
 311     };
 312     virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
 313     {
 314         int     iter;
 315
 316         if(pBlobPrev)
 317         {
 318             m_Blob = pBlobPrev[0];
 319         }
 320         for(iter=0;iter<10;++iter)
 321         {
 322 //            float   newx=0,newy=0,sum=0;
 323             float   dx=0,dy=0,sum=0;
 324             int     x,y,si;
 325             CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 326             CvSize  Size = cvSize(cvRound(m_Blob.w),cvRound(m_Blob.h));
 327
 328             if(m_ObjSize.width != Size.width || m_ObjSize.height != Size.height)
 329             {   /* realloc kernels */
 330                 ReAllocKernel(Size.width,Size.height);
 331             }   /* realloc kernels */
 332
 333             /* mean shift in coordinate space */
 334             calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
 335             calcWeights(pImg, pImgFG, Center);
 336             for(si=1;si<(SCALE_NUM-1);++si)
 337             {
 338                 CvMat*  pKernel = m_KernelMeanShiftK[si];
 339                 float   sdx = 0, sdy=0, ssum=0;
 340                 int     s = si-SCALE_RANGE;
 341                 float   factor = (1.0f-( float(s)/float(SCALE_RANGE) )*( float(s)/float(SCALE_RANGE) ));
 342
 343                 for(y=0;y<m_KernelMeanShiftSize.height;++y)
 344                 for(x=0;x<m_KernelMeanShiftSize.width;++x)
 345                 {
 346                     float W = *(float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,x,sizeof(float));
 347                     float K = *(float*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,x,sizeof(float));
 348                     float KW = K*W;
 349                     ssum += (float)fabs(KW);
 350                     sdx += KW*(x-m_KernelMeanShiftSize.width*0.5f);
 351                     sdy += KW*(y-m_KernelMeanShiftSize.height*0.5f);
 352                 }/* next pixel */
 353                 dx += sdx * factor;
 354                 dy += sdy * factor;
 355                 sum  += ssum * factor;
 356             }/* next scale */
 357             if(sum > 0)
 358             {
 359                 dx /= sum;
 360                 dy /= sum;
 361             }
 362
 363             m_Blob.x += dx;
 364             m_Blob.y += dy;
 365
 366             {   /* mean shift in scale space */
 367                 float   news = 0;
 368                 float   sum = 0;
 369                 float   scale;
 370
 371                 Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 372                 calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
 373                 calcWeights(pImg, pImgFG, Center);
 374                 //cvSet(m_Weights,cvScalar(1));
 375                 for(si=0; si<SCALE_NUM; si++)
 376                 {
 377                     double  W = cvDotProduct(m_Weights, m_KernelMeanShiftG[si]);;
 378                     int     s = si-SCALE_RANGE;
 379                     sum += (float)fabs(W);
 380                     news += (float)(s*W);
 381                 }
 382                 if(sum>0)
 383                 {
 384                     news /= sum;
 385                 }
 386                 scale = (float)pow((double)SCALE_BASE,(double)news);
 387                 m_Blob.w *= scale;
 388                 m_Blob.h *= scale;
 389             }   /* mean shift in scale space */
 390
 391             /* check fo finish */
 392             if(fabs(dx)<0.1 && fabs(dy)<0.1) break;
 393         }/* next iteration */
 394
 395         if(m_Alpha>0)
 396         {/* update hist */
 397             double  Vol, WM, WC;
 398             CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
 399             calcHist(pImg, pImgFG, Center, m_KernelHistModel, m_HistCandidate, &m_HistCandidateVolume);
 400             Vol = 0.5*(m_HistModelVolume + m_HistCandidateVolume);
 401             WM = Vol*(1-m_Alpha)/m_HistModelVolume;
 402             WC = Vol*(m_Alpha)/m_HistCandidateVolume;
 403             cvAddWeighted(m_HistModel, WM, m_HistCandidate,WC,0,m_HistModel);
 404             m_HistModelVolume = (float)cvSum(m_HistModel).val[0];
 405         }/* update hist */
 406
 407         return &m_Blob;
 408     }; /* Process */
 409     virtual void Release(){delete this;};
 410 }; /*CvBlobTrackerOneMSFGS*/
 411
 412 CvBlobTrackerOne* cvCreateBlobTrackerOneMSFGS()
 413 {
 414     return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFGS;
 415 }
 416 CvBlobTracker* cvCreateBlobTrackerMSFGS()
 417 {
 418     return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFGS);
 419 }
 420