blobtrackingccwithcr.cpp

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

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cvaux.h"

/*============== BLOB TRACKERCC CLASS DECLARATION =============== */
typedef struct DefBlobTrackerCR
{
    CvBlob                      blob;
    CvBlobTrackPredictor*       pPredictor;
    CvBlob                      BlobPredict;
    CvBlob                      BlobPrev;
    int                         Collision;
    CvBlobSeq*                  pBlobHyp;
    CvBlobTrackerOne*           pResolver;
}DefBlobTrackerCR;
void cvFindBlobsByCCClasters(IplImage* pFG, CvBlobSeq* pBlobs, CvMemStorage* storage);
class CvBlobTrackerCCCR : public CvBlobTracker
{
private:
    float           m_AlphaSize;
    int             m_Collision;
    CvBlobSeq       m_BlobList;
    CvBlobSeq       m_BlobListNew;
    CvMemStorage*   m_pMem;
    CvBlobTrackerOne* (*m_CreateCR)();
    char            m_ModuleName[1024];


public:
    CvBlobTrackerCCCR(CvBlobTrackerOne* (*CreateCR)(), char* CRName):m_BlobList(sizeof(DefBlobTrackerCR))
    {
        m_CreateCR = CreateCR;
        m_pMem = cvCreateMemStorage();

        m_Collision = 1; /* if 1 then collistion will be detected and processed */

        m_AlphaSize = 0.05f;
        AddParam("AlphaSize",&m_AlphaSize);
        CommentParam("AlphaSize", "Size update speed (0..1)");

        strcpy(m_ModuleName, "CC");
        if(CRName)strcat(m_ModuleName,CRName);
        SetModuleName(m_ModuleName);

        {
            CvBlobTrackerOne* pM = m_CreateCR();
            TransferParamsFromChild(pM,NULL);
            pM->Release();
        }
        SetParam("SizeVar",0);
    };
    ~CvBlobTrackerCCCR()
    {
        if(m_pMem)cvReleaseMemStorage(&m_pMem);
    };

    /* blob functions*/
    virtual int     GetBlobNum() {return m_BlobList.GetBlobNum();};
    virtual CvBlob* GetBlob(int BlobIndex){return m_BlobList.GetBlob(BlobIndex);};
    virtual void    SetBlob(int BlobIndex, CvBlob* pBlob)
    {
        CvBlob* pB = m_BlobList.GetBlob(BlobIndex);
        if(pB) pB[0] = pBlob[0];
    };
    virtual CvBlob* GetBlobByID(int BlobID){return m_BlobList.GetBlobByID(BlobID);};
    virtual void    DelBlob(int BlobIndex)
    {
        DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(BlobIndex);
        if(pBT->pResolver)pBT->pResolver->Release();
        if(pBT->pPredictor)pBT->pPredictor->Release();
        delete pBT->pBlobHyp;
        m_BlobList.DelBlob(BlobIndex);
    };
    virtual void    DelBlobByID(int BlobID)
    {
        DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(BlobID);
        if(pBT->pResolver)pBT->pResolver->Release();
        if(pBT->pPredictor)pBT->pPredictor->Release();
        delete pBT->pBlobHyp;
        m_BlobList.DelBlobByID(BlobID);
    };
    virtual void    Release(){delete this;};

    /* Add new blob to track it and assign to this blob personal ID */
    /* pBlob - pinter to structure with blob parameters (ID is ignored)*/
    /* pImg - current image */
    /* pImgFG - current foreground mask */
    /* return pointer to new added blob */
    virtual CvBlob* AddBlob(CvBlob* pB, IplImage* pImg, IplImage* pImgFG = NULL )
    {
        DefBlobTrackerCR NewB;
        NewB.blob = pB[0];
        NewB.pBlobHyp = new CvBlobSeq;
        NewB.pPredictor = cvCreateModuleBlobTrackPredictKalman(); /* module for predict position */
        NewB.pPredictor->SetParam("DataNoisePos",0.001); 
        NewB.pPredictor->ParamUpdate(); 
        NewB.pResolver = NULL;
        if(m_CreateCR)
        {
            NewB.pResolver = m_CreateCR();
            TransferParamsToChild(NewB.pResolver,NULL);
            NewB.pResolver->Init(pB, pImg, pImgFG);
        }
        m_BlobList.AddBlob((CvBlob*)&NewB);
        return m_BlobList.GetBlob(m_BlobList.GetBlobNum()-1);
    };
    virtual void    Process(IplImage* pImg, IplImage* pImgFG = NULL)
    {
        CvSeq*      cnts;
        CvSeq*      cnt;
        int i;
        //CvMat*      pMC = NULL;

        if(m_BlobList.GetBlobNum() <= 0 ) return;
        
        /* clear blob list for new blobs */
        m_BlobListNew.Clear();

        assert(m_pMem);
        cvClearMemStorage(m_pMem);
        assert(pImgFG);

        {/* one contour - one blob */
            IplImage* pBin = cvCloneImage(pImgFG);
            assert(pBin);
            cvThreshold(pBin,pBin,128,255,CV_THRESH_BINARY);
            cvFindContours(pBin, m_pMem, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);
            /* process each contours*/
            for(cnt = cnts;cnt;cnt=cnt->h_next)
            {
                CvBlob  NewBlob;
                /* image moments */
                double      M00,X,Y,XX,YY;
                CvMoments   m;
                CvRect      r = ((CvContour*)cnt)->rect;
                CvMat       mat;
                if(r.height < 3 || r.width < 3) continue;
                cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
                M00 = cvGetSpatialMoment( &m, 0, 0 );
                if(M00 <= 0 ) continue;
                X = cvGetSpatialMoment( &m, 1, 0 )/M00;
                Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
                XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
                YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
                NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
                m_BlobListNew.AddBlob(&NewBlob);
            }/* next contour */
            cvReleaseImage(&pBin);
        }

        for(i=m_BlobList.GetBlobNum();i>0;--i)
        {/* predict new blob position */
            CvBlob*             pB = NULL;
            DefBlobTrackerCR*   pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
            /*update predictor */
            pBT->pPredictor->Update(&(pBT->blob));
            pB = pBT->pPredictor->Predict();
            if(pB)
            {
                pBT->BlobPredict = pB[0];
            }
            pBT->BlobPrev = pBT->blob;
        }/* predict new blob position */


        if(m_BlobList.GetBlobNum()>0 && m_BlobListNew.GetBlobNum()>0)
        {/* resolve new blob to old */
            int i,j;
            int NOld = m_BlobList.GetBlobNum();
            int NNew = m_BlobListNew.GetBlobNum();
            
            for(i=0;i<NOld;i++)
            {/* set 0 collision and clear all hyp */
                DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
                pF->Collision = 0;
                pF->pBlobHyp->Clear();
            }/* set 0 collision */

            /* create correspondance records */
            for(j=0;j<NNew;++j)
            {
                CvBlob*             pB1 = m_BlobListNew.GetBlob(j);
                DefBlobTrackerCR*   pFLast = NULL;
                
                for(i=0;i<NOld;i++)
                {/* check intersection */
                    int Intersection = 0;
                    DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);