video_header.cpp

基于粒子滤波原理

#include "video_header.h"

int qqq=0;

//给定位置point和大小area，在特定图像*pimage中画矩形
IplImage* rectangle_drawing(IplImage* pimage, CvPoint point, CvSize area)
{
	if(point.x > area.width && point.x < pimage->width - area.width && point.y > area.height && point.y < pimage->height - area.height)
	{
		//using cvLine, more simpler

		cvLine(pimage, cvPoint(point.x - area.width, point.y - area.height), cvPoint(point.x + area.width, point.y - area.height), CV_RGB(255, 0, 0), 1, 0);
		cvLine(pimage, cvPoint(point.x - area.width, point.y - area.height), cvPoint(point.x - area.width, point.y + area.height), CV_RGB(255, 0, 0), 1, 0);
		cvLine(pimage, cvPoint(point.x + area.width, point.y + area.height), cvPoint(point.x + area.width, point.y - area.height), CV_RGB(255, 0, 0), 1, 0);
		cvLine(pimage, cvPoint(point.x + area.width, point.y + area.height), cvPoint(point.x - area.width, point.y + area.height), CV_RGB(255, 0, 0), 1, 0);
		//int rows = point.y-area.height;
		//for(int j=point.x-area.width;j<=point.x+area.width;j++)
		//pimage->imageData[pimage->nChannels*(rows*pimage->width+j)] = 255;
		//rows = point.y+area.height;
		//for(int j=point.x-area.width;j<=point.x+area.width;j++)
		//pimage->imageData[pimage->nChannels*(rows*pimage->width+j)] = 255;
		//int columes = point.x-area.width;
		//for(int i=point.y-area.height;i<=point.y+area.height;i++)
		//pimage->imageData[pimage->nChannels*(i*pimage->width+columes)] = 255;
		//columes = point.x+area.width;
		//for(int i=point.y-area.height;i<=point.y+area.height;i++)
		//pimage->imageData[pimage->nChannels*(i*pimage->width+columes)] = 255;
	}

	return pimage;
}
//给定图像，返回颜色直方图向量
CvHistogram* hist_calculation(IplImage* pimage, CvHistogram* hist, int histnum, float* histranges)
{
	hist = cvCreateHist( 1, &histnum, CV_HIST_ARRAY, &histranges, 1 );  // 创建直方图
	cvCalcHist( &pimage, hist, 0, 0 ); // 计算直方图
	
	return hist;
}
//画颜色直方图
IplImage* histogram_drawing(CvHistogram* hist, IplImage* histimage, int histnum)
{
	float max_val=0;
	cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 );  // 只找最大值
	cvZero( histimage );
	int bin_width = histimage->width / histnum;  // histnum: 条的个数，则 bin_w 为条的宽度
	for( int i = 0; i < histnum; i++ )
	{
		double val = ( cvGetReal1D(hist->bins,i)*histimage->height/max_val );
		CvScalar color = CV_RGB(255,255,0); //(hsv2rgb(i*180.f/histnum);
		cvRectangle( histimage, cvPoint(cvRound((double)i*bin_width),histimage->height),
			cvPoint((i+1)*bin_width,(int)(histimage->height - val)),color, 1, 8, 0 );
	}

	return histimage;
}

//从灰度图像中得到想要的一小块图像
IplImage* patchimage_getting(IplImage* pimage, IplImage* dstimage, CvPoint point, CvSize area)
{
	int k=0;
	//IplImage* dstimage = cvCreateImage(cvSize(2*area.width-1, 2*area.height-1), IPL_DEPTH_8U, 1);
	for(int i=point.y-area.height+1;i<=point.y+area.height-1;i++)
		for(int j=point.x-area.width+1;j<=point.x+area.width-1;j++)
		{
			dstimage->imageData[k]=pimage->imageData[i*pimage->width+j];
			k++;
		}

	return dstimage;
}
//计算Bhattacharyya距离
float bhattacharyya(CvHistogram* hist1, CvHistogram* hist2, int histnum)
{
	double sumhist1 = 0;
	double sumhist2 = 0;

	for(int i=0;i<histnum;i++)
	{
		sumhist1 += cvGetReal1D(hist1->bins,i);
	}

	for(int i=0;i<histnum;i++)
	{
		sumhist2 += cvGetReal1D(hist2->bins,i);
	}

	if(sumhist1==0 || sumhist2==0)
	{
		std::cout<<"sumhist = 0!!"<<"\n";
		return 0;
	}
			
	//////////计算difference巴特查里亚距离//////////////
	float sum=0;
	for(int i=0;i<histnum;i++)
	{
		sum += (float)sqrt(cvGetReal1D(hist1->bins,i)/sumhist1*cvGetReal1D(hist2->bins,i)/sumhist2);
	}

	return (1-sum);
}

//权值计算，未进行归一化
void CondProbDens(CvConDensation* CD,  float* Measurement, float exptect_dist)
{     
	float Prob = 1;
    float stdev = 1.0/(4*exptect_dist*exptect_dist);
    for(int i = 0; i < CD->SamplesNum;i++)
    {
		Prob =1;
		for(int j =0; j < CD->DP;j++)
		{
			// assume a guaddian prob guassian with given std.dev of.. If too small nothing will associate and you get 0 prob...
			Prob*=(float)exp(-stdev * (Measurement[j] - CD->flSamples[i][j])*(Measurement[j]-CD->flSamples[i][j]));
		}
		CD->flConfidence[i] = Prob;
	}
}
void ConDensWeightsCalculation(CvConDensation* CD, float bt, float exptect_dist)
{
	float stdev = -1.0/(4*exptect_dist*exptect_dist);
	for(int i=0;i<CD->SamplesNum;i++)
	{
		CD->flConfidence[i]=exp(stdev * bt * bt);
	}
}



//重采样
void resampling(CvConDensation* CD, CvMat* noise)
{
	int j = 0;
	for(int i=0;i<CD->SamplesNum;i++)
	{
		j = 0;
		while(noise->data.fl[i] > CD->flCumulative[j])
		{
			j++;
		}

		if(j < CD->SamplesNum)
		{
			CD->flNewSamples[i][0] = CD->flSamples[j][0];
			CD->flNewSamples[i][1] = CD->flSamples[j][1];
		}
	}

	for(int i=0;i<CD->SamplesNum;i++)
	{
		for(int j=0;j<CD->DP;j++)
		{
			CD->flSamples[i][j] = CD->flNewSamples[i][j];
		}
	}
}
//粒子滤波主过程，输出滤波结果--估计位置。
CvPoint PF_result(IplImage* pGrey, CvConDensation* CD, CvPoint state_prediction, CvHistogram* hist, CvSize area, int histnum, float* histranges, float exptect_dist, int steps, CvRandState rng)
{
	
	CvPoint prediction_position = cvPoint(0, 0);
	float bt=0;
	//float radius = 10;
	float sum = 0;
	IplImage* patch= cvCreateImage(cvSize(2*area.width-1, 2*area.height-1), IPL_DEPTH_8U, 1);
	CvHistogram* particlehist = NULL;
	//int stdev = -1.0/(4*exptect_dist*exptect_dist);
	
	CvMat* noise = cvCreateMat( CD->SamplesNum, 1, CV_32FC1 );
	//CvRandState RandS;
	//cvRandInit( &RandS, 0, 1, -4, CV_RAND_UNI );


	//for(int j=0;j<steps;j++)
	{
		sum = 0;
		for(int i=0;i<CD->SamplesNum;i++)
		{
			patch = patchimage_getting(pGrey, patch, cvPoint(cvRound(CD->flSamples[i][0]), cvRound(CD->flSamples[i][1])), area);
			particlehist = hist_calculation(patch, particlehist, histnum, histranges);
			bt = bhattacharyya(hist, particlehist, histnum);
			CD->flConfidence[i] = exp(-20 * bt);
			sum += CD->flConfidence[i];
		}
		//normalize the weights
		CD->flConfidence[0] /= sum;
		CD->flCumulative[0] = CD->flConfidence[0];
		for(int i=1;i<CD->SamplesNum;i++)
		{
			CD->flConfidence[i] /= sum;
			CD->flCumulative[i] = CD->flCumulative[i-1] + CD->flConfidence[i];

		}

		for(int i=0;i<CD->SamplesNum;i++)
		{
			std::cout<<"\n"<<CD->flSamples[i][0]<<"  "<<CD->flConfidence[i]<<"\n";
		}


		cvRandSetRange(&rng, 0, 1, 0 );
		rng.disttype = CV_RAND_UNI;
		cvRand(&rng, noise);
		resampling(CD, noise);

		for(int i=0;i<CD->SamplesNum;i++)
		{
			std::cout<<"\n"<<CD->flSamples[i][0]<<"  ";
		}
		
		//update particles加重采样程序时，这一段需要重新写
		cvRandSetRange(&rng, -10, 10, 0);
		rng.disttype = CV_RAND_UNI;
		for(int i=0;i<CD->DP;i++)
		{
			cvRand(&rng, noise);
			for(int j=0;j<CD->SamplesNum;j++)
			{
				CD->flSamples[j][i] += noise->data .fl[j];
			}
		}
	}

	//prediction		
	//normalize the weights
	float position_x = 0, position_y = 0;
	for(int i=0;i<CD->SamplesNum;i++)
	{
		//CD->flConfidence[i] /= sum;
		position_x += CD->flSamples[i][0] / CD->SamplesNum;
		position_y += CD->flSamples[i][1] / CD->SamplesNum;
	}


	prediction_position.x = cvRound(position_x);
	prediction_position.y = cvRound(position_y);

	cvReleaseImage(&patch);

	return prediction_position;
}