三维高斯模型 opencv实现

OnProbabilityModel()
{
	int i;
	for(int x=0;x<workImg->height;x++)
	{
		for(int y=0;y<workImg->width;y++)
		{
			//double cur[3];
			CvMat* cur=cvCreateMat(3,1,CV_32F);
			for(i=0;i<3;i++){
				double tt=((uchar*)(workImg->imageData+x*workImg->widthStep))[y*3+i];
				cvmSet(cur,i,0,tt);
			}
			CvMat dst=cvRGB2YCbCr(cur);
			if(CalProbability(WHITE,&dst)<0.1&&CalProbability(YELLOW,&dst)<0.1)
				for (i=0;i<3;i++)
				((uchar*)(workImg->imageData+x*workImg->widthStep))[y*3+i]=0;
		}
	}
	Invalidate();
}

double CalProbability(int classid,CvMat* cur)
{
	/************************************************************************/
	/* function:
	一个像素点cur[3]={r,g,b}; 它属于classid色类的概率
	*/
	/************************************************************************/
	double temp,t1;

	 CvMat inv_w,inv_y;
	 cvInitMatHeader(&inv_w,3,3,CV_32F,Inv_white);
	 cvInitMatHeader(&inv_y,3,3,CV_32F,Inv_yellow);
	 CvMat* tmp=cvCreateMat(1,3,CV_32F);
	 CvMat* tmp1=cvCreateMat(1,3,CV_32F);
	 CvMat* res=cvCreateMat(1,1,CV_32F);

	//double tmp[3][3],tmp1[3][3];
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	temp=1/pow(2*PI,3/2)/sqrt(norm[classid]);
	//cvmGetMat()

	for (i=0;i<3;i++)	{
		double x=cvmGet(cur,i,0);
		x-=mean_ycbcr[classid][i];
		if(x<0)
			x=0;
		cvmSet(cur,i,0,x);
	}

	double c1=cvmGet(cur,0,0);
	double c2=cvmGet(cur,1,0);
	double c3=cvmGet(cur,2,0);

	cvTranspose(cur,tmp);//转置

	if(classid==WHITE)
		cvmMul(tmp,&inv_w,tmp1);
	else if(classid==YELLOW)
		cvmMul(tmp,&inv_y,tmp1);

	cvmMul(tmp1,cur,res);
	//t1=cvNorm(tmp,0,CV_L1,0);
	t1=cvmGet(res,0,0);
	t1*=(-0.5);
	temp*=pow(Ezhishu,t1);

	return temp;
}