stdafx.cpp

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

	int SamplesNumWhenUnsheltered = CD->SamplesNum / steps;
	//IplImage* patch= cvCreateImage(cvSize(2*area.width-1, 2*area.height-1), IPL_DEPTH_8U, 3);
	CvMat* ColorHistMat = cvCreateMat(36, 1, CV_32FC1);
	CvMat* noise = cvCreateMat( CD->SamplesNum, 1, CV_32FC1 );

	if (*pshelter == 0)
	{
		//UnshelteredFrameNum++;
		for(int step=0; step < steps; step++)
		{
			sum = 0;
			//PredictionStateConfidence = 0
			for(int i=0;i<SamplesNumWhenUnsheltered;i++)
			{
				if(CD->flSamples[i][0]-area.width+1<0 || CD->flSamples[i][0]+area.width-1>pFrame->width || \
					CD->flSamples[i][1]-area.height+1<0 || CD->flSamples[i][1]+area.height-1>pFrame->height)
					continue;//当粒子出界时，给予抛弃
				cvSetImageROI(pFrame, cvRect(cvRound(CD->flSamples[i][0]) - area.width + 1, cvRound(CD->flSamples[i][1]) - area.height + 1, 2*area.width-1, 2*area.height-1));
				ColorHistMat = CalcColorHist(pFrame, ColorHistMat);
				bt = bhattacharyya_color(ModelColorHistMat, ColorHistMat);
				CD->flConfidence[i] = exp(-20 * bt);
				sum += CD->flConfidence[i];
				if(CD->flConfidence[i] > PredictionStateConfidence)
				{
					PredictionStateConfidence = CD->flConfidence[i];
					//用权值最大的粒子的位置去最为目标的位置
					prediction_position = cvPoint(CD->flSamples[i][0], CD->flSamples[i][1]);
				}

				cvResetImageROI(pFrame);
			}

			//to plot the position of the particles with the weights as their size]
			if(step == 0)
				for(int i=0;i<SamplesNumWhenUnsheltered;i++)
				{
					draw_plus(pFrame, cvPoint(CD->flSamples[i][0], CD->flSamples[i][1]), CV_RGB(255, 255, 255), CD->flConfidence[i]*10); 
				}

			//normalize the weights
			//权值归一化是为了重采样，否则没必要进行归一化
			CD->flConfidence[0] /= sum;
			CD->flCumulative[0] = CD->flConfidence[0];
			for(int i=1;i<SamplesNumWhenUnsheltered;i++)
			{
				CD->flConfidence[i] /= sum;
				CD->flCumulative[i] = CD->flCumulative[i-1] + CD->flConfidence[i];

			}

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

			cvRandSetRange(&rng, 0, 2, 0);//加小扰动，用来逼近真实状态！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
			rng.disttype = CV_RAND_NORMAL;
			for(int i=0;i<CD->DP;i++)
			{
				cvRand(&rng, noise);
				for(int k=0;k<SamplesNumWhenUnsheltered;k++)
				{
					CD->flSamples[k][i] += noise->data.fl[k];
				}
			}

		}	//end of for(int step=0; step < steps; step++)
		
		//update particles
		cvRandSetRange(&rng, -20, 20, 0);//！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
		rng.disttype = CV_RAND_UNI;
		for(int i=0;i<CD->DP;i++)
		{
			cvRand(&rng, noise);
			for(int j=0;j<SamplesNumWhenUnsheltered;j++)
			{
				CD->flSamples[j][i] += noise->data .fl[j];
			}
		}
	}
	else
	{
		//ShelteredFrameNum++;
		cvRandSetRange(&rng, -100, 100, 0);//！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
		rng.disttype = CV_RAND_UNI;

		cvRand(&rng, noise);
		for(int j=0;j<CD->SamplesNum;j++)
		{
			CD->flSamples[j][0] = state_prediction.x + noise->data .fl[j];
		}
		cvRand(&rng, noise);
		for(int j=0;j<CD->SamplesNum;j++)
		{
			CD->flSamples[j][1] = state_prediction.y + noise->data .fl[j];
		}


		sum = 0;
		//PredictionStateConfidence = 0
		for(int i=0;i<CD->SamplesNum;i++)
		{
			if(CD->flSamples[i][0]-area.width+1<0 || CD->flSamples[i][0]+area.width-1>pFrame->width || \
				CD->flSamples[i][1]-area.height+1<0 || CD->flSamples[i][1]+area.height-1>pFrame->height)
				continue;//当粒子出界时，给予抛弃
			cvSetImageROI(pFrame, cvRect(cvRound(CD->flSamples[i][0]) - area.width + 1, cvRound(CD->flSamples[i][1]) - area.height + 1, 2*area.width-1, 2*area.height-1));
			ColorHistMat = CalcColorHist(pFrame, ColorHistMat);
			bt = bhattacharyya_color(ModelColorHistMat, ColorHistMat);
			CD->flConfidence[i] = exp(-20 * bt);
			sum += CD->flConfidence[i];
			if(CD->flConfidence[i] > PredictionStateConfidence)
			{
				PredictionStateConfidence = CD->flConfidence[i];
				//用权值最大的粒子的位置去作为目标的位置
				prediction_position = cvPoint(CD->flSamples[i][0], CD->flSamples[i][1]);
			}

			cvResetImageROI(pFrame);
		}

		//to plot the position of the particles with the weights as their size]
		for(int i=0;i<CD->SamplesNum;i++)
		{
			draw_plus(pFrame, cvPoint(CD->flSamples[i][0], CD->flSamples[i][1]), CV_RGB(255, 255, 255), CD->flConfidence[i]*10); 
		}
	}


	//prediction		

	//此段代码为用所有粒子的加权平均去估计目标的位置
	//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);

	//to define weather the target is sheltered
	if(PredictionStateConfidence < 0.3)
	{
		*pshelter = 1;
		prediction_position = state_prediction;
	}
	else
	{
		*pshelter = 0;
	}
	//to update the ModelColorHistMat
	cvSetImageROI(pFrame, cvRect(prediction_position.x, prediction_position.y, 2*area.width-1, 2*area.height-1));
	ColorHistMat = CalcColorHist(pFrame, ColorHistMat);
	ColorModelUpdate(ModelColorHistMat, ColorHistMat);
	cvResetImageROI(pFrame);


	cvReleaseMat(&ColorHistMat);
	cvReleaseMat(&noise);


	return prediction_position;
}

//to update color-histogram
//现根据直方图算出更新之后的直方图，然后生成相应的图片结构，在对图片结构计算直方图，
//以解决不能对直方图进行写操作的问题
//void ColorModelUpdate(CvHistogram* dsthist, CvHistogram* prediction_hist, int histnum, CvSize area)
//{
//	float alpha = 1.0f;
//	float belta = 1.0f - alpha;
//	int j=0, m=0;
//	IplImage* pImage = cvCreateImage(cvSize(2*area.width-1, 2*area.height-1), IPL_DEPTH_8U, 1);
//	//CvMat* tempMat = cvCreateMat(2*area.height-1, 2*area.width-1, CV_8UC1);
//	for(int i=0;i<histnum;i++)
//	{
//		std::cout<<"    "<</*"dsthist:"<<*/cvGetReal1D(dsthist->bins, i)/*<<"    predictionhist:"<<cvGetReal1D(prediction_hist->bins, i)*/;
//		j += cvFloor(alpha*cvGetReal1D(dsthist->bins , i) + belta*cvGetReal1D(prediction_hist->bins, i));
//		//std::cout<<"    j["<<i<<"]="<<j-m<<"  "<<"\n";
//		for(int k=m;k<j;k++)
//		{
//			//if(i>33)
//			//	pImage->imageData[k] = i;
//			//else
//				pImage->imageData[k] = i;
//			//std::cout<<i*5+2<<"    ";
//		}
//		m=j;
//	}
//
//	std::cout<<"\n";
//
//	cvCalcHist(&pImage, dsthist, 0, 0);
//	for(int i=0;i<histnum;i++)
//	{
//		std::cout<<cvGetReal1D(dsthist->bins, i)<<"    ";
//	}
//
//	cvReleaseImage(&pImage);
//
//}


//上面那种方法真是傻到家啦，只需将直方图的数据读出，放在一个向量里面，然后用向量进行更新
//再根据向量计算bhattacharyya距离就行了吗！
void ColorModelUpdate(CvMat* ModelColorHistMat, CvMat* PredictionStateColorHistMat)
{
	float alpha = 1.0f;
	float belta = 1 - alpha;
	for(int i=0;i<ModelColorHistMat->rows;i++)
	{
		ModelColorHistMat->data.fl[i] = alpha*ModelColorHistMat->data.fl[i] + belta*PredictionStateColorHistMat->data.fl[i];
	}
}
//some useful programs
//{
//	//to get the hist_img_b/g/r
//	for( int b = 0; b < b_bins; b++ )
//    {
//            float bin_val = cvQueryHistValue_1D( hist_b, b );
//			int val = cvRound(bin_val*hist_img_b->height/b_max);
//			cvRectangle( hist_img_b, cvPoint( b*scale, hist_img_b->height ),
//				cvPoint( (b+1)*scale, hist_img_b->height - val), CV_RGB(255,255,0), CV_FILLED );
//    }
//	for( int g = 0; g < g_bins; g++ )
//    {
//            float bin_val = cvQueryHistValue_1D( hist_g, g );
//			int val = cvRound(bin_val*hist_img_g->height/g_max);
//			cvRectangle( hist_img_g, cvPoint( g*scale, hist_img_g->height ),
//				cvPoint( (g+1)*scale, hist_img_g->height - val), CV_RGB(255,255,0), CV_FILLED );
//    }
//	for( int r = 0; r < r_bins; r++ )
//    {
//            float bin_val = cvQueryHistValue_1D( hist_r, r );
//			int val = cvRound(bin_val*hist_img_r->height/r_max);
//			cvRectangle( hist_img_r, cvPoint( r*scale, hist_img_r->height ),
//				cvPoint( (r+1)*scale, hist_img_r->height - val), CV_RGB(255,255,0), CV_FILLED );
//    }
//
//
//}