stdafx.cpp

基于粒子滤波原理

// stdafx.cpp : source file that includes just the standard includes
// video2.pch will be the pre-compiled header
// stdafx.obj will contain the pre-compiled type information

#include "stdafx.h"


//给定位置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);
	}

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

//欧几里德距离计算
float eujilide(CvMat* Model_WenLi, CvMat* WenLi)
{
	float et = 0;
	float sum =0;
	//std::cout<<"\n\nWenLi\n";
	for(int i=0;i<36;i++)
	{
		//std::cout<<Model_WenLi->data.fl[i]<<"    "<<WenLi->data.fl[i]<<"\n";
		sum += pow(Model_WenLi->data.fl[i]-WenLi->data.fl[i], 2);
	}
	et = sqrt(sum);

	return et;
}
//权值计算，未进行归一化
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, CvMat* FuzhiMat, CvMat* FangxiangMat, \
				  CvMat* Model_WenLi, CvMat* WenLi, float exptect_dist, int steps, CvRandState rng)
{
	
	CvPoint prediction_position = cvPoint(0, 0);
	float bt=0, et=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;
	
	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);

			//根据纹理特征计算可能性大小
			grads_calculation(patch, FuzhiMat, FangxiangMat);
			wen_li_statistic(FuzhiMat, FangxiangMat, WenLi);
			et = eujilide(Model_WenLi, WenLi);
			CD->flConfidence[i] = exp(-10 * et);

			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);
		
		//update particles
		cvRandSetRange(&rng, -10, 10, 0);	//（-10， 10）这个范围的设计要根据目标的速度来决定，
											//原则就是：保证粒子范围能够覆盖目标下一步可能出现的位置
		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);
	cvReleaseHist( &particlehist );
	cvReleaseMat( &noise);

	return prediction_position;
}
//纹理统计
void wen_li_statistic(CvMat* FuzhiMat, CvMat* FangxiangMat, CvMat* WenLi)
{
	cvZero(WenLi);
	float sum = 0;
 	for(int i=1;i<FuzhiMat->cols-1;i++)
		for(int j=1;j<FuzhiMat->rows-1;j++)
		{
			WenLi->data.fl[cvFloor((FangxiangMat->data.fl[j*FangxiangMat->cols+i] * 180 / pi + 180 ) / 10)] += \
				FuzhiMat->data.fl[j*FuzhiMat->cols+i]; 
		}

	for(int i=0;i<36;i++)
	{
		sum += WenLi->data.fl[i] * WenLi->data.fl[i];
	}
	sum = sqrt(sum);
	for(int i=0;i<36;i++)
	{
		WenLi->data.fl[i] /= sum;
	}
}


////梯度计算
void grads_calculation(IplImage* pImgs, CvMat* FuzhiMat, CvMat* FangxiangMat)
{	
	float mask_x[9] = { -1, 0, 1, \
						-1, 0, 1, \
						-1, 0, 1};
	float mask_y[9] = { -1, -1, -1, \
						0, 0, 0, \
						1, 1, 1};
	float dx=0,dy=0;
	for(int i=1;i<pImgs->width-1;i++)
	for(int j=1;j<pImgs->height-1;j++)
	{				
		dx = ((mask_x[2] * pImgs->imageData[(j-1) * pImgs->width + i + 1] + mask_x[0] * pImgs->imageData[(j-1) * pImgs->width + i - 1]) \
			+ (mask_x[5] * pImgs->imageData[j * pImgs->width + i + 1] + mask_x[3] * pImgs->imageData[j * pImgs->width + i - 1])  \
			+ (mask_x[8] * pImgs->imageData[(j+1) * pImgs->width + i + 1] + mask_x[6] * pImgs->imageData[(j+1) * pImgs->width + i - 1]))/6;
		dy = ((mask_y[6] * pImgs->imageData[(j+1) * pImgs->width + i - 1] + mask_y[0] * pImgs->imageData[(j-1) * pImgs->width + i - 1]) \
			+ (mask_y[7] * pImgs->imageData[(j+1) * pImgs->width + i] + mask_y[1] * pImgs->imageData[(j-1) * pImgs->width + i])  \
			+ (mask_y[8] * pImgs->imageData[(j+1) * pImgs->width + i + 1] + mask_y[2] * pImgs->imageData[(j-1) * pImgs->width + i + 1]))/6;
		
		FuzhiMat->data.fl[j * FuzhiMat->cols + i] = sqrt(dx*dx + dy*dy);
		FangxiangMat->data.fl[j * FangxiangMat->cols + i] = atan2(dy, dx);

		//std::cout<<FuzhiMat->data.fl[j * FuzhiMat->cols + i]<<"    "<<FangxiangMat->data.fl[j * FangxiangMat->cols + i]<<"\n";
	}
}

// TODO: reference any additional headers you need in STDAFX.H
// and not in this file