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. */
Ejemplo n.º 2
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 */
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. */
 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 */
Ejemplo n.º 8
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)
    {
        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 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);
        }

        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 correspondence 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);
                    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
            {   /* Non-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
                        {    /* Choose 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);
            }   /* Non-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    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. */