void CvBlobTrackPredictKalman::Update(CvBlob* pBlob)
{
    float   Z[4];
    CvMat   Zmat = cvMat(4,1,CV_32F,Z);
    m_BlobPredict = pBlob[0];

    if(m_Frame < 2)
    {   /* First call: */
        m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
        m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
        if(m_pKalman->DP>6)
        {
            m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
            m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
        }
        m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
        m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
        m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
        m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
    }
    else
    {   /* Nonfirst call: */
        Z[0] = CV_BLOB_X(pBlob);
        Z[1] = CV_BLOB_Y(pBlob);
        Z[2] = CV_BLOB_WX(pBlob);
        Z[3] = CV_BLOB_WY(pBlob);
        cvKalmanCorrect(m_pKalman,&Zmat);
    }

    cvKalmanPredict(m_pKalman,0);

    m_Frame++;

}   /* Update. */
static void SaveTrack(DefBlobTrack* pTrack, char* pFileName, int norm = 0)
{   /* Save blob track: */
    int         j;
    FILE*       out = NULL;
    CvBlobSeq*  pS = pTrack->pSeq;
    CvBlob*     pB0 = pS?pS->GetBlob(0):NULL;

    if(pFileName == NULL) return;
    if(pTrack == NULL) return;

    out = fopen(pFileName,"at");
    if(out == NULL)
    {
        printf("Warning! Cannot open %s file for track output\n", pFileName);
        return;
    }

    fprintf(out,"%d",pTrack->FrameBegin);
    
    if(pS) for(j=0; j<pS->GetBlobNum(); ++j)
    {
        CvBlob* pB = pS->GetBlob(j);
        
        fprintf(out,", %.1f, %.1f", CV_BLOB_X(pB),CV_BLOB_Y(pB));

        if(CV_BLOB_WX(pB0)>0)
            fprintf(out,", %.2f",CV_BLOB_WX(pB)/(norm?CV_BLOB_WX(pB0):1));

        if(CV_BLOB_WY(pB0)>0)
            fprintf(out,", %.2f",CV_BLOB_WY(pB)/(norm?CV_BLOB_WY(pB0):1));
    }
    fprintf(out,"\n");
    fclose(out);
    pTrack->Saved = 1;
}   /* Save blob track. */
Exemplo n.º 3
0
    CvBlob* GetNearestBlob(CvBlob* pB)
    {
        //DefBlobTracker* pBT = (DefBlobTracker*)pB;
        CvBlob*         pBBest = NULL;
        double          DistBest = -1;

        if(pB==NULL) return NULL;

        for(int j=m_BlobListNew.GetBlobNum(); j>0; --j)
        {   /* Find best CC: */
            double  Dist = -1;
            CvBlob* pBNew = m_BlobListNew.GetBlob(j-1);
            double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
            double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
            if(dx > 2*CV_BLOB_WX(pB) || dy > 2*CV_BLOB_WY(pB)) continue;

            Dist = sqrt(dx*dx+dy*dy);
            if(Dist < DistBest || pBBest == NULL)
            {
                DistBest = Dist;
                pBBest = pBNew;
            }

        }   /* Find best CC. */

        return pBBest;

    }; /* GetNearestBlob */
 virtual void Init(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
 {
     m_Blob = pBlob[0];
     m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
     m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
     m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
     m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
 }
CvBlob* CvBlobTrackPostProcKalman::Process(CvBlob* pBlob)
{
    CvBlob* pBlobRes = &m_Blob;
    float   Z[4];
    CvMat   Zmat = cvMat(4,1,CV_32F,Z);
    m_Blob = pBlob[0];

    if(m_Frame < 2)
    {   /* First call: */
        m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
        m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
        if(m_pKalman->DP>6)
        {
            m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
            m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
        }
        m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
        m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
        m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
        m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
    }
    else
    {   /* Nonfirst call: */
        cvKalmanPredict(m_pKalman,0);
        Z[0] = CV_BLOB_X(pBlob);
        Z[1] = CV_BLOB_Y(pBlob);
        Z[2] = CV_BLOB_WX(pBlob);
        Z[3] = CV_BLOB_WY(pBlob);
        cvKalmanCorrect(m_pKalman,&Zmat);
        cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_post, NULL, &Zmat);
        CV_BLOB_X(pBlobRes) = Z[0];
        CV_BLOB_Y(pBlobRes) = Z[1];
//        CV_BLOB_WX(pBlobRes) = Z[2];
//        CV_BLOB_WY(pBlobRes) = Z[3];
    }
    m_Frame++;
    return pBlobRes;
}
 virtual CvBlob* Process(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
 {
     CvBlob* pBlobRes = &m_Blob;
     float   Z[4];
     CvMat   Zmat = cvMat(4,1,CV_32F,Z);
     m_Blob = pBlob[0];
     if(m_Frame < 2)
     {/* first call */
         m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
         m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
         if(m_pKalman->DP>6)
         {
             m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
             m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
         }
         m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
         m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
         m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
         m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
         memcpy(m_pKalman->state_pre->data.fl,m_pKalman->state_post->data.fl,sizeof(float)*STATE_NUM);
     }
     else
     {/* another call */
         Z[0] = CV_BLOB_X(pBlob);
         Z[1] = CV_BLOB_Y(pBlob);
         Z[2] = CV_BLOB_WX(pBlob);
         Z[3] = CV_BLOB_WY(pBlob);
         cvKalmanCorrect(m_pKalman,&Zmat);
         cvKalmanPredict(m_pKalman,0);
         cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_pre, NULL, &Zmat);
         CV_BLOB_X(pBlobRes) = Z[0];
         CV_BLOB_Y(pBlobRes) = Z[1];
         CV_BLOB_WX(pBlobRes) = Z[2];
         CV_BLOB_WY(pBlobRes) = Z[3];
     }
     m_Frame++;
     return pBlobRes;
 }
/* cvDetectNewBlobs
 * Return 1 and fill blob pNewBlob  with
 * blob parameters if new blob is detected:
 */
int CvBlobDetectorCC::DetectNewBlob(IplImage* /*pImg*/, IplImage* pFGMask, CvBlobSeq* pNewBlobList, CvBlobSeq* pOldBlobList)
{
    int         result = 0;
    CvSize      S = cvSize(pFGMask->width,pFGMask->height);

    /* Shift blob list: */
    {
        int     i;
        if(m_pBlobLists[SEQ_SIZE-1]) delete m_pBlobLists[SEQ_SIZE-1];

        for(i=SEQ_SIZE-1; i>0; --i)  m_pBlobLists[i] = m_pBlobLists[i-1];

        m_pBlobLists[0] = new CvBlobSeq;

    }   /* Shift blob list. */

    /* Create contours and add new blobs to blob list: */
    {   /* Create blobs: */
        CvBlobSeq       Blobs;
        CvMemStorage*   storage = cvCreateMemStorage();

        if(m_Clastering)
        {   /* Glue contours: */
            cvFindBlobsByCCClasters(pFGMask, &Blobs, storage );
        }   /* Glue contours. */
        else
        { /**/
            IplImage*       pIB = cvCloneImage(pFGMask);
            CvSeq*          cnts = NULL;
            CvSeq*          cnt = NULL;
            cvThreshold(pIB,pIB,128,255,CV_THRESH_BINARY);
            cvFindContours(pIB,storage, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);

            /* Process each contour: */
            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 < S.height*m_HMin || r.width < S.width*m_WMin) continue;
                cvMoments( cvGetSubRect(pFGMask,&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)));
                Blobs.AddBlob(&NewBlob);

            }   /* Next contour. */

            cvReleaseImage(&pIB);

        }   /* One contour - one blob. */

        {   /* Delete small and intersected blobs: */
            int i;
            for(i=Blobs.GetBlobNum(); i>0; i--)
            {
                CvBlob* pB = Blobs.GetBlob(i-1);

                if(pB->h < S.height*m_HMin || pB->w < S.width*m_WMin)
                {
                    Blobs.DelBlob(i-1);
                    continue;
                }

                if(pOldBlobList)
                {
                    int j;
                    for(j=pOldBlobList->GetBlobNum(); j>0; j--)
                    {
                        CvBlob* pBOld = pOldBlobList->GetBlob(j-1);
                        if((fabs(pBOld->x-pB->x) < (CV_BLOB_RX(pBOld)+CV_BLOB_RX(pB))) &&
                           (fabs(pBOld->y-pB->y) < (CV_BLOB_RY(pBOld)+CV_BLOB_RY(pB))))
                        {   /* Intersection detected, delete blob from list: */
                            Blobs.DelBlob(i-1);
                            break;
                        }
                    }   /* Check next old blob. */
                }   /*  if pOldBlobList. */
            }   /*  Check next blob. */
        }   /*  Delete small and intersected blobs. */

        {   /* Bubble-sort blobs by size: */
            int N = Blobs.GetBlobNum();
            int i,j;
            for(i=1; i<N; ++i)
            {
                for(j=i; j>0; --j)
                {
                    CvBlob  temp;
                    float   AreaP, AreaN;
                    CvBlob* pP = Blobs.GetBlob(j-1);
                    CvBlob* pN = Blobs.GetBlob(j);
                    AreaP = CV_BLOB_WX(pP)*CV_BLOB_WY(pP);
                    AreaN = CV_BLOB_WX(pN)*CV_BLOB_WY(pN);
                    if(AreaN < AreaP)break;
                    temp = pN[0];
                    pN[0] = pP[0];
                    pP[0] = temp;
                }
            }

            /* Copy only first 10 blobs: */
            for(i=0; i<MIN(N,10); ++i)
            {
                m_pBlobLists[0]->AddBlob(Blobs.GetBlob(i));
            }

        }   /* Sort blobs by size. */

        cvReleaseMemStorage(&storage);

    }   /* Create blobs. */

    {   /* Shift each track: */
        int j;
        for(j=0; j<m_TrackNum; ++j)
        {
            int     i;
            DefSeq* pTrack = m_TrackSeq+j;

            for(i=SEQ_SIZE-1; i>0; --i)
                pTrack->pBlobs[i] = pTrack->pBlobs[i-1];

            pTrack->pBlobs[0] = NULL;
            if(pTrack->size == SEQ_SIZE)pTrack->size--;
        }
    }   /* Shift each track. */

    /* Analyze blob list to find best blob trajectory: */
    {
        double      BestError = -1;
        int         BestTrack = -1;;
        CvBlobSeq*  pNewBlobs = m_pBlobLists[0];
        int         i;
        int         NewTrackNum = 0;
        for(i=pNewBlobs->GetBlobNum(); i>0; --i)
        {
            CvBlob* pBNew = pNewBlobs->GetBlob(i-1);
            int     j;
            int     AsignedTrack = 0;
            for(j=0; j<m_TrackNum; ++j)
            {
                double  dx,dy;
                DefSeq* pTrack = m_TrackSeq+j;
                CvBlob* pLastBlob = pTrack->size>0?pTrack->pBlobs[1]:NULL;
                if(pLastBlob == NULL) continue;
                dx = fabs(CV_BLOB_X(pLastBlob)-CV_BLOB_X(pBNew));
                dy = fabs(CV_BLOB_Y(pLastBlob)-CV_BLOB_Y(pBNew));
                if(dx > 2*CV_BLOB_WX(pLastBlob) || dy > 2*CV_BLOB_WY(pLastBlob)) continue;
                AsignedTrack++;

                if(pTrack->pBlobs[0]==NULL)
                {   /* Fill existed track: */
                    pTrack->pBlobs[0] = pBNew;
                    pTrack->size++;
                }
                else if((m_TrackNum+NewTrackNum)<SEQ_NUM)
                {   /* Duplicate existed track: */
                    m_TrackSeq[m_TrackNum+NewTrackNum] = pTrack[0];
                    m_TrackSeq[m_TrackNum+NewTrackNum].pBlobs[0] = pBNew;
                    NewTrackNum++;
                }
            }   /* Next track. */

            if(AsignedTrack==0 && (m_TrackNum+NewTrackNum)<SEQ_NUM )
            {   /* Initialize new track: */
                m_TrackSeq[m_TrackNum+NewTrackNum].size = 1;
                m_TrackSeq[m_TrackNum+NewTrackNum].pBlobs[0] = pBNew;
                NewTrackNum++;
            }
        }   /* Next new blob. */

        m_TrackNum += NewTrackNum;

        /* Check each track: */
        for(i=0; i<m_TrackNum; ++i)
        {
            int     Good = 1;
            DefSeq* pTrack = m_TrackSeq+i;
            CvBlob* pBNew = pTrack->pBlobs[0];
            if(pTrack->size != SEQ_SIZE) continue;
            if(pBNew == NULL ) continue;

            /* Check intersection last blob with existed: */
            if(Good && pOldBlobList)
            {
                int k;
                for(k=pOldBlobList->GetBlobNum(); k>0; --k)
                {
                    CvBlob* pBOld = pOldBlobList->GetBlob(k-1);
                    if((fabs(pBOld->x-pBNew->x) < (CV_BLOB_RX(pBOld)+CV_BLOB_RX(pBNew))) &&
                       (fabs(pBOld->y-pBNew->y) < (CV_BLOB_RY(pBOld)+CV_BLOB_RY(pBNew))))
                        Good = 0;
                }
            }   /* Check intersection last blob with existed. */

            /* Check distance to image border: */
            if(Good)
            {   /* Check distance to image border: */
                float    dx = MIN(pBNew->x,S.width-pBNew->x)/CV_BLOB_RX(pBNew);
                float    dy = MIN(pBNew->y,S.height-pBNew->y)/CV_BLOB_RY(pBNew);
                if(dx < m_MinDistToBorder || dy < m_MinDistToBorder) Good = 0;
            }   /* Check distance to image border. */

            /* Check uniform motion: */
            if(Good)
            {   /* Check uniform motion: */
                double      Error = 0;
                int         N = pTrack->size;
                CvBlob**    pBL = pTrack->pBlobs;
                float       sum[2] = {0,0};
                float       jsum[2] = {0,0};
                float       a[2],b[2]; /* estimated parameters of moving x(t) = a*t+b*/
                int         j;

                for(j=0; j<N; ++j)
                {
                    float   x = pBL[j]->x;
                    float   y = pBL[j]->y;
                    sum[0] += x;
                    jsum[0] += j*x;
                    sum[1] += y;
                    jsum[1] += j*y;
                }

                a[0] = 6*((1-N)*sum[0]+2*jsum[0])/(N*(N*N-1));
                b[0] = -2*((1-2*N)*sum[0]+3*jsum[0])/(N*(N+1));
                a[1] = 6*((1-N)*sum[1]+2*jsum[1])/(N*(N*N-1));
                b[1] = -2*((1-2*N)*sum[1]+3*jsum[1])/(N*(N+1));

                for(j=0; j<N; ++j)
                {
                    Error +=
                        pow(a[0]*j+b[0]-pBL[j]->x,2)+
                        pow(a[1]*j+b[1]-pBL[j]->y,2);
                }

                Error = sqrt(Error/N);

                if( Error > S.width*0.01 ||
                    fabs(a[0])>S.width*0.1 ||
                    fabs(a[1])>S.height*0.1)
                    Good = 0;

                /* New best trajectory: */
                if(Good && (BestError == -1 || BestError > Error))
                {   /* New best trajectory: */
                    BestTrack = i;
                    BestError = Error;
                }   /* New best trajectory. */
            }   /*  Check uniform motion. */
        }   /*  Next track. */

        #if 0
        {   /**/
            printf("BlobDetector configurations = %d [",m_TrackNum);
            int i;
            for(i=0; i<SEQ_SIZE; ++i)
            {
                printf("%d,",m_pBlobLists[i]?m_pBlobLists[i]->GetBlobNum():0);
            }
            printf("]\n");
        }
        #endif

        if(BestTrack >= 0)
        {   /* Put new blob to output and delete from blob list: */
            assert(m_TrackSeq[BestTrack].size == SEQ_SIZE);
            assert(m_TrackSeq[BestTrack].pBlobs[0]);
            pNewBlobList->AddBlob(m_TrackSeq[BestTrack].pBlobs[0]);
            m_TrackSeq[BestTrack].pBlobs[0] = NULL;
            m_TrackSeq[BestTrack].size--;
            result = 1;
        }   /* Put new blob to output and mark in blob list to delete. */
    }   /*  Analyze blod list to find best blob trajectory. */

    {   /* Delete bad tracks: */
        int i;
        for(i=m_TrackNum-1; i>=0; --i)
        {   /* Delete bad tracks: */
            if(m_TrackSeq[i].pBlobs[0]) continue;
            if(m_TrackNum>0)
                m_TrackSeq[i] = m_TrackSeq[--m_TrackNum];
        }   /* Delete bad tracks: */
    }

#ifdef USE_OBJECT_DETECTOR
    if( m_split_detector && pNewBlobList->GetBlobNum() > 0 )
    {
        int num_new_blobs = pNewBlobList->GetBlobNum();
        int i = 0;

        if( m_roi_seq ) cvClearSeq( m_roi_seq );
        m_debug_blob_seq.Clear();
        for( i = 0; i < num_new_blobs; ++i )
        {
            CvBlob* b = pNewBlobList->GetBlob(i);
            CvMat roi_stub;
            CvMat* roi_mat = 0;
            CvMat* scaled_roi_mat = 0;

            CvDetectedBlob d_b = cvDetectedBlob( CV_BLOB_X(b), CV_BLOB_Y(b), CV_BLOB_WX(b), CV_BLOB_WY(b), 0 );
            m_debug_blob_seq.AddBlob(&d_b);

            float scale = m_param_roi_scale * m_min_window_size.height / CV_BLOB_WY(b);

            float b_width =   MAX(CV_BLOB_WX(b), m_min_window_size.width / scale)
                            + (m_param_roi_scale - 1.0F) * (m_min_window_size.width / scale)
                            + 2.0F * m_max_border / scale;
            float b_height = CV_BLOB_WY(b) * m_param_roi_scale + 2.0F * m_max_border / scale;

            CvRect roi = cvRectIntersection( cvRect( cvFloor(CV_BLOB_X(b) - 0.5F*b_width),
                                                     cvFloor(CV_BLOB_Y(b) - 0.5F*b_height),
                                                     cvCeil(b_width), cvCeil(b_height) ),
                                             cvRect( 0, 0, pImg->width, pImg->height ) );
            if( roi.width <= 0 || roi.height <= 0 )
                continue;

            if( m_roi_seq ) cvSeqPush( m_roi_seq, &roi );

            roi_mat = cvGetSubRect( pImg, &roi_stub, roi );
            scaled_roi_mat = cvCreateMat( cvCeil(scale*roi.height), cvCeil(scale*roi.width), CV_8UC3 );
            cvResize( roi_mat, scaled_roi_mat );

            m_detected_blob_seq.Clear();
            m_split_detector->Detect( scaled_roi_mat, &m_detected_blob_seq );
            cvReleaseMat( &scaled_roi_mat );

            for( int k = 0; k < m_detected_blob_seq.GetBlobNum(); ++k )
            {
                CvDetectedBlob* b = (CvDetectedBlob*) m_detected_blob_seq.GetBlob(k);

                /* scale and shift each detected blob back to the original image coordinates */
                CV_BLOB_X(b) = CV_BLOB_X(b) / scale + roi.x;
                CV_BLOB_Y(b) = CV_BLOB_Y(b) / scale + roi.y;
                CV_BLOB_WX(b) /= scale;
                CV_BLOB_WY(b) /= scale;

                CvDetectedBlob d_b = cvDetectedBlob( CV_BLOB_X(b), CV_BLOB_Y(b), CV_BLOB_WX(b), CV_BLOB_WY(b), 1,
                        b->response );
                m_debug_blob_seq.AddBlob(&d_b);
            }

            if( m_detected_blob_seq.GetBlobNum() > 1 )
            {
                /*
                 * Split blob.
                 * The original blob is replaced by the first detected blob,
                 * remaining detected blobs are added to the end of the sequence:
                 */
                CvBlob* first_b = m_detected_blob_seq.GetBlob(0);
                CV_BLOB_X(b)  = CV_BLOB_X(first_b);  CV_BLOB_Y(b)  = CV_BLOB_Y(first_b);
                CV_BLOB_WX(b) = CV_BLOB_WX(first_b); CV_BLOB_WY(b) = CV_BLOB_WY(first_b);

                for( int j = 1; j < m_detected_blob_seq.GetBlobNum(); ++j )
                {
                    CvBlob* detected_b = m_detected_blob_seq.GetBlob(j);
                    pNewBlobList->AddBlob(detected_b);
                }
            }
        }   /* For each new blob. */

        for( i = 0; i < pNewBlobList->GetBlobNum(); ++i )
        {
            CvBlob* b = pNewBlobList->GetBlob(i);
            CvDetectedBlob d_b = cvDetectedBlob( CV_BLOB_X(b), CV_BLOB_Y(b), CV_BLOB_WX(b), CV_BLOB_WY(b), 2 );
            m_debug_blob_seq.AddBlob(&d_b);
        }
    }   // if( m_split_detector )
#endif

    return result;

}   /* cvDetectNewBlob */
/* cvDetectNewBlobs
 * return 1 and fill blob pNewBlob by blob parameters
 * if new blob is detected:
 */
int CvBlobDetectorSimple::DetectNewBlob(IplImage* /*pImg*/, IplImage* pFGMask, CvBlobSeq* pNewBlobList, CvBlobSeq* pOldBlobList)
{
    int         result = 0;
    CvSize      S = cvSize(pFGMask->width,pFGMask->height);
    if(m_pMaskBlobNew == NULL ) m_pMaskBlobNew = cvCreateImage(S,IPL_DEPTH_8U,1);
    if(m_pMaskBlobExist == NULL ) m_pMaskBlobExist = cvCreateImage(S,IPL_DEPTH_8U,1);

    /* Shift blob list: */
    {
        int     i;
        if(m_pBlobLists[0]) delete m_pBlobLists[0];
        for(i=1;i<EBD_FRAME_NUM;++i)m_pBlobLists[i-1]=m_pBlobLists[i];
        m_pBlobLists[EBD_FRAME_NUM-1] = new CvBlobSeq;
    }   /* Shift blob list. */

    /* Create exist blob mask: */
    cvCopy(pFGMask, m_pMaskBlobNew);

    /* Create contours and add new blobs to blob list: */
    {   /* Create blobs: */
        CvBlobSeq       Blobs;
        CvMemStorage*   storage = cvCreateMemStorage();

#if 1
        {   /* Glue contours: */
            cvFindBlobsByCCClasters(m_pMaskBlobNew, &Blobs, storage );
        }   /* Glue contours. */
#else
        {   /**/
            IplImage*       pIB = cvCloneImage(m_pMaskBlobNew);
            CvSeq*          cnts = NULL;
            CvSeq*          cnt = NULL;
            cvThreshold(pIB,pIB,128,255,CV_THRESH_BINARY);
            cvFindContours(pIB,storage, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);

            /* Process each contour: */
            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 < S.height*0.02 || r.width < S.width*0.02) continue;

                cvMoments( cvGetSubRect(m_pMaskBlobNew,&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)));

                Blobs.AddBlob(&NewBlob);

            }   /* Next contour. */

            cvReleaseImage(&pIB);

        }   /* One contour - one blob. */
#endif

        {   /* Delete small and intersected blobs: */
            int i;
            for(i=Blobs.GetBlobNum(); i>0; i--)
            {
                CvBlob* pB = Blobs.GetBlob(i-1);

                if(pB->h < S.height*0.02 || pB->w < S.width*0.02)
                {
                    Blobs.DelBlob(i-1);
                    continue;
                }
                if(pOldBlobList)
                {
                    int j;
                    for(j=pOldBlobList->GetBlobNum(); j>0; j--)
                    {
                        CvBlob* pBOld = pOldBlobList->GetBlob(j-1);
                        if((fabs(pBOld->x-pB->x) < (CV_BLOB_RX(pBOld)+CV_BLOB_RX(pB))) &&
                           (fabs(pBOld->y-pB->y) < (CV_BLOB_RY(pBOld)+CV_BLOB_RY(pB))))
                        {   /* Intersection is present, so delete blob from list: */
                            Blobs.DelBlob(i-1);
                            break;
                        }
                    }   /* Check next old blob. */
                }   /*  if pOldBlobList */
            }   /* Check next blob. */
        }   /*  Delete small and intersected blobs. */

        {   /* Bubble-sort blobs by size: */
            int N = Blobs.GetBlobNum();
            int i,j;
            for(i=1; i<N; ++i)
            {
                for(j=i; j>0; --j)
                {
                    CvBlob  temp;
                    float   AreaP, AreaN;
                    CvBlob* pP = Blobs.GetBlob(j-1);
                    CvBlob* pN = Blobs.GetBlob(j);
                    AreaP = CV_BLOB_WX(pP)*CV_BLOB_WY(pP);
                    AreaN = CV_BLOB_WX(pN)*CV_BLOB_WY(pN);
                    if(AreaN < AreaP)break;
                    temp = pN[0];
                    pN[0] = pP[0];
                    pP[0] = temp;
                }
            }

            /* Copy only first 10 blobs: */
            for(i=0; i<MIN(N,10); ++i)
            {
                m_pBlobLists[EBD_FRAME_NUM-1]->AddBlob(Blobs.GetBlob(i));
            }

        }   /* Sort blobs by size. */

        cvReleaseMemStorage(&storage);

    }   /* Create blobs. */

    /* Analyze blob list to find best blob trajectory: */
    {
        int     Count = 0;
        int     pBLIndex[EBD_FRAME_NUM];
        int     pBL_BEST[EBD_FRAME_NUM];
        int     i;
        int     finish = 0;
        double  BestError = -1;
        int     Good = 1;

        for(i=0; i<EBD_FRAME_NUM; ++i)
        {
            pBLIndex[i] = 0;
            pBL_BEST[i] = 0;
        }

        /* Check configuration exist: */
        for(i=0; Good && (i<EBD_FRAME_NUM); ++i)
            if(m_pBlobLists[i] == NULL || m_pBlobLists[i]->GetBlobNum() == 0)
                Good = 0;

        if(Good)
        do{ /* For each configuration: */
            CvBlob* pBL[EBD_FRAME_NUM];
            int     Good = 1;
            double  Error = 0;
            CvBlob* pBNew = m_pBlobLists[EBD_FRAME_NUM-1]->GetBlob(pBLIndex[EBD_FRAME_NUM-1]);

            for(i=0; i<EBD_FRAME_NUM; ++i)  pBL[i] = m_pBlobLists[i]->GetBlob(pBLIndex[i]);

            Count++;

            /* Check intersection last blob with existed: */
            if(Good && pOldBlobList)
            {   /* Check intersection last blob with existed: */
                int     k;
                for(k=pOldBlobList->GetBlobNum(); k>0; --k)
                {
                    CvBlob* pBOld = pOldBlobList->GetBlob(k-1);
                    if((fabs(pBOld->x-pBNew->x) < (CV_BLOB_RX(pBOld)+CV_BLOB_RX(pBNew))) &&
                       (fabs(pBOld->y-pBNew->y) < (CV_BLOB_RY(pBOld)+CV_BLOB_RY(pBNew))))
                        Good = 0;
                }
            }   /* Check intersection last blob with existed. */

            /* Check distance to image border: */
            if(Good)
            {   /* Check distance to image border: */
                CvBlob*  pB = pBNew;
                float    dx = MIN(pB->x,S.width-pB->x)/CV_BLOB_RX(pB);
                float    dy = MIN(pB->y,S.height-pB->y)/CV_BLOB_RY(pB);

                if(dx < 1.1 || dy < 1.1) Good = 0;
            }   /* Check distance to image border. */

            /* Check uniform motion: */
            if(Good)
            {
                int     N = EBD_FRAME_NUM;
                float   sum[2] = {0,0};
                float   jsum[2] = {0,0};
                float   a[2],b[2]; /* estimated parameters of moving x(t) = a*t+b*/

                int j;
                for(j=0; j<N; ++j)
                {
                    float   x = pBL[j]->x;
                    float   y = pBL[j]->y;
                    sum[0] += x;
                    jsum[0] += j*x;
                    sum[1] += y;
                    jsum[1] += j*y;
                }

                a[0] = 6*((1-N)*sum[0]+2*jsum[0])/(N*(N*N-1));
                b[0] = -2*((1-2*N)*sum[0]+3*jsum[0])/(N*(N+1));
                a[1] = 6*((1-N)*sum[1]+2*jsum[1])/(N*(N*N-1));
                b[1] = -2*((1-2*N)*sum[1]+3*jsum[1])/(N*(N+1));

                for(j=0; j<N; ++j)
                {
                    Error +=
                        pow(a[0]*j+b[0]-pBL[j]->x,2)+
                        pow(a[1]*j+b[1]-pBL[j]->y,2);
                }

                Error = sqrt(Error/N);

                if( Error > S.width*0.01 ||
                    fabs(a[0])>S.width*0.1 ||
                    fabs(a[1])>S.height*0.1)
                    Good = 0;

            }   /* Check configuration. */


            /* New best trajectory: */
            if(Good && (BestError == -1 || BestError > Error))
            {
                for(i=0; i<EBD_FRAME_NUM; ++i)
                {
                    pBL_BEST[i] = pBLIndex[i];
                }
                BestError = Error;
            }   /* New best trajectory. */

            /* Set next configuration: */
            for(i=0; i<EBD_FRAME_NUM; ++i)
            {
                pBLIndex[i]++;
                if(pBLIndex[i] != m_pBlobLists[i]->GetBlobNum()) break;
                pBLIndex[i]=0;
            }   /* Next time shift. */

            if(i==EBD_FRAME_NUM)finish=1;

        } while(!finish);	/* Check next time configuration of connected components. */

        #if 0
        {/**/
            printf("BlobDetector configurations = %d [",Count);
            int i;
            for(i=0; i<EBD_FRAME_NUM; ++i)
            {
                printf("%d,",m_pBlobLists[i]?m_pBlobLists[i]->GetBlobNum():0);
            }
            printf("]\n");

        }
        #endif

        if(BestError != -1)
        {   /* Write new blob to output and delete from blob list: */
            CvBlob* pNewBlob = m_pBlobLists[EBD_FRAME_NUM-1]->GetBlob(pBL_BEST[EBD_FRAME_NUM-1]);
            pNewBlobList->AddBlob(pNewBlob);

            for(i=0; i<EBD_FRAME_NUM; ++i)
            {   /* Remove blob from each list: */
                m_pBlobLists[i]->DelBlob(pBL_BEST[i]);
            }   /* Remove blob from each list. */

            result = 1;

        }   /* Write new blob to output and delete from blob list. */
    }   /*  Analyze blob list to find best blob trajectory. */

    return result;

}   /* cvDetectNewBlob */
void CvBlobTrackerAuto1::Process(IplImage* pImg, IplImage* pMask)
{
    int         CurBlobNum = 0;
    int         i;
    IplImage*   pFG = pMask;

    /* Bump frame counter: */
    m_FrameCount++;

    if(m_TimesFile)
    {
        static int64  TickCount = cvGetTickCount();
        static double TimeSum = 0;
        static int Count = 0;
        Count++;

        if(Count%100==0)
        {
#ifndef WINCE
            time_t ltime;
            time( &ltime );
			char* stime = ctime( &ltime );
#else
			/* WINCE does not have above POSIX functions (time,ctime) */
			const char* stime = " wince ";
#endif
            FILE* out = fopen(m_TimesFile,"at");
            double Time;
            TickCount = cvGetTickCount()-TickCount;
            Time = TickCount/FREQ;
            if(out){fprintf(out,"- %sFrame: %d ALL_TIME - %f\n",stime,Count,Time/1000);fclose(out);}

            TimeSum = 0;
            TickCount = cvGetTickCount();
        }
    }

    /* Update BG model: */
    TIME_BEGIN()

    if(m_pFG)
    {   /* If FG detector is needed: */
        m_pFG->Process(pImg);
        pFG = m_pFG->GetMask();
    }   /* If FG detector is needed. */

    TIME_END("FGDetector",-1)

    m_pFGMask = pFG; /* For external use. */

    /*if(m_pFG && m_pFG->GetParam("DebugWnd") == 1)
    {// debug foreground result
        IplImage *pFG = m_pFG->GetMask();
        if(pFG)
        {
            cvNamedWindow("FG",0);
            cvShowImage("FG", pFG);
        }
    }*/

    /* Track blobs: */
    TIME_BEGIN()
    if(m_pBT)
    {
        int i;
        m_pBT->Process(pImg, pFG);

        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Update data of tracked blob list: */
            CvBlob* pB = m_BlobList.GetBlob(i-1);
            int     BlobID = CV_BLOB_ID(pB);
            int     i = m_pBT->GetBlobIndexByID(BlobID);
            m_pBT->ProcessBlob(i, pB, pImg, pFG);
            pB->ID = BlobID;
        }
        CurBlobNum = m_pBT->GetBlobNum();
    }
    TIME_END("BlobTracker",CurBlobNum)

    /* This part should be removed: */
    if(m_BTReal && m_pBT)
    {   /* Update blob list (detect new blob for real blob tracker): */
        int i;

        for(i=m_pBT->GetBlobNum(); i>0; --i)
        {   /* Update data of tracked blob list: */
            CvBlob* pB = m_pBT->GetBlob(i-1);
            if(pB && m_BlobList.GetBlobByID(CV_BLOB_ID(pB)) == NULL )
            {
                CvBlobTrackAuto     NewB;
                NewB.blob = pB[0];
                NewB.BadFrames = 0;
                m_BlobList.AddBlob((CvBlob*)&NewB);
            }
        }   /* Next blob. */

        /* Delete blobs: */
        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Update tracked-blob list: */
            CvBlob* pB = m_BlobList.GetBlob(i-1);
            if(pB && m_pBT->GetBlobByID(CV_BLOB_ID(pB)) == NULL )
            {
                m_BlobList.DelBlob(i-1);
            }
        }   /* Next blob. */
    }   /* Update bloblist. */


    TIME_BEGIN()
    if(m_pBTPostProc)
    {   /* Post-processing module: */
        int i;
        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Update tracked-blob list: */
            CvBlob* pB = m_BlobList.GetBlob(i-1);
            m_pBTPostProc->AddBlob(pB);
        }
        m_pBTPostProc->Process();

        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Update tracked-blob list: */
            CvBlob* pB = m_BlobList.GetBlob(i-1);
            int     BlobID = CV_BLOB_ID(pB);
            CvBlob* pBN = m_pBTPostProc->GetBlobByID(BlobID);

            if(pBN && m_UsePPData && pBN->w >= CV_BLOB_MINW && pBN->h >= CV_BLOB_MINH)
            {   /* Set new data for tracker: */
                m_pBT->SetBlobByID(BlobID, pBN );
            }

            if(pBN)
            {   /* Update blob list with results from postprocessing: */
                pB[0] = pBN[0];
            }
        }
    }   /* Post-processing module. */

    TIME_END("PostProcessing",CurBlobNum)

    /* Blob deleter (experimental and simple): */
    TIME_BEGIN()
    if(pFG)
    {   /* Blob deleter: */
        int i;
        if(!m_BTReal)for(i=m_BlobList.GetBlobNum();i>0;--i)
        {   /* Check all blobs on list: */
            CvBlobTrackAuto* pB = (CvBlobTrackAuto*)(m_BlobList.GetBlob(i-1));
            int     Good = 0;
            int     w=pFG->width;
            int     h=pFG->height;
            CvRect  r = CV_BLOB_RECT(pB);
            CvMat   mat;
            double  aver = 0;
            double  area = CV_BLOB_WX(pB)*CV_BLOB_WY(pB);
            if(r.x < 0){r.width += r.x;r.x = 0;}
            if(r.y < 0){r.height += r.y;r.y = 0;}
            if(r.x+r.width>=w){r.width = w-r.x-1;}
            if(r.y+r.height>=h){r.height = h-r.y-1;}

            if(r.width > 4 && r.height > 4 && r.x < w && r.y < h &&
                r.x >=0 && r.y >=0 &&
                r.x+r.width < w && r.y+r.height < h && area > 0)
            {
                aver = cvSum(cvGetSubRect(pFG,&mat,r)).val[0] / area;
                /* if mask in blob area exists then its blob OK*/
                if(aver > 0.1*255)Good = 1;
            }
            else
            {
                pB->BadFrames+=2;
            }

            if(Good)
            {
                pB->BadFrames = 0;
            }
            else
            {
                pB->BadFrames++;
            }
        }   /* Next blob: */

        /* Check error count: */
        for(i=0; i<m_BlobList.GetBlobNum(); ++i)
        {
            CvBlobTrackAuto* pB = (CvBlobTrackAuto*)m_BlobList.GetBlob(i);

            if(pB->BadFrames>3)
            {   /* Delete such objects */
                /* from tracker...     */
                m_pBT->DelBlobByID(CV_BLOB_ID(pB));

                /* ... and from local list: */
                m_BlobList.DelBlob(i);
                i--;
            }
        }   /* Check error count for next blob. */
    }   /*  Blob deleter. */

    TIME_END("BlobDeleter",m_BlobList.GetBlobNum())

    /* Update blobs: */
    TIME_BEGIN()
    if(m_pBT)
        m_pBT->Update(pImg, pFG);
    TIME_END("BlobTrackerUpdate",CurBlobNum)

    /* Detect new blob: */
    TIME_BEGIN()
    if(!m_BTReal && m_pBD && pFG && (m_FrameCount > m_FGTrainFrames) )
    {   /* Detect new blob: */
        static CvBlobSeq    NewBlobList;
        CvBlobTrackAuto     NewB;

        NewBlobList.Clear();

        if(m_pBD->DetectNewBlob(pImg, pFG, &NewBlobList, &m_BlobList))
        {   /* Add new blob to tracker and blob list: */
            int i;
            IplImage* pMask = pFG;

            /*if(0)if(NewBlobList.GetBlobNum()>0 && pFG )
            {// erode FG mask (only for FG_0 and MS1||MS2)
                pMask = cvCloneImage(pFG);
                cvErode(pFG,pMask,NULL,2);
            }*/

            for(i=0; i<NewBlobList.GetBlobNum(); ++i)
            {
                CvBlob* pBN = NewBlobList.GetBlob(i);
                pBN->ID = m_NextBlobID;

                if(pBN && pBN->w >= CV_BLOB_MINW && pBN->h >= CV_BLOB_MINH)
                {
                    CvBlob* pB = m_pBT->AddBlob(pBN, pImg, pMask );
                    if(pB)
                    {
                        NewB.blob = pB[0];
                        NewB.BadFrames = 0;
                        m_BlobList.AddBlob((CvBlob*)&NewB);
                        m_NextBlobID++;
                    }
                }
            }   /* Add next blob from list of detected blob. */

            if(pMask != pFG) cvReleaseImage(&pMask);

        }   /* Create and add new blobs and trackers. */

    }   /*  Detect new blob. */

    TIME_END("BlobDetector",-1)

    TIME_BEGIN()
    if(m_pBTGen)
    {   /* Run track generator: */
        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Update data of tracked blob list: */
            CvBlob* pB = m_BlobList.GetBlob(i-1);
            m_pBTGen->AddBlob(pB);
        }
        m_pBTGen->Process(pImg, pFG);
    }   /* Run track generator: */
    TIME_END("TrajectoryGeneration",-1)

    TIME_BEGIN()
    if(m_pBTA)
    {   /* Trajectory analysis module: */
        int i;
        for(i=m_BlobList.GetBlobNum(); i>0; i--)
            m_pBTA->AddBlob(m_BlobList.GetBlob(i-1));

        m_pBTA->Process(pImg, pFG);

    }   /* Trajectory analysis module. */

    TIME_END("TrackAnalysis",m_BlobList.GetBlobNum())

} /* CvBlobTrackerAuto1::Process */
Exemplo n.º 10
0
    virtual void    Process(IplImage* pImg, IplImage* pImgFG = NULL)
    {
        CvSeq*      cnts;
        CvSeq*      cnt;
        int i;

        m_pImg = pImg;
        m_pImgFG = pImgFG;

        if(m_BlobList.GetBlobNum() <= 0 ) return;

        /* Clear bloblist for new blobs: */
        m_BlobListNew.Clear();

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


        /* Find CC: */
#if 0
        {   // By contour clustering:
            cvFindBlobsByCCClasters(pImgFG, &m_BlobListNew, m_pMem);
        }
#else
        {   /* 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 contour: */
            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);
        }
#endif
        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Predict new blob position: */
            CvBlob*         pB=NULL;
            DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(i-1);

            /* Update predictor by previous value of blob: */
            pBT->pPredictor->Update(&(pBT->blob));

            /* Predict current position: */
            pB = pBT->pPredictor->Predict();

            if(pB)
            {
                pBT->BlobPredict = pB[0];
            }
            else
            {
                pBT->BlobPredict = pBT->blob;
            }
        }   /* Predict new blob position. */

        if(m_Collision)
        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Predict collision. */
            int             Collision = 0;
            int             j;
            DefBlobTracker* pF = (DefBlobTracker*)m_BlobList.GetBlob(i-1);

            for(j=m_BlobList.GetBlobNum(); j>0; --j)
            {   /* Predict collision: */
                CvBlob* pB1;
                CvBlob* pB2;
                DefBlobTracker* pF2 = (DefBlobTracker*)m_BlobList.GetBlob(j-1);
                if(i==j) continue;
                pB1 = &pF->BlobPredict;
                pB2 = &pF2->BlobPredict;

                if( fabs(pB1->x-pB2->x)<0.6*(pB1->w+pB2->w) &&
                    fabs(pB1->y-pB2->y)<0.6*(pB1->h+pB2->h) ) Collision = 1;

                pB1 = &pF->blob;
                pB2 = &pF2->blob;

                if( fabs(pB1->x-pB2->x)<0.6*(pB1->w+pB2->w) &&
                    fabs(pB1->y-pB2->y)<0.6*(pB1->h+pB2->h) ) Collision = 1;

                if(Collision) break;

            }   /* Check next blob to cross current. */

            pF->Collision = Collision;

        }   /* Predict collision. */

        for(i=m_BlobList.GetBlobNum(); i>0; --i)
        {   /* Find a neighbour on current frame
             * for each blob from previous frame:
             */
            CvBlob*         pBl = m_BlobList.GetBlob(i-1);
            DefBlobTracker* pBT = (DefBlobTracker*)pBl;
            //int             BlobID = CV_BLOB_ID(pB);
            //CvBlob*         pBBest = NULL;
            //double          DistBest = -1;
            //int j;

            if(pBT->pBlobHyp->GetBlobNum()>0)
            {   /* Track all hypotheses: */
                int h,hN = pBT->pBlobHyp->GetBlobNum();
                for(h=0; h<hN; ++h)
                {
                    int         j, jN = m_BlobListNew.GetBlobNum();
                    CvBlob*     pB = pBT->pBlobHyp->GetBlob(h);
                    int         BlobID = CV_BLOB_ID(pB);
                    CvBlob*     pBBest = NULL;
                    double      DistBest = -1;
                    for(j=0; j<jN; j++)
                    {   /* Find best CC: */
                        double  Dist = -1;
                        CvBlob* pBNew = m_BlobListNew.GetBlob(j);
                        double  dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
                        double  dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
                        if(dx > 2*CV_BLOB_WX(pB) || dy > 2*CV_BLOB_WY(pB)) continue;

                        Dist = sqrt(dx*dx+dy*dy);
                        if(Dist < DistBest || pBBest == NULL)
                        {
                            DistBest = Dist;
                            pBBest = pBNew;
                        }
                    }   /* Find best CC. */

                    if(pBBest)
                    {
                        pB[0] = pBBest[0];
                        CV_BLOB_ID(pB) = BlobID;
                    }
                    else
                    {   /* Delete this hypothesis. */
                        pBT->pBlobHyp->DelBlob(h);
                        h--;
                        hN--;
                    }
                }   /* Next hypothysis. */
            }   /*  Track all hypotheses. */
        }   /*  Track next blob. */

        m_ClearHyp = 1;

    } /* Process. */
	virtual void    Process(IplImage* pImg, IplImage* pImgFG = NULL)
	{//第一步:从前景里提取团块:位置+大小 ==》保存到m_BlobListNew链表里
		//第二步:预测团块位置 更新到各自的BlobPredict里
		//第三步:标记每个目标的碰撞
		//已经跟踪的团块链表里, 相互匹配预测位置 和当前位置。如果重合了,就将Collision=1 
		//第四步: 对于每个目标, 用他可能的位置 和新的团块链表比较,
		//求最近的一个 作为他新的可能位置。


		CvSeq*      cnts;
		CvSeq*      cnt;
		int i;

		m_pImg = pImg;
		m_pImgFG = pImgFG;

		if (m_BlobList.GetBlobNum() <= 0) return;

		/* Clear bloblist for new blobs: */
		m_BlobListNew.Clear();

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

		//第一步:从前景里提取团块:位置+大小 ==》保存到m_BlobListNew链表里



		//第二步:预测团块位置 更新到各自的BlobPredict里
		for (i = m_BlobList.GetBlobNum(); i>0; --i)
		{   /* Predict new blob position: */

			CvBlob*         pB = NULL;
			DefBlobTrackerColorTracker* pBT = (DefBlobTrackerColorTracker*)m_BlobList.GetBlob(i - 1);

			/* Update predictor by previous value of blob: */
			//更新上一帧的位置
			pBT->pPredictor->Update(&(pBT->blob));

			//预测下一帧位置
			/* Predict current position: */
			pB = pBT->pPredictor->Predict();

			if (pB)
			{
				pBT->BlobPredict = pB[0];
			}
			else
			{
				pBT->BlobPredict = pBT->blob;
			}
		}   /* Predict new blob position. */

		//第三步:标记每个目标的碰撞
		//已经跟踪的团块链表里, 相互匹配预测位置 和当前位置。如果重合了,就将Collision=1 
		if (m_Collision)//如果需要考虑碰撞
		for (i = m_BlobList.GetBlobNum(); i>0; --i)
		{   /* Predict collision. */
			int             Collision = 0;
			int             j;
			DefBlobTrackerColorTracker* pF = (DefBlobTrackerColorTracker*)m_BlobList.GetBlob(i - 1);

			for (j = m_BlobList.GetBlobNum(); j>0; --j)
			{   /* Predict collision: */
				CvBlob* pB1;
				CvBlob* pB2;
				DefBlobTrackerColorTracker* pF2 = (DefBlobTrackerColorTracker*)m_BlobList.GetBlob(j - 1);
				if (i == j) continue;
				pB1 = &pF->BlobPredict;
				pB2 = &pF2->BlobPredict;

				//两个团块预测位置是否碰撞了
				if (fabs(pB1->x - pB2->x)<0.6*(pB1->w + pB2->w) &&
					fabs(pB1->y - pB2->y)<0.6*(pB1->h + pB2->h)) Collision = 1;

				pB1 = &pF->blob;
				pB2 = &pF2->blob;

				//当前位置是否碰撞了
				if (fabs(pB1->x - pB2->x)<0.6*(pB1->w + pB2->w) &&
					fabs(pB1->y - pB2->y)<0.6*(pB1->h + pB2->h)) Collision = 1;

				if (Collision) break;

			}   /* Check next blob to cross current. */

			pF->Collision = Collision;

		}   /* Predict collision. */


		//第四步: 对于每个目标, 用他可能的位置 和新的团块链表比较,
		//求最近的一个 作为他新的可能位置。
		for (i = m_BlobList.GetBlobNum(); i>0; --i)
		{   /* Find a neighbour on current frame
			* for each blob from previous frame:
			*/
			CvBlob*         pBl = m_BlobList.GetBlob(i - 1);
			DefBlobTrackerColorTracker* pBT = (DefBlobTrackerColorTracker*)pBl;
			//int             BlobID = CV_BLOB_ID(pB);
			//CvBlob*         pBBest = NULL;
			//double          DistBest = -1;
			//int j;

			if (pBT->pBlobHyp->GetBlobNum()>0)//pBlobHyp难道是每个目标可能的下一个位置???
			{   /* Track all hypotheses: */

				int h, hN = pBT->pBlobHyp->GetBlobNum();
				for (h = 0; h<hN; ++h)
				{//提取每一个可能的位置 pBlobHyp,
					//和当前的前景团块链表m_BlobListNew 比较,求得最近团块,
					//将最近团块值==》赋值给pBlobHyp
					//如果最近团块也很远就删除 这个可能
					int         j, jN = m_BlobListNew.GetBlobNum();
					CvBlob*     pB = pBT->pBlobHyp->GetBlob(h);//可能的位置
					int         BlobID = CV_BLOB_ID(pB);
					CvBlob*     pBBest = NULL;
					double      DistBest = -1;
					for (j = 0; j<jN; j++)
					{   /* Find best CC: */
						double  Dist = -1;
						CvBlob* pBNew = m_BlobListNew.GetBlob(j);
						double  dx = fabs(CV_BLOB_X(pB) - CV_BLOB_X(pBNew));
						double  dy = fabs(CV_BLOB_Y(pB) - CV_BLOB_Y(pBNew));
						if (dx > 2 * CV_BLOB_WX(pB) || dy > 2 * CV_BLOB_WY(pB)) continue;

						Dist = sqrt(dx*dx + dy*dy);
						if (Dist < DistBest || pBBest == NULL)
						{
							DistBest = Dist;
							pBBest = pBNew;
						}
					}   /* Find best CC. */

					if (pBBest)
					{
						pB[0] = pBBest[0];
						CV_BLOB_ID(pB) = BlobID;
					}
					else
					{   /* Delete this hypothesis. */
						pBT->pBlobHyp->DelBlob(h);
						h--;
						hN--;
					}
				}   /* Next hypothysis. */
			}   /*  Track all hypotheses. */
		}   /*  Track next blob. */

		m_ClearHyp = 1;

	} /* Process. */