git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackingccwithcr.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 /* Blob (foreground-pixel connected-component) tracking with collision resolution.
  44  *
  45  * For entrypoints into the literature see:
  46  *
  47  *  A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking
  48  *  Arulampalam &t al, 2001, 15p
  49  *  http://www-clmc.usc.edu/publications/A/arulampalam-TSP2002.pdf
  50  *
  51  *  Particle Filters for Positioning, Navigation, and Tracking
  52  *  Gustafsson et al, 2002 12p
  53  *  http://www.control.isy.liu.se/~fredrik/reports/01SPpf4pos.pdf
  54  *
  55  *  Particle Filtering in High Clutter Environments
  56  *  Korhonen et al, 2005 4p
  57  *  http://www.cs.uku.fi/finsig05/papers/paper26_FINSIG05.pdf
  58  *
  59  *   Appearance Models for Occlusion Handling
  60  *   Andrew Senior &t al, 8p 2001
  61  *   http://www.research.ibm.com/peoplevision/PETS2001.pdf
  62  *
  63  */
  64
  65 /*============== BLOB TRACKERCC CLASS DECLARATION =============== */
  66 typedef struct DefBlobTrackerCR
  67 {
  68     CvBlob                      blob;
  69     CvBlobTrackPredictor*       pPredictor;
  70     CvBlob                      BlobPredict;
  71     CvBlob                      BlobPrev;
  72     int                         Collision;
  73     CvBlobSeq*                  pBlobHyp;
  74     CvBlobTrackerOne*           pResolver;
  75 } DefBlobTrackerCR;
  76
  77 void cvFindBlobsByCCClasters(IplImage* pFG, CvBlobSeq* pBlobs, CvMemStorage* storage);
  78
  79 class CvBlobTrackerCCCR : public CvBlobTracker
  80 {
  81 private:
  82     float           m_AlphaSize;
  83     int             m_Collision;
  84     CvBlobSeq       m_BlobList;
  85     CvBlobSeq       m_BlobListNew;
  86     CvMemStorage*   m_pMem;
  87     CvBlobTrackerOne* (*m_CreateCR)();
  88     char            m_ModuleName[1024];
  89
  90
  91 public:
  92     CvBlobTrackerCCCR(CvBlobTrackerOne* (*CreateCR)(), char* CRName):m_BlobList(sizeof(DefBlobTrackerCR))
  93     {
  94         m_CreateCR = CreateCR;
  95         m_pMem = cvCreateMemStorage();
  96
  97         m_Collision = 1; /* if 1 then collistion will be detected and processed */
  98
  99         m_AlphaSize = 0.05f;
 100         AddParam("AlphaSize",&m_AlphaSize);
 101         CommentParam("AlphaSize", "Size update speed (0..1)");
 102
 103         strcpy(m_ModuleName, "CC");
 104         if(CRName)strcat(m_ModuleName,CRName);
 105         SetModuleName(m_ModuleName);
 106
 107         {
 108             CvBlobTrackerOne* pM = m_CreateCR();
 109             TransferParamsFromChild(pM,NULL);
 110             pM->Release();
 111         }
 112         SetParam("SizeVar",0);
 113     };
 114
 115     ~CvBlobTrackerCCCR()
 116     {
 117         if(m_pMem)cvReleaseMemStorage(&m_pMem);
 118     };
 119
 120     /* Blob functions: */
 121     virtual int     GetBlobNum() {return m_BlobList.GetBlobNum();};
 122     virtual CvBlob* GetBlob(int BlobIndex){return m_BlobList.GetBlob(BlobIndex);};
 123     virtual void    SetBlob(int BlobIndex, CvBlob* pBlob)
 124     {
 125         CvBlob* pB = m_BlobList.GetBlob(BlobIndex);
 126         if(pB) pB[0] = pBlob[0];
 127     };
 128
 129     virtual CvBlob* GetBlobByID(int BlobID){return m_BlobList.GetBlobByID(BlobID);};
 130     virtual void    DelBlob(int BlobIndex)
 131     {
 132         DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(BlobIndex);
 133         if(pBT->pResolver)pBT->pResolver->Release();
 134         if(pBT->pPredictor)pBT->pPredictor->Release();
 135         delete pBT->pBlobHyp;
 136         m_BlobList.DelBlob(BlobIndex);
 137     };
 138
 139     virtual void    DelBlobByID(int BlobID)
 140     {
 141         DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(BlobID);
 142         if(pBT->pResolver)pBT->pResolver->Release();
 143         if(pBT->pPredictor)pBT->pPredictor->Release();
 144         delete pBT->pBlobHyp;
 145         m_BlobList.DelBlobByID(BlobID);
 146     };
 147
 148     virtual void    Release(){delete this;};
 149
 150     /* Add new blob to track it and assign to this blob personal ID */
 151     /* pBlob - pinter to structure with blob parameters (ID is ignored)*/
 152     /* pImg - current image */
 153     /* pImgFG - current foreground mask */
 154     /* Return pointer to new added blob: */
 155     virtual CvBlob* AddBlob(CvBlob* pB, IplImage* pImg, IplImage* pImgFG = NULL )
 156     {
 157         DefBlobTrackerCR NewB;
 158         NewB.blob = pB[0];
 159         NewB.pBlobHyp = new CvBlobSeq;
 160         NewB.pPredictor = cvCreateModuleBlobTrackPredictKalman(); /* module for predict position */
 161         NewB.pPredictor->SetParam("DataNoisePos",0.001);
 162         NewB.pPredictor->ParamUpdate();
 163         NewB.pResolver = NULL;
 164         if(m_CreateCR)
 165         {
 166             NewB.pResolver = m_CreateCR();
 167             TransferParamsToChild(NewB.pResolver,NULL);
 168             NewB.pResolver->Init(pB, pImg, pImgFG);
 169         }
 170         m_BlobList.AddBlob((CvBlob*)&NewB);
 171         return m_BlobList.GetBlob(m_BlobList.GetBlobNum()-1);
 172     };
 173
 174     virtual void    Process(IplImage* pImg, IplImage* pImgFG = NULL)
 175     {
 176         CvSeq*      cnts;
 177         CvSeq*      cnt;
 178         int i;
 179         //CvMat*      pMC = NULL;
 180
 181         if(m_BlobList.GetBlobNum() <= 0 ) return;
 182
 183         /* Clear blob list for new blobs: */
 184         m_BlobListNew.Clear();
 185
 186         assert(m_pMem);
 187         cvClearMemStorage(m_pMem);
 188         assert(pImgFG);
 189
 190         {   /* One contour - one blob: */
 191             IplImage* pBin = cvCloneImage(pImgFG);
 192             assert(pBin);
 193             cvThreshold(pBin,pBin,128,255,CV_THRESH_BINARY);
 194             cvFindContours(pBin, m_pMem, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);
 195
 196             /* Process each contour: */
 197             for(cnt = cnts; cnt; cnt=cnt->h_next)
 198             {
 199                 CvBlob  NewBlob;
 200
 201                 /* Image moments: */
 202                 double      M00,X,Y,XX,YY;
 203                 CvMoments   m;
 204                 CvRect      r = ((CvContour*)cnt)->rect;
 205                 CvMat       mat;
 206                 if(r.height < 3 || r.width < 3) continue;
 207                 cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
 208                 M00 = cvGetSpatialMoment( &m, 0, 0 );
 209                 if(M00 <= 0 ) continue;
 210                 X = cvGetSpatialMoment( &m, 1, 0 )/M00;
 211                 Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
 212                 XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
 213                 YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
 214                 NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
 215                 m_BlobListNew.AddBlob(&NewBlob);
 216
 217             }   /* Next contour. */
 218
 219             cvReleaseImage(&pBin);
 220         }
 221
 222         for(i=m_BlobList.GetBlobNum(); i>0; --i)
 223         {   /* Predict new blob position. */
 224             CvBlob*             pB = NULL;
 225             DefBlobTrackerCR*   pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
 226
 227             /* Update predictor. */
 228             pBT->pPredictor->Update(&(pBT->blob));
 229             pB = pBT->pPredictor->Predict();
 230             if(pB)
 231             {
 232                 pBT->BlobPredict = pB[0];
 233             }
 234             pBT->BlobPrev = pBT->blob;
 235         }   /* Predict new blob position. */
 236
 237
 238         if(m_BlobList.GetBlobNum()>0 && m_BlobListNew.GetBlobNum()>0)
 239         {   /* Resolve new blob to old: */
 240             int i,j;
 241             int NOld = m_BlobList.GetBlobNum();
 242             int NNew = m_BlobListNew.GetBlobNum();
 243
 244             for(i=0; i<NOld; i++)
 245             {   /* Set 0 collision and clear all hyp: */
 246                 DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
 247                 pF->Collision = 0;
 248                 pF->pBlobHyp->Clear();
 249             }   /* Set 0 collision. */
 250
 251             /* Create correspondence records: */
 252             for(j=0; j<NNew; ++j)
 253             {
 254                 CvBlob*             pB1 = m_BlobListNew.GetBlob(j);
 255                 DefBlobTrackerCR*   pFLast = NULL;
 256
 257                 for(i=0; i<NOld; i++)
 258                 {   /* Check intersection: */
 259                     int Intersection = 0;
 260                     DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
 261                     CvBlob* pB2 = &(pF->BlobPredict);
 262
 263                     if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
 264                         fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Intersection = 1;
 265
 266                     if(Intersection)
 267                     {
 268                         if(pFLast)
 269                         {
 270                             pF->Collision = pFLast->Collision = 1;
 271                         }
 272                         pFLast = pF;
 273                         pF->pBlobHyp->AddBlob(pB1);
 274                     }
 275                 }   /* Check intersection. */
 276             }   /*  Check next new blob. */
 277         }   /*  Resolve new blob to old. */
 278
 279         for(i=m_BlobList.GetBlobNum(); i>0; --i)
 280         {   /* Track each blob. */
 281             CvBlob*             pB = m_BlobList.GetBlob(i-1);
 282             DefBlobTrackerCR*   pBT = (DefBlobTrackerCR*)pB;
 283             int                 BlobID = CV_BLOB_ID(pB);
 284           //CvBlob*             pBBest = NULL;
 285           //double              DistBest = -1;
 286             int j;
 287
 288             if(pBT->pResolver)
 289             {
 290                 pBT->pResolver->SetCollision(pBT->Collision);
 291             }
 292
 293             if(pBT->Collision)
 294             {   /* Tracking in collision: */
 295                 if(pBT->pResolver)
 296                 {
 297                     pB[0] = pBT->pResolver->Process(&(pBT->BlobPredict),pImg, pImgFG)[0];
 298                 }
 299             }   /* Tracking in collision. */
 300             else
 301             {   /* Non-collision tracking: */
 302                 CvBlob  NewCC = pBT->BlobPredict;
 303                 if(pBT->pBlobHyp->GetBlobNum()==1)
 304                 {   /* One blob to one CC: */
 305                     NewCC = pBT->pBlobHyp->GetBlob(0)[0];
 306                 }
 307                 else
 308                 {   /* One blob several CC: */
 309                     CvBlob* pBBest = NULL;
 310                     double  DistBest = -1;
 311                     double  CMax = 0;
 312                     for(j=pBT->pBlobHyp->GetBlobNum();j>0;--j)
 313                     {   /* Find best CC: */
 314                         CvBlob* pBNew = pBT->pBlobHyp->GetBlob(j-1);
 315                         if(pBT->pResolver)
 316                         {     /* Choose CC by confidence: */
 317 //                            double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
 318 //                            double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
 319                             double  C = pBT->pResolver->GetConfidence(pBNew,pImg, pImgFG);
 320                             if(C > CMax || pBBest == NULL)
 321                             {
 322                                 CMax = C;
 323                                 pBBest = pBNew;
 324                             }
 325                         }
 326                         else
 327                         {    /* Choose CC by distance: */
 328                             double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
 329                             double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
 330                             double  Dist = sqrt(dx*dx+dy*dy);
 331                             if(Dist < DistBest || pBBest == NULL)
 332                             {
 333                                 DistBest = Dist;
 334                                 pBBest = pBNew;
 335                             }
 336                         }
 337                     }   /* Find best CC. */
 338                     if(pBBest)
 339                         NewCC = pBBest[0];
 340                 }   /* One blob several CC. */
 341                 pB->x = NewCC.x;
 342                 pB->y = NewCC.y;
 343                 pB->w = (m_AlphaSize)*NewCC.w+(1-m_AlphaSize)*pB->w;
 344                 pB->h = (m_AlphaSize)*NewCC.h+(1-m_AlphaSize)*pB->h;
 345                 pBT->pResolver->SkipProcess(&(pBT->BlobPredict),pImg, pImgFG);
 346             }   /* Non-collision tracking. */
 347
 348             pBT->pResolver->Update(pB, pImg, pImgFG);
 349
 350             CV_BLOB_ID(pB)=BlobID;
 351
 352         }   /* Track next blob. */
 353
 354         if(m_Wnd)
 355         {
 356             IplImage* pI = cvCloneImage(pImg);
 357             int i;
 358             for(i=m_BlobListNew.GetBlobNum(); i>0; --i)
 359             {   /* Draw each new CC: */
 360                 CvBlob* pB = m_BlobListNew.GetBlob(i-1);
 361                 CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
 362                 int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
 363                 CvSize  s = cvSize(MAX(1,x), MAX(1,y));
 364                 //int c = 255;
 365                 cvEllipse( pI,
 366                     p,
 367                     s,
 368                     0, 0, 360,
 369                     CV_RGB(255,255,0), 1 );
 370             }
 371
 372             for(i=m_BlobList.GetBlobNum(); i>0; --i)
 373             {   /* Draw each new CC: */
 374                 DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
 375                 CvBlob* pB = &(pF->BlobPredict);
 376                 CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
 377                 int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
 378                 CvSize  s = cvSize(MAX(1,x), MAX(1,y));
 379                 cvEllipse( pI,
 380                     p,
 381                     s,
 382                     0, 0, 360,
 383                     CV_RGB(0,0,255), 1 );
 384
 385                 pB = &(pF->blob);
 386                 p = cvPointFrom32f(CV_BLOB_CENTER(pB));
 387                 x = cvRound(CV_BLOB_RX(pB)); y = cvRound(CV_BLOB_RY(pB));
 388                 s = cvSize(MAX(1,x), MAX(1,y));
 389                 cvEllipse( pI,
 390                     p,
 391                     s,
 392                     0, 0, 360,
 393                     CV_RGB(0,255,0), 1 );
 394             }
 395
 396             //cvNamedWindow("CCwithCR",0);
 397             //cvShowImage("CCwithCR",pI);
 398             cvReleaseImage(&pI);
 399         }
 400
 401     } /* Process. */
 402
 403     virtual void SaveState(CvFileStorage* fs)
 404     {
 405         int     b,bN = m_BlobList.GetBlobNum();
 406         cvWriteInt(fs,"BlobNum",m_BlobList.GetBlobNum());
 407         cvStartWriteStruct(fs,"BlobList",CV_NODE_SEQ);
 408
 409         for(b=0; b<bN; ++b)
 410         {
 411             DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(b);
 412             cvStartWriteStruct(fs,NULL,CV_NODE_MAP);
 413             cvWriteInt(fs,"ID",CV_BLOB_ID(pF));
 414             cvStartWriteStruct(fs,"Blob",CV_NODE_SEQ|CV_NODE_FLOW);
 415             cvWriteRawData(fs,&(pF->blob),1,"ffffi");
 416             cvEndWriteStruct(fs);
 417             cvStartWriteStruct(fs,"BlobPredict",CV_NODE_SEQ|CV_NODE_FLOW);
 418             cvWriteRawData(fs,&(pF->BlobPredict),1,"ffffi");
 419             cvEndWriteStruct(fs);
 420             cvStartWriteStruct(fs,"BlobPrev",CV_NODE_SEQ|CV_NODE_FLOW);
 421             cvWriteRawData(fs,&(pF->BlobPrev),1,"ffffi");
 422             cvEndWriteStruct(fs);
 423             pF->pBlobHyp->Write(fs,"BlobHyp");
 424             cvWriteInt(fs,"Collision",pF->Collision);
 425
 426             cvStartWriteStruct(fs,"Predictor",CV_NODE_MAP);
 427             pF->pPredictor->SaveState(fs);
 428             cvEndWriteStruct(fs);
 429
 430             cvStartWriteStruct(fs,"Resolver",CV_NODE_MAP);
 431             pF->pResolver->SaveState(fs);
 432             cvEndWriteStruct(fs);
 433             cvEndWriteStruct(fs);
 434         }
 435
 436         cvEndWriteStruct(fs);
 437
 438     }   /* SaveState. */
 439
 440     virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
 441     {
 442         int         b,bN = cvReadIntByName(fs,node,"BlobNum",0);
 443         CvFileNode* pBlobListNode = cvGetFileNodeByName(fs,node,"BlobList");
 444         if(!CV_NODE_IS_SEQ(pBlobListNode->tag)) return;
 445         bN = pBlobListNode->data.seq->total;
 446
 447         for(b=0; b<bN; ++b)
 448         {
 449             DefBlobTrackerCR*   pF = NULL;
 450             CvBlob              Blob;
 451             CvFileNode*         pSeqNode = NULL;
 452             CvFileNode*         pBlobNode = (CvFileNode*)cvGetSeqElem(pBlobListNode->data.seq,b);
 453             assert(pBlobNode);
 454
 455             Blob.ID = cvReadIntByName(fs,pBlobNode,"ID",0);
 456
 457             pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Blob");
 458             if(CV_NODE_IS_SEQ(pSeqNode->tag))
 459                 cvReadRawData( fs, pSeqNode, &Blob, "ffffi" );
 460
 461             AddBlob(&Blob,NULL,NULL);
 462             pF = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(Blob.ID);
 463
 464             pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPredict");
 465             if(CV_NODE_IS_SEQ(pSeqNode->tag))
 466                 cvReadRawData( fs, pSeqNode, &pF->BlobPredict, "ffffi" );
 467
 468             pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPrev");
 469             if(CV_NODE_IS_SEQ(pSeqNode->tag))
 470                 cvReadRawData( fs, pSeqNode, &pF->BlobPrev, "ffffi" );
 471
 472             pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobHyp");
 473             if(pSeqNode)
 474                 pF->pBlobHyp->Load(fs,pSeqNode);
 475             pF->Collision = cvReadIntByName(fs, pBlobNode,"Collision",pF->Collision);
 476
 477             pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Predictor");
 478             if(pSeqNode)
 479                 pF->pPredictor->LoadState(fs,pSeqNode);
 480
 481             pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Resolver");
 482             if(pSeqNode)
 483                 pF->pResolver->LoadState(fs,pSeqNode);
 484         }   /* Read next blob. */
 485     }   /*  CCwithCR LoadState */
 486
 487     //void SetCollision(int Collision){m_Collision = Collision;};
 488 };
 489
 490 CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF();
 491 CvBlobTracker* cvCreateBlobTrackerCCMSPF()
 492 {
 493     return (CvBlobTracker*) new CvBlobTrackerCCCR(cvCreateBlobTrackerOneMSPF,"MSPF");
 494 }
 495 /*============== BLOB TRACKERCC CLASS DECLARATION =============== */