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. */
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( <ime );
char* stime = ctime( <ime );
#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 */
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. */