stdafx.cpp

在粒子滤波+颜色直方图+遮挡处理的基础上

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

//从彩色图像中得到目标小图像
IplImage* color_patchimage_getting( IplImage* pFrame, IplImage* dstimage, CvPoint point, CvSize area)
{
	for(int ii=0, i=point.y-area.height+1;ii<2*area.height-1 && i<=point.y+area.height-1;ii++,i++)
		for(int jj=0, j=point.x-area.width+1;jj<2*area.width-1 && j<=point.x+area.width-1;jj++, j++)
			for(int k=0;k<3;k++)
			{
				dstimage->imageData[(ii*dstimage->widthStep+3*jj)+k] = pFrame->imageData[(i*pFrame->widthStep+3*j) + k];
			}

	return dstimage;
 }

//to get the color histogram mat
CvMat* CalcColorHist(IplImage* pImage, CvMat* ColorHistMat)
{
	IplImage* b_plane = cvCreateImage( cvGetSize(pImage), 8, 1 );
	IplImage* g_plane = cvCreateImage( cvGetSize(pImage), 8, 1 );
	IplImage* r_plane = cvCreateImage( cvGetSize(pImage), 8, 1 );
	
	int b_bins = 12, g_bins = 12, r_bins = 12;

    float b_ranges[] = { 0, 255 };
	float* pb_ranges = b_ranges;
    float g_ranges[] = { 0, 255 };
	float* pg_ranges = g_ranges;
	float r_ranges[] = { 0, 255 };
	float* pr_ranges = r_ranges;
		
    cvCvtPixToPlane( pImage, b_plane, g_plane, r_plane, 0 );
    CvHistogram* hist_b = cvCreateHist( 1, &b_bins, 0, &pb_ranges, 1 );
	CvHistogram* hist_g = cvCreateHist( 1, &g_bins, 0, &pg_ranges, 1 );
	CvHistogram* hist_r = cvCreateHist( 1, &r_bins, 0, &pr_ranges, 1 );

    cvCalcHist( &b_plane, hist_b, 0, 0 );
	cvCalcHist( &g_plane, hist_g, 0, 0 );
	cvCalcHist( &r_plane, hist_r, 0, 0 );

	for(int i=0;i<ColorHistMat->rows / 3;i++)
	{
		ColorHistMat->data.fl[i] = cvGetReal1D(hist_b->bins, i);
		ColorHistMat->data.fl[i+ColorHistMat->rows / 3] = cvGetReal1D(hist_g->bins, i);
		ColorHistMat->data.fl[i+2*ColorHistMat->rows/3] = cvGetReal1D(hist_r->bins, i);
	}

	cvReleaseImage(&b_plane);
	cvReleaseImage(&g_plane);
	cvReleaseImage(&r_plane);
	cvReleaseHist(&hist_b);
	cvReleaseHist(&hist_g);
	cvReleaseHist(&hist_r);

	return ColorHistMat;
}

//计算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 bhattacharyya_color(CvMat* ModelColorHistMat, CvMat* ColorHistMat)
{
	double sumhist1 = 0;
	double sumhist2 = 0;

	for(int i=0;i<ModelColorHistMat->rows;i++)
	{
		sumhist1 += ModelColorHistMat->data.fl[i];
	}

	for(int i=0;i<ColorHistMat->rows;i++)
	{
		sumhist2 += ColorHistMat->data.fl[i];
	}

	if(sumhist1==0 || sumhist2==0)
	{
		std::cout<<"sumhist = 0!!"<<"\n";
		return 0;
	}
			
	//////////计算difference巴特查里亚距离//////////////
	float sum=0;
	for(int i=0;i<ModelColorHistMat->rows;i++)
	{
		sum += sqrt(ModelColorHistMat->data.fl[i]/sumhist1*ColorHistMat->data.fl[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* pFrame, CvConDensation* CD, CvPoint state_prediction, \
				  float vx, float vy, CvMat* ModelColorHistMat, CvSize area, \
				  int steps, CvRandState rng, int* pshelter)
{
	
	CvPoint prediction_position = cvPoint(0, 0);
	//float ConfidenceThresh = 0.9f;//不一定每一次都要进行5次重采样，当最大权值大于某个门限的时候即可认为已经找到准确的位置
									//而不用继续重采样进行逼近
	float bt = 0;
	float sum = 0;
	float PredictionStateConfidence = 0;