git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackingcc.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 static float CalcAverageMask(CvBlob* pBlob, IplImage* pImgFG )
  44 {/* calc summ of mask */
  45     double  Area, Aver = 0;
  46     CvRect  r;
  47     CvMat   mat;
  48
  49     if(pImgFG==NULL) return 0;
  50
  51     r.x = cvRound(pBlob->x - pBlob->w*0.5);
  52     r.y = cvRound(pBlob->y - pBlob->h*0.5);
  53     r.width = cvRound(pBlob->w);
  54     r.height = cvRound(pBlob->h);
  55     Area = r.width*r.height;
  56     if(r.x<0){r.width += r.x;r.x = 0;}
  57     if(r.y<0){r.height += r.y;r.y = 0;}
  58     if((r.x+r.width)>=pImgFG->width){r.width=pImgFG->width-r.x-1;}
  59     if((r.y+r.height)>=pImgFG->height){r.height=pImgFG->height-r.y-1;}
  60     if(r.width>0 && r.height>0)
  61     {
  62         double Sum = cvSum(cvGetSubRect(pImgFG,&mat,r)).val[0]/255.0;
  63         assert(Area>0);
  64         Aver = Sum/Area;
  65     }
  66     return (float)Aver;
  67 }/* calc summ of mask */
  68
  69
  70 /*============== BLOB TRACKERCC CLASS DECLARATION =============== */
  71 typedef struct DefBlobTracker
  72 {
  73     CvBlob                      blob;
  74     CvBlobTrackPredictor*       pPredictor;
  75     CvBlob                      BlobPredict;
  76     int                         Collision;
  77     CvBlobSeq*                  pBlobHyp;
  78     float                       AverFG;
  79 }DefBlobTracker;
  80 void cvFindBlobsByCCClasters(IplImage* pFG, CvBlobSeq* pBlobs, CvMemStorage* storage);
  81
  82 class CvBlobTrackerCC : public CvBlobTracker
  83 {
  84 private:
  85     float           m_AlphaSize;
  86     float           m_AlphaPos;
  87     float           m_Alpha;
  88     int             m_Collision;
  89     int             m_ConfidenceType;
  90     char*           m_ConfidenceTypeStr;
  91     CvBlobSeq       m_BlobList;
  92     CvBlobSeq       m_BlobListNew;
  93 //    int             m_LastID;
  94     CvMemStorage*   m_pMem;
  95     int             m_ClearHyp;
  96     IplImage*       m_pImg;
  97     IplImage*       m_pImgFG;
  98 public:
  99     CvBlobTrackerCC():m_BlobList(sizeof(DefBlobTracker))
 100     {
 101 //        m_LastID = 0;
 102         m_ClearHyp = 0;
 103         m_pMem = cvCreateMemStorage();
 104         m_Collision = 1; /* if 1 then collistion will be detected and processed */
 105         AddParam("Collision",&m_Collision);
 106         CommentParam("Collision", "if 1 then collision cases are processed in special way");
 107
 108         m_AlphaSize = 0.02f;
 109         AddParam("AlphaSize",&m_AlphaSize);
 110         CommentParam("AlphaSize", "Size update speed (0..1)");
 111
 112         m_AlphaPos = 1.0f;
 113         AddParam("AlphaPos",&m_AlphaPos);
 114         CommentParam("AlphaPos", "Pos update speed (0..1)");
 115
 116         m_Alpha = 0.001f;
 117         AddParam("Alpha", &m_Alpha);
 118         CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");
 119
 120         m_ConfidenceType=0;
 121         m_ConfidenceTypeStr = "NearestBlob";
 122         AddParam("ConfidenceType", &m_ConfidenceTypeStr);
 123         CommentParam("ConfidenceType","Type of calculated Confidence (NearestBlob, AverFG, BC)");
 124
 125         SetModuleName("CC");
 126     };
 127     ~CvBlobTrackerCC()
 128     {
 129         if(m_pMem)cvReleaseMemStorage(&m_pMem);
 130     };
 131
 132     /* blob functions*/
 133     virtual int     GetBlobNum() {return m_BlobList.GetBlobNum();};
 134     virtual CvBlob* GetBlob(int BlobIndex){return m_BlobList.GetBlob(BlobIndex);};
 135     virtual void    SetBlob(int BlobIndex, CvBlob* pBlob)
 136     {
 137         CvBlob* pB = m_BlobList.GetBlob(BlobIndex);
 138         if(pB) pB[0] = pBlob[0];
 139     };
 140     virtual CvBlob* GetBlobByID(int BlobID){return m_BlobList.GetBlobByID(BlobID);};
 141     virtual void    DelBlob(int BlobIndex)
 142     {
 143         DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
 144         if(pBT==NULL) return;
 145         if(pBT->pPredictor)
 146         {
 147             pBT->pPredictor->Release();
 148         }
 149         else
 150         {
 151             printf("WARNING!!! Invalid Predictor in CC tracker");
 152         }
 153         delete pBT->pBlobHyp;
 154         m_BlobList.DelBlob(BlobIndex);
 155     };
 156 #if 0
 157     virtual void    DelBlobByID(int BlobID)
 158     {
 159         DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlobByID(BlobID);
 160         pBT->pPredictor->Release();
 161         delete pBT->pBlobHyp;
 162         m_BlobList.DelBlobByID(BlobID);
 163     };
 164 #endif
 165     virtual void    Release(){delete this;};
 166
 167     /* Add new blob to track it and assign to this blob personal ID */
 168     /* pBlob - pinter to structure with blob parameters (ID is ignored)*/
 169     /* pImg - current image */
 170     /* pImgFG - current foreground mask */
 171     /* return pointer to new added blob */
 172     virtual CvBlob* AddBlob(CvBlob* pB, IplImage* /*pImg*/, IplImage* pImgFG = NULL )
 173     {
 174         assert(pImgFG); /* this tracker use only foreground mask */
 175         DefBlobTracker NewB;
 176         NewB.blob = pB[0];
 177 //        CV_BLOB_ID(&NewB) = m_LastID;
 178         NewB.pBlobHyp = new CvBlobSeq;
 179         NewB.pPredictor = cvCreateModuleBlobTrackPredictKalman(); /* module for predict position */
 180         NewB.pPredictor->Update(pB);
 181         NewB.AverFG = pImgFG?CalcAverageMask(pB,pImgFG):0;
 182         m_BlobList.AddBlob((CvBlob*)&NewB);
 183         return m_BlobList.GetBlob(m_BlobList.GetBlobNum()-1);
 184     };
 185     virtual void    Process(IplImage* pImg, IplImage* pImgFG = NULL)
 186     {
 187         CvSeq*      cnts;
 188         CvSeq*      cnt;
 189         int i;
 190
 191         m_pImg = pImg;
 192         m_pImgFG = pImgFG;
 193
 194         if(m_BlobList.GetBlobNum() <= 0 ) return;
 195
 196         /* clear blob list for new blobs */
 197         m_BlobListNew.Clear();
 198
 199         assert(m_pMem);
 200         cvClearMemStorage(m_pMem);
 201         assert(pImgFG);
 202
 203
 204         /* find CC */
 205 #if 0
 206         {// by contur clastring
 207             cvFindBlobsByCCClasters(pImgFG, &m_BlobListNew, m_pMem);
 208         }
 209 #else
 210         {/* one contour - one blob */
 211             IplImage* pBin = cvCloneImage(pImgFG);
 212             assert(pBin);
 213             cvThreshold(pBin,pBin,128,255,CV_THRESH_BINARY);
 214             cvFindContours(pBin, m_pMem, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);
 215             /* process each contours*/
 216             for(cnt = cnts;cnt;cnt=cnt->h_next)
 217             {
 218                 CvBlob  NewBlob;
 219                 /* image moments */
 220                 double      M00,X,Y,XX,YY;
 221                 CvMoments   m;
 222                 CvRect      r = ((CvContour*)cnt)->rect;
 223                 CvMat       mat;
 224                 if(r.height < 3 || r.width < 3) continue;
 225                 cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
 226                 M00 = cvGetSpatialMoment( &m, 0, 0 );
 227                 if(M00 <= 0 ) continue;
 228                 X = cvGetSpatialMoment( &m, 1, 0 )/M00;
 229                 Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
 230                 XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
 231                 YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
 232                 NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
 233                 m_BlobListNew.AddBlob(&NewBlob);
 234             }/* next contour */
 235             cvReleaseImage(&pBin);
 236         }
 237 #endif
 238         for(i=m_BlobList.GetBlobNum();i>0;--i)
 239         {/* predict new blob position */
 240             CvBlob*         pB=NULL;
 241             DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(i-1);
 242
 243             /* update predictor by previouse value of blob*/
 244             pBT->pPredictor->Update(&(pBT->blob));
 245
 246             /* predict current position */
 247             pB = pBT->pPredictor->Predict();
 248
 249             if(pB)
 250             {
 251                 pBT->BlobPredict = pB[0];
 252             }
 253             else
 254             {
 255                 pBT->BlobPredict = pBT->blob;
 256             }
 257         }/* predict new blob position */
 258
 259         if(m_Collision)
 260         for(i=m_BlobList.GetBlobNum();i>0;--i)
 261         {/* predict collision */
 262             int             Collision = 0;
 263             int             j;
 264             DefBlobTracker* pF = (DefBlobTracker*)m_BlobList.GetBlob(i-1);
 265             for(j=m_BlobList.GetBlobNum();j>0;--j)
 266             {/* predict collision */
 267                 CvBlob* pB1;
 268                 CvBlob* pB2;
 269                 DefBlobTracker* pF2 = (DefBlobTracker*)m_BlobList.GetBlob(j-1);
 270                 if(i==j) continue;
 271                 pB1 = &pF->BlobPredict;
 272                 pB2 = &pF2->BlobPredict;
 273                 if( fabs(pB1->x-pB2->x)<0.6*(pB1->w+pB2->w) &&
 274                     fabs(pB1->y-pB2->y)<0.6*(pB1->h+pB2->h) ) Collision = 1;
 275                 pB1 = &pF->blob;
 276                 pB2 = &pF2->blob;
 277                 if( fabs(pB1->x-pB2->x)<0.6*(pB1->w+pB2->w) &&
 278                     fabs(pB1->y-pB2->y)<0.6*(pB1->h+pB2->h) ) Collision = 1;
 279                 if(Collision) break;
 280             }/* check next blob to cross current*/
 281             pF->Collision = Collision;
 282         }/* predict collision */
 283
 284         for(i=m_BlobList.GetBlobNum();i>0;--i)
 285         {/* find a neighbour on cur frame for each blob from prev frame */
 286             CvBlob*         pB = m_BlobList.GetBlob(i-1);
 287             DefBlobTracker* pBT = (DefBlobTracker*)pB;
 288             //int             BlobID = CV_BLOB_ID(pB);
 289             //CvBlob*         pBBest = NULL;
 290             //double          DistBest = -1;
 291             //int j;
 292
 293             if(pBT->pBlobHyp->GetBlobNum()>0)
 294             {/* track all hypothesis */
 295                 int h,hN = pBT->pBlobHyp->GetBlobNum();
 296                 for(h=0;h<hN;++h)
 297                 {
 298                     int         j, jN = m_BlobListNew.GetBlobNum();
 299                     CvBlob*     pB = pBT->pBlobHyp->GetBlob(h);
 300                     int         BlobID = CV_BLOB_ID(pB);
 301                     CvBlob*     pBBest = NULL;
 302                     double      DistBest = -1;
 303                     for(j=0;j<jN;j++)
 304                     {/* find best CC */
 305                         double  Dist = -1;
 306                         CvBlob* pBNew = m_BlobListNew.GetBlob(j);
 307                         double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
 308                         double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
 309                         if(dx > 2*CV_BLOB_WX(pB) || dy > 2*CV_BLOB_WY(pB)) continue;
 310
 311                         Dist = sqrt(dx*dx+dy*dy);
 312                         if(Dist < DistBest || pBBest == NULL)
 313                         {
 314                             DistBest = Dist;
 315                             pBBest = pBNew;
 316                         }
 317                     }/* find best CC */
 318                     if(pBBest)
 319                     {
 320                         pB[0] = pBBest[0];
 321                         CV_BLOB_ID(pB) = BlobID;
 322                     }
 323                     else
 324                     { /* delete this hypothesis */
 325                         pBT->pBlobHyp->DelBlob(h);
 326                         h--;
 327                         hN--;
 328                     }
 329                 }/* next hyp*/
 330             }/* track all hypothesis */
 331         }/* track next blob */
 332
 333         m_ClearHyp = 1;
 334
 335     }/* Process */
 336
 337     virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
 338     {
 339         int             ID = pBlob->ID;
 340         CvBlob*         pB = m_BlobList.GetBlob(BlobIndex);
 341         DefBlobTracker* pBT = (DefBlobTracker*)pB;
 342         //CvBlob*         pBBest = NULL;
 343         //double          DistBest = -1;
 344         int             BlobID;
 345
 346         if(pB==NULL) return;
 347
 348         BlobID = pB->ID;
 349
 350         if(m_Collision && pBT->Collision)
 351         {/* tracking in collision */
 352             pB[0]=pBT->BlobPredict;
 353             CV_BLOB_ID(pB)=BlobID;
 354         }/* tracking in collision */
 355         else
 356         {/* not collision tracking */
 357             CvBlob* pBBest = GetNearestBlob(pB);
 358             if(pBBest)
 359             {
 360                 float   w = pBlob->w*(1-m_AlphaSize)+m_AlphaSize*pBBest->w;
 361                 float   h = pBlob->h*(1-m_AlphaSize)+m_AlphaSize*pBBest->h;
 362                 float   x = pBlob->x*(1-m_AlphaPos)+m_AlphaPos*pBBest->x;
 363                 float   y = pBlob->y*(1-m_AlphaPos)+m_AlphaPos*pBBest->y;
 364                 pB->w = w;
 365                 pB->h = h;
 366                 pB->x = x;
 367                 pB->y = y;
 368                 CV_BLOB_ID(pB) = BlobID;
 369             }
 370         }/* not collision tracking */
 371
 372         pBlob[0] = pB[0];
 373         pBlob->ID = ID;
 374     };
 375
 376     virtual double  GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* /*pImg*/, IplImage* pImgFG = NULL)
 377     {
 378         /* define koefficients in exp by exp(-XT*K)=VT */
 379         static double _KS = -log(0.1)/pow(0.5,2); /* XT = 1, VT = 0.1 - when size is Larger in 2 times Confidence is smoller in 10 times */
 380         static double _KP = -log(0.1)/pow(m_pImg->width*0.02,2); /* XT = 0.02*ImgWidth, VT = 0.1*/
 381         DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
 382         float   dx,dy,dw,dh;
 383         float   dp2,ds2;
 384         double  W = 1;
 385         CvBlob* pBC = GetNearestBlob(pBlob);
 386         if(pBC == NULL ) return 0;
 387
 388         dx = pBC->x-pBlob->x;
 389         dy = pBC->y-pBlob->y;
 390         dw = (pBC->w-pBlob->w)/pBC->w;
 391         dh = (pBC->h-pBlob->h)/pBC->h;
 392
 393         dp2 = dx*dx+dy*dy;
 394         ds2 = dw*dw+dh*dh;
 395
 396         if(!pBT->Collision)
 397         { /* Confidence for size by nearest blob */
 398             W*=exp(-_KS*ds2);
 399         }
 400
 401         if(m_ConfidenceType==0 && !pBT->Collision)
 402         { /* confinence by nearest blob */
 403             W*=exp(-_KP*dp2);
 404         }
 405
 406         if(m_ConfidenceType==1 && pBT->AverFG>0)
 407         {/* calc summ of mask */
 408             float   Aver = CalcAverageMask(pBlob, pImgFG );
 409             if(Aver < pBT->AverFG)
 410             {
 411                 float diff = 1+0.9f*(Aver-pBT->AverFG)/pBT->AverFG;
 412                 if(diff < 0.1f) diff = 0.1f;
 413                 W *= diff;
 414             }
 415         }/* calc summ of mask */
 416
 417         if(m_ConfidenceType==2)
 418         {/* calc BCoeff */
 419             float   S = 0.2f;
 420             float   Aver = CalcAverageMask(pBlob, pImgFG );
 421             double B = sqrt(Aver*pBT->AverFG)+sqrt((1-Aver)*(1-pBT->AverFG));
 422
 423             W *= exp((B-1)/(2*S));
 424         }/* calc summ of mask */
 425         return W;
 426     };
 427
 428     virtual void UpdateBlob(int BlobIndex, CvBlob* /*pBlob*/, IplImage* /*pImg*/, IplImage* pImgFG = NULL)
 429     {
 430         DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
 431
 432         if(pImgFG==NULL || pBT==NULL) return;
 433
 434         if(!pBT->Collision)
 435         {
 436         //pBT->AverFG = pBT->AverFG * (1-m_Alpha) + m_Alpha * CalcAverageMask(pBlob,pImgFG);
 437         }
 438     };
 439
 440     virtual void ParamUpdate()
 441     {
 442         char*   pCT[3] = {"NearestBlob","AverFG","BC"};
 443         int     i;
 444
 445         CvBlobTracker::ParamUpdate();
 446
 447         for(i=0;i<3;++i)
 448         {
 449             if(cv_stricmp(m_ConfidenceTypeStr,pCT[i])==0)
 450             {
 451                 m_ConfidenceType = i;
 452             }
 453         }
 454         SetParamStr("ConfidenceType",pCT[m_ConfidenceType]);
 455     }
 456     /*  ===============  MULTI HYPOTHESIS INTERFACE ==================  */
 457     /* return number of position hyposetis of currently tracked blob */
 458     virtual int     GetBlobHypNum(int BlobIdx)
 459     {
 460         DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIdx);
 461         assert(pBT->pBlobHyp);
 462         return pBT->pBlobHyp->GetBlobNum();
 463     };/* CvBlobtrackerList::GetBlobHypNum() */
 464
 465     /* return pointer to specified blob hypothesis by index blob */
 466     virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis)
 467     {
 468         DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
 469         assert(pBT->pBlobHyp);
 470         return pBT->pBlobHyp->GetBlob(hypothesis);
 471     };/* CvBlobtrackerList::GetBlobHyp() */
 472     /* Set new parameters for specified (by index) blob hyp (can be called several times for each hyp )*/
 473     virtual void    SetBlobHyp(int BlobIndex, CvBlob* pBlob)
 474     {
 475         if(m_ClearHyp)
 476         {/* clear all hypothesis */
 477             int b, bN = m_BlobList.GetBlobNum();
 478             for(b=0;b<bN;++b)
 479             {
 480                 DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(b);
 481                 assert(pBT->pBlobHyp);
 482                 pBT->pBlobHyp->Clear();
 483             }
 484             m_ClearHyp = 0;
 485         }
 486         {/* add hypothesis */
 487             DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
 488             assert(pBT->pBlobHyp);
 489             pBT->pBlobHyp->AddBlob(pBlob);
 490         }
 491     };
 492
 493 private:
 494     CvBlob* GetNearestBlob(CvBlob* pB)
 495     {
 496         //DefBlobTracker* pBT = (DefBlobTracker*)pB;
 497         CvBlob*         pBBest = NULL;
 498         double          DistBest = -1;
 499         int             j,BlobID;
 500
 501         if(pB==NULL) return NULL;
 502
 503         BlobID = pB->ID;
 504
 505         for(j=m_BlobListNew.GetBlobNum();j>0;--j)
 506         {/* find best CC */
 507             double  Dist = -1;
 508             CvBlob* pBNew = m_BlobListNew.GetBlob(j-1);
 509             double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
 510             double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
 511             if(dx > 2*CV_BLOB_WX(pB) || dy > 2*CV_BLOB_WY(pB)) continue;
 512
 513             Dist = sqrt(dx*dx+dy*dy);
 514             if(Dist < DistBest || pBBest == NULL)
 515             {
 516                 DistBest = Dist;
 517                 pBBest = pBNew;
 518             }
 519         }/* find best CC */
 520         return pBBest;
 521     }; /* GetNearestBlob */
 522
 523 };
 524
 525 CvBlobTracker* cvCreateBlobTrackerCC()
 526 {
 527     return (CvBlobTracker*) new CvBlobTrackerCC;
 528 }
 529 /*============== BLOB TRACKERCC CLASS DECLARATION =============== */