经典的meanshift算法，实现对运动目标的检测，跟踪

/* RecogObjt.c * ----------- * A VC version by Waksman * of the matlab implementation * Track. */ #include "RecogObjt.h" /* Private Function Prototypes */ void DisplayImage(char *win_name, IplImage *frame); void DisplayMatrix(char *win_name, CvMat *mat); CvPoint MotionCompensation(CvMat *Ma, CvMat *Mb, int w, int h, int DispL, int DispH); void InitRect(int i, int j, int w, int h, CvRect *recta, CvRect *rectb); void Concentrate(CvMat *Mdiff, CvMat *Mdiff_conctr, int w, int h); int SalientSquare(CvMat *Mdiff_conctr, int w_index, int h_index, CvPoint Psalient[]); void DisplaySalient(CvPoint Psalient[], int z, CvMat *Ma1, CvMat *Mb1, int w_index, int h_index); int CountObjects(CvPoint Psalient[], int nsalient, int block_num[], int block_index[MAX_OBJECTS][MAX_BLOCK]); int IsNeighbourPoint(CvPoint *P1, CvPoint *P2); void GroupOneObject(CvPoint Psalient[], int nsalient, int block_num[], int block_index[MAX_OBJECTS][MAX_BLOCK], int object_num, char *flag, int first); CvRect FindObject(CvPoint Psalient[], int block_num, int block_index[MAX_BLOCK], CvMat *Ma1, CvMat *Mb1); CvRect ExtractLocal(CvPoint Psalient[], int block_num, int block_index[MAX_BLOCK], CvMat *mat); void InitialColor(CvScalar color[MAX_OBJECTS]); /**/ /* the frames are expected to be 3-channel and 8-bit */ int RecogObjt(IplImage* frame_a, IplImage* frame_b, CvRect rect[]) { IplImage *frame_sa, *frame_sb; //single channel images CvSize size; CvMat *Ma, *Mb; //original data from image CvMat *Ma1, *Mb1; //after background compensation CvPoint vector, p1, p2; CvRect recta, rectb; CvMat *Mdiff, *Mdiff_conctr; //difference and concentrated difference CvPoint Psalient[MAX_POINT]; //salient points int nsalient; int object_num, block_num[MAX_OBJECTS], block_index[MAX_OBJECTS][MAX_BLOCK]; //counters of objects and their blocks CvScalar color[MAX_OBJECTS]; int w, h, w1, h1, i; w=frame_a->width; h=frame_a->height; size.height=h; size.width=w; frame_sa=cvCreateImage(size, 8, 1); frame_sb=cvCreateImage(size, 8, 1); cvCvtColor(frame_a, frame_sa, CV_BGR2GRAY); cvCvtColor(frame_b, frame_sb, CV_BGR2GRAY); //image to matrix Ma=cvCreateMat(h, w, CV_8UC1); Mb=cvCreateMat(h, w, CV_8UC1); cvGetMat(frame_sa, Ma, NULL, 0); cvGetMat(frame_sb, Mb, NULL, 0); //background compensation vector=MotionCompensation(Ma, Mb, w, h, L, H); InitRect(vector.x, vector.y, w, h, &recta, &rectb); w1=recta.width; h1=recta.height; Ma1=cvCreateMat(h1, w1, CV_8UC1); Mb1=cvCreateMat(h1, w1, CV_8UC1); cvGetSubArr(Ma, Ma1, recta); cvGetSubArr(Mb, Mb1, rectb); cvReleaseMat(&Ma); cvReleaseMat(&Mb); //calculate difference Mdiff=cvCreateMat(h1, w1, CV_8UC1); Mdiff_conctr=cvCreateMat(h1, w1, CV_8UC1); cvAbsDiff(Ma1, Mb1, Mdiff); DisplayMatrix("Mdiff", Mdiff); //display matrix Concentrate(Mdiff, Mdiff_conctr, w1, h1); DisplayMatrix("Mdiff_conctr", Mdiff_conctr); //display matrix cvReleaseMat(&Mdiff); //find salient squares nsalient=SalientSquare(Mdiff_conctr, w1/C, h1/C, Psalient); DisplaySalient(Psalient, nsalient, Ma1, Mb1, w1/C, h1/C); cvReleaseMat(&Mdiff_conctr); //count objects object_num=CountObjects(Psalient, nsalient, block_num, block_index); //find each object in Ma1 and Mb1 for(i=0; i<object_num; i++) { rect[i]=FindObject(Psalient, block_num[i], block_index[i], Ma1, Mb1); } cvReleaseMat(&Ma1); cvReleaseMat(&Mb1); cvReleaseImage(&frame_sa); cvReleaseImage(&frame_sb); //draw the rectangular containing the objects InitialColor(color); for(i=0; i<object_num; i++) { //adjust to position in frame_b rect[i].x+=rectb.x; rect[i].y+=rectb.y; p1.x=rect[i].x; p1.y=rect[i].y; p2.x=rect[i].x+rect[i].width; p2.y=rect[i].y+rect[i].height; cvRectangle(frame_b, p1, p2, color[i], 1, 8, 0); } DisplayImage("frame_b", frame_b); return(object_num); } /* Private Function Implimentation */ void InitialColor(CvScalar color[MAX_OBJECTS]) { int i; for(i=0; i<MAX_OBJECTS; i++) { switch(i) { case 0: color[i]=CV_RGB(255, 0, 0); break; //red case 1: color[i]=CV_RGB(0, 255, 0); break; //green case 2: color[i]=CV_RGB(0, 0, 255); break; //blue case 3: color[i]=CV_RGB(255, 255, 0); break; //yellow case 4: color[i]=CV_RGB(0, 255, 255); break; //skyblue case 5: color[i]=CV_RGB(255, 0, 255); break; //purple case 6: color[i]=CV_RGB(255, 255, 255); break; //white default: color[i]=CV_RGB(0, 0, 0); //black } } } /* Return a rectangular containing the object in Mb1 */ CvRect FindObject(CvPoint Psalient[], int block_num, int block_index[MAX_BLOCK], CvMat *Ma1, CvMat *Mb1) { CvMat *Ma2, *Mb2; //local region around the object CvMat *Ma3, *Mb3; //region containing the object in each image CvPoint vector; int w2, h2, w3, h3; CvRect rect, recta, rectb; //extract the local region around the object rect=ExtractLocal(Psalient, block_num, block_index, Ma1); Ma2=cvCreateMat(rect.height, rect.width, CV_8UC1); Mb2=cvCreateMat(rect.height, rect.width, CV_8UC1); cvGetSubArr(Ma1, Ma2, rect); cvGetSubArr(Mb1, Mb2, rect); // DisplayMatrix("Ma2", Ma2); // DisplayMatrix("Mb2", Mb2); //motion compensation // if( abs(cvAvg(Ma2, NULL).val[0]) != abs(cvAvg(Mb2, NULL).val[0]) ) { w2=Ma2->cols; h2=Ma2->rows; vector=MotionCompensation(Ma2, Mb2, w2, h2, w2/2, h2/2); InitRect(vector.x, vector.y, w2, h2, &recta, &rectb); w3=recta.width; h3=recta.height; Ma3=cvCreateMat(h3, w3, CV_8UC1); Mb3=cvCreateMat(h3, w3, CV_8UC1); cvGetSubArr(Ma2, Ma3, recta); cvGetSubArr(Mb2, Mb3, rectb); // DisplayMatrix("Ma3", Ma3); // DisplayMatrix("Mb3", Mb3); } cvReleaseMat(&Ma2); cvReleaseMat(&Mb2); cvReleaseMat(&Ma3); cvReleaseMat(&Mb3); rectb.x+=rect.x; rectb.y+=rect.y; return(rectb); } /* extract the local region around the object */ CvRect ExtractLocal(CvPoint Psalient[], int block_num, int block_index[MAX_BLOCK], CvMat *mat) { CvRect rect; CvPoint p; int w1, h1; int i, minx, miny, maxx, maxy; w1=mat->cols; h1=mat->rows; minx=w1; miny=h1; maxx=0; maxy=0; for(i=0; i<block_num; i++) { p=Psalient[block_index[i]]; if(p.x<minx) minx=p.x; if(p.x>maxx) maxx=p.x; if(p.y<miny) miny=p.y; if(p.y>maxy) maxy=p.y; } rect.x=minx*C; rect.y=miny*C; rect.height=(maxy-miny+1)*C; rect.width=(maxx-minx+1)*C; if(maxx==w1/C) { rect.width=w1-minx*C; } if(maxy==h1/C) { rect.height=h1-miny*C; } return(rect); } int CountObjects(CvPoint Psalient[], int nsalient, int block_num[], int block_index[MAX_OBJECTS][MAX_BLOCK]) { int object_num=0; //result char *flag; //signal whether a point has been processed int i; flag=(char *)malloc(sizeof(char)*nsalient); memset(flag, 0, nsalient); for(i=0; i<nsalient; i++) //for every unprocessed { if(flag[i]==0) { block_num[object_num]=0; GroupOneObject(Psalient, nsalient, block_num, block_index, object_num, flag, i); object_num++; } if(object_num>=MAX_OBJECTS) { printf("Too many objects!\n"); return(object_num); } } printf("%d objects found.\n\n", object_num); return(object_num); } /* Group Psalient[first]'s neighbour and neighbour's neighbour as the object_num's object */ void GroupOneObject(CvPoint Psalient[], int nsalient, int block_num[], int block_index[MAX_OBJECTS][MAX_BLOCK], int object_num, char *flag, int first) { int i; block_index[object_num][(block_num[object_num])]=first; (block_num[object_num])++; if(block_num[object_num]>=MAX_BLOCK) { printf("Too many blocks!\n"); return; } flag[first]=1; for(i=0/*first+1*/; i<nsalient; i++) { if(flag[i]==0 && i!=first && IsNeighbourPoint(&Psalient[first], &Psalient[i])==1) { GroupOneObject(Psalient, nsalient, block_num, block_index, object_num, flag, i); } } } /* T * TPT * T */ int IsNeighbourPoint(CvPoint *P1, CvPoint *P2) { if(P1->x==P2->x) { if(abs(P1->y-P2->y)<=1) return(1); } if(P1->y==P2->y) { if(abs(P1->x-P2->x)<=1) return(1); } return(0); } void DisplaySalient(CvPoint Psalient[], int z, CvMat *Ma1, CvMa