my.c

不错的particle filter的程序

/* File: my.cpp
 * -----------
 */

#include "defs.h"
#include "utils.h"
#include "observation.h"
#include "my.h"

/* Private Function Prototype */
double EvalOverlap(particle* particles, int n);
int CommonArea(CvRect *rect1, CvRect *rect2);

//update histogram to the current position
void UpdateHisto( IplImage* img, int r, int c, int w, int h, histogram* ref_histo )
{
  IplImage* tmp;
  histogram* histo;
  int i, n;

  /* extract region around (r,c) and compute and normalize its histogram */
  cvSetImageROI( img, cvRect( c - w / 2, r - h / 2, w, h ) );
  tmp = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 3 );
  cvCopy( img, tmp, NULL );	//temp=ROI of img
  cvResetImageROI( img );
  histo = calc_histogram( &tmp, 1 );
  cvReleaseImage( &tmp );
  normalize_histogram( histo );

  //update
  n=histo->n;
  for(i=0; i<n; i++)
  {
	  ref_histo->histo[i]=(1-PRE_WEIGHT)*histo->histo[i]+PRE_WEIGHT*ref_histo->histo[i];
  }

  free( histo );
}

void MaskParticleImage(IplImage* img, particle p, FILE *outfile)
{
  CvRect rect;
  CvScalar v;
  int i, j;
  unsigned char z, nz;

  v.val[0]=255;
  rect.x = cvRound( p.x - 0.5 * p.sw * p.width );
  rect.y = cvRound( p.y - 0.5 * p.sh * p.height );
  rect.width = cvRound( p.sw * p.width );
  rect.height = cvRound( p.sh * p.height );
	z=0;
	nz=255;
	for(j=img->height-1; j>=0; j--)
	{
		for(i=0; i<img->width; i++)
		{
			if(i>=rect.x && i<rect.x+rect.width && j>=rect.y && j<rect.y+rect.height)
			{
				fwrite(&nz, 1, 1, outfile);
			}
			else
			{
				fwrite(&z, 1, 1, outfile);
			}
		}
	}
  cvSetZero(img);

  cvSetImageROI(img, rect);
  cvSet(img, v, NULL);
  rect=cvRect(0, 0, img->width, img->height);
  cvSetImageROI(img, rect);

}

void DisplayDistribution(particle* particles, float sum, int n, char *window, int export)
{
	int i, h;
	IplImage* img;
	int Height=n;
	FILE *file;
	char name[250];

	if(export>0)
	{
		sprintf(name, "Distri_%d.txt", export);
		file=fopen(name, "w");
		for(i=0; i<n; i++)
		{
			fprintf(file, "%f\r\n", particles[i].w);
		}
		fclose(file);
	}

	img=cvCreateImage(cvSize(n*2, Height), 8, 3);
	cvSet(img, cvScalar(255, 255, 255, 255), NULL);

	h=min(cvRound(sum), Height);
//	printf("%d %f\n", h, sum);
	cvLine(img, cvPoint(0, h), cvPoint(n*2-1, h), CV_RGB(200, 0, 0), 1, 8, 0);
	for(i=0; i<n; i++)
	{
		h=min(cvRound(particles[i].w*1000), Height);
		cvLine(img, cvPoint(2*i, 0), cvPoint(2*i, h), CV_RGB(0, 0, 0), 1, 8, 0);
	}

	img->origin=1;
	cvNamedWindow(window, 1);
	cvShowImage(window, img);
//	cvWaitKey(0);
	cvReleaseImage(&img);
}

void DisplayParticleHistogram(histogram* ref_histo, IplImage* img, int r, int c, int w, int h, char *window, FILE *file)
{
	IplImage* tmp;
	histogram* histo;

	/* extract region around (r,c) and compute and normalize its histogram */
	cvSetImageROI( img, cvRect( c - w / 2, r - h / 2, w, h ) );
	tmp = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 3 );
	cvCopy( img, tmp, NULL );	//temp=ROI of img
	cvResetImageROI( img );
	histo = calc_histogram( &tmp, 1 );
	cvReleaseImage( &tmp );
	normalize_histogram( histo );

	fprintf(file, "%f\n", histo_dist_sq( histo, ref_histo ));
	/* display histo and ref_histo */
	DisplayHistogram(ref_histo, histo, window);

	free( histo );
}

void DisplayHistogram(histogram* ref_histo, histogram* histo, char *window)
{
	int i, h;
	IplImage* img;
	int Height=ref_histo->n;

	img=cvCreateImage(cvSize(Height*2, Height), 8, 3);
	cvSet(img, cvScalar(255, 255, 255, 255), NULL);

	for(i=0; i<ref_histo->n; i++)
	{
		if(ref_histo->histo[i]==0)
		{
			h=0;
		}
		else
		{
			h=cvRound(max((5+log(ref_histo->histo[i]))*20, 0));
			h=min(h, Height);
		}
		cvLine(img, cvPoint(2*i, h), cvPoint(2*i, 0), CV_RGB(255, 0, 0), 1, 8, 0 );	//cvPoint(2*i+1, h)
		if(histo->histo[i]==0)
		{
			h=0;
		}
		else
		{
			h=cvRound(max((5+log(histo->histo[i]))*20, 0));
			h=min(h, Height);
		}
		cvCircle(img, cvPoint(2*i, h), 1, CV_RGB(0, 0, 0), -1, 8, 0 );
	}

	img->origin=1;
	cvNamedWindow(window, 1);
	cvShowImage(window, img);
//	cvWaitKey(0);
	cvReleaseImage(&img);
}

int TryUpdateHistogram(particle* particles, int n, histogram* ref_histo)
{
	int i=0, user;

	qsort( particles, n, sizeof( particle ), &particle_cmp );
	while(i<n && particles[i].w>=L_WEIGHT)
	{
		i++;
	}
	if(i>=L_WEIGHT_NUM)
	{
		if(EvalOverlap(particles, i)>=OVERLAP_RATE)
		{
			printf("Do update? ");
			scanf("%d", &user);
			if(user==1)
			{
				return(1);
			}
		}
	}
	return(0);
}

double EvalOverlap(particle* particles, int n)
{
	int i;
	CvRect ref_rect, rect;
	double overlap=0;
	particle p;

	p=particles[0];
	ref_rect=cvRect(cvRound( p.x - 0.5 * p.sw * p.width ), cvRound( p.y - 0.5 * p.sh * p.height ),
					cvRound( p.sw * p.width ), cvRound( p.sh * p.height ));
	if(ref_rect.height<MIN_HEIGHT || ref_rect.width<MIN_WIDTH)	//the reference histogram area must be sufficiently large
	{
		return(0);
	}
	for(i=1; i<n; i++)
	{
		p=particles[i];
		rect=cvRect(cvRound( p.x - 0.5 * p.sw * p.width ), cvRound( p.y - 0.5 * p.sh * p.height ),
					cvRound( p.sw * p.width ), cvRound( p.sh * p.height ));
		overlap+=(double)CommonArea(&ref_rect, &rect);
	}
	return(overlap/(n-1)/(ref_rect.height*ref_rect.width));
}

int CommonArea(CvRect *rect1, CvRect *rect2)
{
	int left, right, top, bottom;

	left=max(rect1->x, rect2->x);
	right=min(rect1->x+rect1->width, rect2->x+rect2->width);
	if(right<=left) return(0);
	bottom=max(rect1->y, rect2->y);
	top=min(rect1->y+rect1->height, rect2->y+rect2->height);
	if(top<=bottom) return(0);
	return((top-bottom)*(right-left));
}

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

	zero.val[0]=0;
	one.val[0]=255;
	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)
			{
				rect.x=i-offsetm;
				rect.y=j-offsetm;
				cvGetSubArr(Mdiff, Msub, rect);
				element=cvAvg(Msub, NULL);
				if(element.val[0]>G)
				{
					cvSetAt(Mdiff_conctr, one, 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);
}

/* Input Ma, Mb must be single channeled */
CvPoint MotionCompensation(IplImage *Ma, IplImage *Mb, int w, int h, int DispL, int DispH)
{
	/* *MaSub, *MbSub, */
	CvMat *Mcorrel;
	IplImage *Mdiff;
	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*/);
	Mdiff=cvCreateImage(cvGetSize(Ma), 8, 1);	//cvCreateMat(h, w, CV_8UC1);

	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);
			cvSetImageROI(Ma, recta);
			cvSetImageROI(Mb, rectb);
			cvSetImageROI(Mdiff, rectb);

//			cvGetSubArr(Ma, MaSub, recta);
//			cvGetSubArr(Mb, MbSub, rectb);
			cvAbsDiff(Ma, Mb, 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);	//do not release memory?
//			cvReleaseMat(&MbSub);
//			cvReleaseMat(&Mdiff);
			cvResetImageROI(Ma);
			cvResetImageROI(Mb);
			cvResetImageROI(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);
	cvReleaseImage(&Mdiff);

	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;
	};
}