blobtrackanalysistrackdist.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
//                For Open Source Computer Vision Library
//
// 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"

typedef struct DefTrackPoint
{
    float x,y,r,vx,vy,v;
} DefTrackPoint;

class DefTrackRec
{
private:
    int     ID;
public:
    DefTrackRec(int id = 0,int BlobSize = sizeof(DefTrackPoint))
    {
        ID = id;
        m_pMem = cvCreateMemStorage();
        m_pSeq = cvCreateSeq(0,sizeof(CvSeq),BlobSize,m_pMem);
    }
    ~DefTrackRec()
    { 
        cvReleaseMemStorage(&m_pMem);
    };
    inline DefTrackPoint* GetPoint(int PointIndex)
    {
        return (DefTrackPoint*)cvGetSeqElem(m_pSeq,PointIndex);
    };
    inline void DelPoint(int PointIndex)
    {
        cvSeqRemove(m_pSeq,PointIndex);
    };
    inline void Clear()
    {
        cvClearSeq(m_pSeq);
    };
    inline void AddPoint(float x, float y, float r)
    {
        DefTrackPoint   p = {x,y,r,0};
        int             Num = GetPointNum();
        if(Num > 0)
        {
            DefTrackPoint* pPrev = GetPoint(Num-1);
            float   Alpha = 0.8f;
            float  dx = x-pPrev->x;
            float  dy = y-pPrev->y;
            p.vx = Alpha*dx+(1-Alpha)*pPrev->vx;
            p.vy = Alpha*dy+(1-Alpha)*pPrev->vy;
            p.v = Alpha*dx+(1-Alpha)*pPrev->v;
        }
        AddPoint(&p);
    }
    inline void AddPoint(DefTrackPoint* pB)
    {/* add point and recal last velocities */
        int     wnd=3;
        int     Num;
        int     i;
        cvSeqPush(m_pSeq,pB);
        
        Num = GetPointNum();
        for(i=MAX(0,Num-wnd-1);i<Num;++i)
        {/* next updating point */
            DefTrackPoint*  p = GetPoint(i);
            int             j0 = i - wnd;
            int             j1 = i + wnd;

            if(j0<0) j0 = 0;
            if(j1>=Num)j1=Num-1;

            if(j1>j0)
            {
                float           dt = (float)(j1-j0);
                DefTrackPoint*  p0 = GetPoint(j0);
                DefTrackPoint*  p1 = GetPoint(j1);
                p->vx = (p1->x - p0->x) / dt;
                p->vy = (p1->y - p0->y) / dt;
                p->v = (float)sqrt(p->vx*p->vx+p->vy*p->vy);
            }
        }/* next updating point */

#if 0
        if(0)
        { /* debug */
            int i;
            printf("Blob %d: ",ID);
            for(i=0;i<GetPointNum();++i)
            {
                DefTrackPoint*  p = GetPoint(i);
                printf(",(%.2f,%.2f,%f.2)",p->vx,p->vy,p->v);
            }
            printf("\n");
        }
#endif
    };
    inline int GetPointNum()
    {
        return m_pSeq->total;
    };
private:
    CvMemStorage*   m_pMem;
    CvSeq*          m_pSeq;
};

/* fill array pIdxPairs by pair of index of correspondent blobs */
/* return number of pairs                                       */
/* pIdxPairs must have size not less that 2*(pSeqNum+pSeqTNum)  */
/* pTmp is pointer to memory which size is pSeqNum*pSeqTNum*16  */
typedef struct DefMatch
{
    int     Idx;  /* prev best blob index */
    int     IdxT; /* prev best template blob index */
    double  D; /* blob to blob distance sum */
}DefMatch;
static int cvTrackMatch(DefTrackRec* pSeq, int MaxLen, DefTrackRec* pSeqT, int* pIdxPairs, void* pTmp)
{
    int         NumPair = 0;
    DefMatch*   pMT = (DefMatch*)pTmp;
    int         Num = pSeq->GetPointNum();
    int         NumT = pSeqT->GetPointNum();
    int         i,it;
    int         i0=0; /* last point in the track sequence */

    if(MaxLen > 0 && Num > MaxLen)
    {/* set new point seq len and new last point in this seq */
        Num = MaxLen;
        i0 = pSeq->GetPointNum() - Num; 
    }

    for(i=0;i<Num;++i)
    { /* for eacj point row */
        for(it=0;it<NumT;++it)
        { /* for each point templet column */
            DefTrackPoint*  pB = pSeq->GetPoint(i+i0);
            DefTrackPoint*  pBT = pSeqT->GetPoint(it);
            DefMatch*       pMT_cur = pMT + i*NumT + it;
            double          dx = pB->x-pBT->x;
            double          dy = pB->y-pBT->y;
            double          D = dx*dx+dy*dy;
            int             DI[3][2] = {{-1,-1},{-1,0},{0,-1}};
            int             iDI;

            pMT_cur->D = D;
            pMT_cur->Idx = -1;
            pMT_cur->IdxT = 0;

            if(i==0) continue;

            for(iDI=0;iDI<3;++iDI)
            {
                int         i_prev = i+DI[iDI][0];
                int         it_prev = it+DI[iDI][1];
                if(i_prev >= 0 && it_prev>=0)
                {
                    double D_cur = D+pMT[NumT*i_prev+it_prev].D;
                    if(pMT_cur->D > D_cur || (pMT_cur->Idx<0) )
                    {/* set new best local way */
                        pMT_cur->D = D_cur;
                        pMT_cur->Idx = i_prev;
                        pMT_cur->IdxT = it_prev;
                    }
                }
            }/* check next direction */
        }/* fill next colum from table */
    }/*fill next row */

    {   /* back tracking */
        /* find best end in template */
        int         it_best = 0;
        DefMatch*   pMT_best = pMT + (Num-1)*NumT;
        i = Num-1; /* set current i to last position */
        for(it=1;it<NumT;++it)
        {
            DefMatch* pMT_new = pMT + it + i*NumT;
            if(pMT_best->D > pMT_new->D)
            {
                pMT_best->D = pMT_new->D;
                it_best = it;
            }
        }/* find best end template point */
        
        /* back tracking whole sequence */
        for(it = it_best;i>=0 && it>=0;)
        {
            DefMatch* pMT_new = pMT + it + i*NumT;
            pIdxPairs[2*NumPair] = i+i0;
            pIdxPairs[2*NumPair+1] = it;
            NumPair++;

            it = pMT_new->IdxT;
            i = pMT_new->Idx;
        }
    }/* end back tracing */

    return NumPair;
}/* cvTrackMatch */

typedef struct DefTrackForDist
{
    CvBlob                  blob;
    DefTrackRec*            pTrack;
    int                     LastFrame;
    float                   state;
    /* for debug */
    int                     close;
} DefTrackForDist;

class CvBlobTrackAnalysisTrackDist : public CvBlobTrackAnalysis
{
    /*---------------- internal functions --------------------*/
private:
    char*               m_pDebugAVIName; /* for debuf purpose */
    //CvVideoWriter*      m_pDebugAVI; /* for debuf purpose */
    IplImage*           m_pDebugImg; /* for debuf purpose */

    char                m_DataFileName[1024];
    CvBlobSeq           m_Tracks;
    CvBlobSeq           m_TrackDataBase;
    int                 m_Frame;
    void*               m_pTempData;
    int                 m_TempDataSize;
    int                 m_TraceLen;
    float               m_AbnormalThreshold;
    float               m_PosThreshold;
    float               m_VelThreshold;
    inline void* ReallocTempData(int Size)
    {
        if(Size <= m_TempDataSize && m_pTempData) return m_pTempData;
        cvFree(&m_pTempData);
        m_TempDataSize = 0;
        m_pTempData = cvAlloc(Size);
        if(m_pTempData) m_TempDataSize = Size;
        return m_pTempData;
    }/* ReallocTempData */
public:
    CvBlobTrackAnalysisTrackDist():m_Tracks(sizeof(DefTrackForDist)),m_TrackDataBase(sizeof(DefTrackForDist))
    {
        m_pDebugImg = 0;
        //m_pDebugAVI = 0;
        m_Frame = 0;
        m_pTempData = NULL;
        m_TempDataSize = 0;