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