recogobjt.c

学习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, CvMat *Mb1, int w_index, int h_index)
{
	IplImage *Ia1, *Ib1;
	CvSize size;
	CvRect rect;
	CvMat *Msub;
	int i;

	size.height=Ma1->rows;
	size.width=Ma1->cols;
	Ia1=cvCreateImage(size, 8, 1);
	Ib1=cvCreateImage(size, 8, 1);
	cvSetZero(Ia1);
	cvSetZero(Ib1);

	for(i=0; i<z; i++)
	{
		//initialize rect size and position
		rect.x=Psalient[i].x*C;
		rect.y=Psalient[i].y*C;
		rect.width=C;
		rect.height=C;
		if(Psalient[i].x==w_index)
		{
			rect.width=Ma1->cols-C*w_index;
		}
		if(Psalient[i].y==h_index)
		{
			rect.height=Ma1->rows-C*h_index;
		}
		if(rect.width==0 || rect.height==0)	//when C is a factor of width or height
		{
			break;
		}

		Msub=cvCreateMat(rect.height, rect.width, CV_8UC1);
		cvSetImageROI(Ia1, rect);
		cvGetSubArr(Ma1, Msub, rect);
		cvCopy(Msub, Ia1, 0);
		cvSetImageROI(Ib1, rect);
		cvGetSubArr(Mb1, Msub, rect);
		cvCopy(Msub, Ib1, 0);
		cvReleaseMat(&Msub);
	}//end for

	rect.x=rect.y=0;
	rect.height=Ia1->height;
	rect.width=Ia1->width;
	cvSetImageROI(Ia1, rect);
	cvSetImageROI(Ib1, rect);
	DisplayImage("Ia1", Ia1);
	DisplayImage("Ib1", Ib1);

	cvReleaseImage(&Ia1);
	cvReleaseImage(&Ib1);
}

/* Find salient squares of size C*C, there are w_index's and h_index's squares in Mdiff_conctr along width and length. */
int SalientSquare(CvMat *Mdiff_conctr, int w_index, int h_index, CvPoint Psalient[])
{
	int i, j;
	CvRect rect;
	CvMat *Msub;
	double max, min;
	int z=0;

	Msub=cvCreateMat(C, C, CV_8UC1);

	for(i=0; i<=w_index; i++)
	{
		for(j=0; j<=h_index; j++)
		{
			//initialize rect size and position
			rect.x=C*i;
			rect.y=C*j;
			rect.width=C;
			rect.height=C;
			if(i==w_index)
			{
				rect.width=Mdiff_conctr->cols-C*w_index;
			}
			if(j==h_index)
			{
				rect.height=Mdiff_conctr->rows-C*h_index;
			}
			if(rect.width==0 || rect.height==0)	//when C is a factor of width or height
			{
				break;
			}

			cvGetSubArr(Mdiff_conctr, Msub, rect);
			cvMinMaxLoc(Msub, &min, &max, NULL, NULL, NULL);
			if(max>G)
			{
				if(z<MAX_POINT)
				{
					Psalient[z].x=i;
					Psalient[z].y=j;
					z++;
				}
				else
				{
					printf("Too many salient squares");
				}
			}
		};//end for j
	};//end for i

	cvReleaseMat(&Msub);
	return(z);
}

void Concentrate(CvMat *Mdiff, CvMat *Mdiff_conctr, int w, int h)
{
	int i, j, offsetp, offsetm;
	CvScalar zero, element;
	CvRect rect;
	CvMat *Msub;

	zero.val[0]=0;
	offsetp=(R+1)/2;
	offsetm=(R-1)/2;
	rect.height=2*offsetm+1;
	rect.width=2*offsetm+1;
	Msub=cvCreateMat(2*offsetm+1, 2*offsetm+1, CV_8UC1);

	for(i=0; i<w; i++)
	{
		for(j=0; j<h; j++)
		{
			if(i>=offsetp && i<=w-offsetm-1 && j>=offsetp && j<=h-offsetm-1)
			{
				if(cvGetAt(Mdiff, j, i).val[0]>G)
				{
					rect.x=i-offsetm;
					rect.y=j-offsetm;
					cvGetSubArr(Mdiff, Msub, rect);
					element=cvAvg(Msub, NULL);
					cvSetAt(Mdiff_conctr, element, j, i);
				}
				else
				{
					cvSetAt(Mdiff_conctr, zero, j, i);
				}
			}
			else	//on the boarder
			{
				cvSetAt(Mdiff_conctr, zero, j, i);
			};
		}//end for j
	}//end for i

	cvReleaseMat(&Msub);
}

/* Just return the motion vector */
CvPoint MotionCompensation(CvMat *Ma, CvMat *Mb, int w, int h, int DispL, int DispH)
{
	CvMat *MaSub, *MbSub, *Mdiff, *Mcorrel;
	int i, j, iopt=0, jopt=0;
	double diff=0, least_diff=10000.0;
	CvRect recta, rectb;
	CvScalar Savg;
	CvPoint vector;	//result;

	Mcorrel=cvCreateMat(2*DispH+1, 2*DispL+1, CV_8UC3/*CV_32SC1*/);

	for(i=-1*DispL; i<=DispL; i++)
	{
		for(j=-1*DispH; j<=DispH; j++)
		{
			InitRect(i, j, w, h, &recta, &rectb);	//initial rect
			MaSub=cvCreateMat(recta.height, recta.width, CV_8UC1);
			MbSub=cvCreateMat(recta.height, recta.width, CV_8UC1);
			Mdiff=cvCreateMat(recta.height, recta.width, CV_8UC1);

			cvGetSubArr(Ma, MaSub, recta);
			cvGetSubArr(Mb, MbSub, rectb);
			cvAbsDiff(MaSub, MbSub, Mdiff);	//find difference
			Savg=cvAvg(Mdiff, NULL);
			diff=Savg.val[0];

//			printf("i=%d, j=%d, diff=%f\n", i, j, diff);
			Savg.val[0]=0;//diff*15;	//B
			if(diff<=50)	Savg.val[1]=(50-diff)*6;	//G when closer
			else	Savg.val[2]=(diff-50)*15;	//R when farther
			cvSetAt(Mcorrel, Savg, j+DispH, i+DispL);

			if(diff<least_diff)	//compare to find the best
			{
				least_diff=diff;
				iopt=i;
				jopt=j;
			}

			cvReleaseMat(&MaSub);
			cvReleaseMat(&MbSub);
			cvReleaseMat(&Mdiff);
		}//end for j
	}//end for i

//	DisplayMatrix("Mcorrel", Mcorrel);
	printf("least_diff=%f\n", least_diff);
	printf("iopt=%d, jopt=%d\n", iopt, jopt);

	cvReleaseMat(&Mcorrel);

	vector.x=iopt;
	vector.y=jopt;

	return(vector);
}

void InitRect(int i, int j, int w, int h, CvRect *recta, CvRect *rectb)
{
	recta->height=h-abs(j);
	recta->width=w-abs(i);
	rectb->height=h-abs(j);
	rectb->width=w-abs(i);
	if(i>=0 && j>=0)
	{
		recta->x=0;
		recta->y=0;
		rectb->x=i;
		rectb->y=j;
	}
	else if(i>=0 && j<0)
	{
		recta->x=0;
		recta->y=-j;
		rectb->x=i;
		rectb->y=0;
	}
	else if(i<0 && j>=0)
	{
		recta->x=-i;
		recta->y=0;
		rectb->x=0;
		rectb->y=j;
	}
	else if(i<0 && j<0)
	{
		recta->x=-i;
		recta->y=-j;
		rectb->x=0;
		rectb->y=0;
	};
}

/* Creat a window to display frame, wait until user close the window */
void DisplayImage(char *win_name, IplImage *frame)
{
	int temp;
	temp=frame->origin;
	frame->origin=1;

	cvNamedWindow( win_name, 1 );
	cvShowImage( win_name, frame);
	cvWaitKey( 0 );
	cvDestroyWindow( win_name );

	frame->origin=temp;
}

void DisplayMatrix(char *win_name, CvMat *mat)
{
	IplImage image;
	cvGetImage(mat, &image);	//matrix to image
	DisplayImage(win_name, &image);
}