blobtrackingccwithcr.cpp

Program use openCV to demo edge detection (algorithm Gradient).

                    CvBlob* pB2 = &(pF->BlobPredict);
                    if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
                        fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Intersection = 1;
                    if(Intersection)
                    {
                        if(pFLast)
                        {
                            pF->Collision = pFLast->Collision = 1;
                        }
                        pFLast = pF;
                        pF->pBlobHyp->AddBlob(pB1);
                    }
                }/* check intersection */
            }/* check next new blob  */
        }/* resolve new blob to old */

        for(i=m_BlobList.GetBlobNum();i>0;--i)
        {/* track each blob */
            CvBlob*             pB = m_BlobList.GetBlob(i-1);
            DefBlobTrackerCR*   pBT = (DefBlobTrackerCR*)pB;
            int                 BlobID = CV_BLOB_ID(pB);
            //CvBlob*             pBBest = NULL;
            //double              DistBest = -1;
            int j;

            if(pBT->pResolver)
            {
                pBT->pResolver->SetCollision(pBT->Collision);
            }
            
            if(pBT->Collision)
            {/* tracking in collision */
                if(pBT->pResolver)
                {
                    pB[0] = pBT->pResolver->Process(&(pBT->BlobPredict),pImg, pImgFG)[0];
                }
            }/* tracking in collision */
            else
            {/* not collision tracking */
                CvBlob  NewCC = pBT->BlobPredict;
                if(pBT->pBlobHyp->GetBlobNum()==1)
                {/* one blob to one CC */
                    NewCC = pBT->pBlobHyp->GetBlob(0)[0];
                }
                else
                {/* one blob several CC */
                    CvBlob* pBBest = NULL;
                    double  DistBest = -1;
                    double  CMax = 0;
                    for(j=pBT->pBlobHyp->GetBlobNum();j>0;--j)
                    {/* find best CC */
                        CvBlob* pBNew = pBT->pBlobHyp->GetBlob(j-1);
                        if(pBT->pResolver)
                        {/* choose CC by confidence */
//                            double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
//                            double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
                            double  C = pBT->pResolver->GetConfidence(pBNew,pImg, pImgFG);
                            if(C > CMax || pBBest == NULL)
                            {
                                CMax = C;
                                pBBest = pBNew;
                            }
                        }
                        else
                        { /* chose CC by distance */
                            double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
                            double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
                            double  Dist = sqrt(dx*dx+dy*dy);
                            if(Dist < DistBest || pBBest == NULL)
                            {
                                DistBest = Dist;
                                pBBest = pBNew;
                            }
                        }
                    }/* find best CC */
                    if(pBBest)
                        NewCC = pBBest[0];
                }/* one blob several CC */
                pB->x = NewCC.x;
                pB->y = NewCC.y;
                pB->w = (m_AlphaSize)*NewCC.w+(1-m_AlphaSize)*pB->w;
                pB->h = (m_AlphaSize)*NewCC.h+(1-m_AlphaSize)*pB->h;
                pBT->pResolver->SkipProcess(&(pBT->BlobPredict),pImg, pImgFG);
            }/* not collision tracking */
            
            pBT->pResolver->Update(pB, pImg, pImgFG);

            CV_BLOB_ID(pB)=BlobID;

        }/* track next blob */

        if(m_Wnd)
        {
            IplImage* pI = cvCloneImage(pImg);
            int i;
            for(i=m_BlobListNew.GetBlobNum();i>0;--i)
            {/* draw each new CC */
                CvBlob* pB = m_BlobListNew.GetBlob(i-1);
                CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
                int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
                CvSize  s = cvSize(MAX(1,x), MAX(1,y));
                //int c = 255;
                cvEllipse( pI,
                    p,
                    s,
                    0, 0, 360,
                    CV_RGB(255,255,0), 1 );
            }

            for(i=m_BlobList.GetBlobNum();i>0;--i)
            {/* draw each new CC */
                DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
                CvBlob* pB = &(pF->BlobPredict);
                CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
                int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
                CvSize  s = cvSize(MAX(1,x), MAX(1,y));
                cvEllipse( pI,
                    p,
                    s,
                    0, 0, 360,
                    CV_RGB(0,0,255), 1 );
                
                pB = &(pF->blob);
                p = cvPointFrom32f(CV_BLOB_CENTER(pB));
                x = cvRound(CV_BLOB_RX(pB)); y = cvRound(CV_BLOB_RY(pB));
                s = cvSize(MAX(1,x), MAX(1,y));
                cvEllipse( pI,
                    p,
                    s,
                    0, 0, 360,
                    CV_RGB(0,255,0), 1 );
            }

            //cvNamedWindow("CCwithCR",0);
            //cvShowImage("CCwithCR",pI);
            cvReleaseImage(&pI);
        }
        
    }/* Process */
    virtual void SaveState(CvFileStorage* fs)
    {
        int     b,bN = m_BlobList.GetBlobNum();
        cvWriteInt(fs,"BlobNum",m_BlobList.GetBlobNum());
        cvStartWriteStruct(fs,"BlobList",CV_NODE_SEQ);
        for(b=0;b<bN;++b)
        {
            DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(b);
            cvStartWriteStruct(fs,NULL,CV_NODE_MAP);
            cvWriteInt(fs,"ID",CV_BLOB_ID(pF));
            cvStartWriteStruct(fs,"Blob",CV_NODE_SEQ|CV_NODE_FLOW);
            cvWriteRawData(fs,&(pF->blob),1,"ffffi");
            cvEndWriteStruct(fs);
            cvStartWriteStruct(fs,"BlobPredict",CV_NODE_SEQ|CV_NODE_FLOW);
            cvWriteRawData(fs,&(pF->BlobPredict),1,"ffffi");
            cvEndWriteStruct(fs);
            cvStartWriteStruct(fs,"BlobPrev",CV_NODE_SEQ|CV_NODE_FLOW);
            cvWriteRawData(fs,&(pF->BlobPrev),1,"ffffi");
            cvEndWriteStruct(fs);
            pF->pBlobHyp->Write(fs,"BlobHyp");
            cvWriteInt(fs,"Collision",pF->Collision);
            
            cvStartWriteStruct(fs,"Predictor",CV_NODE_MAP);
            pF->pPredictor->SaveState(fs);
            cvEndWriteStruct(fs);
            
            cvStartWriteStruct(fs,"Resolver",CV_NODE_MAP);
            pF->pResolver->SaveState(fs);
            cvEndWriteStruct(fs);
            cvEndWriteStruct(fs);
        }
        cvEndWriteStruct(fs);
    }/* SaveState*/
    
    virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
    {
        int         b,bN = cvReadIntByName(fs,node,"BlobNum",0);
        CvFileNode* pBlobListNode = cvGetFileNodeByName(fs,node,"BlobList");
        if(!CV_NODE_IS_SEQ(pBlobListNode->tag)) return;
        bN = pBlobListNode->data.seq->total;
        for(b=0;b<bN;++b)
        {
            DefBlobTrackerCR*   pF = NULL;
            CvBlob              Blob;
            CvFileNode*         pSeqNode = NULL;
            CvFileNode*         pBlobNode = (CvFileNode*)cvGetSeqElem(pBlobListNode->data.seq,b);
            assert(pBlobNode);

            Blob.ID = cvReadIntByName(fs,pBlobNode,"ID",0);

            pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Blob");
            if(CV_NODE_IS_SEQ(pSeqNode->tag))
                cvReadRawData( fs, pSeqNode, &Blob, "ffffi" );

            AddBlob(&Blob,NULL,NULL);
            pF = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(Blob.ID);

            pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPredict");
            if(CV_NODE_IS_SEQ(pSeqNode->tag))
                cvReadRawData( fs, pSeqNode, &pF->BlobPredict, "ffffi" );
            pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPrev");
            if(CV_NODE_IS_SEQ(pSeqNode->tag))
                cvReadRawData( fs, pSeqNode, &pF->BlobPrev, "ffffi" );
            pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobHyp");
            if(pSeqNode)
                pF->pBlobHyp->Load(fs,pSeqNode);
            pF->Collision = cvReadIntByName(fs, pBlobNode,"Collision",pF->Collision);

            pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Predictor");
            if(pSeqNode)
                pF->pPredictor->LoadState(fs,pSeqNode);
            pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Resolver");
            if(pSeqNode)
                pF->pResolver->LoadState(fs,pSeqNode);
        }/* read next blob */
    }/* CCwithCR LoadState */

    //void SetCollision(int Collision){m_Collision = Collision;};
};

CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF();
CvBlobTracker* cvCreateBlobTrackerCCMSPF()
{
    return (CvBlobTracker*) new CvBlobTrackerCCCR(cvCreateBlobTrackerOneMSPF,"MSPF");
}
/*============== BLOB TRACKERCC CLASS DECLARATION =============== */