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