三维高斯模型 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;
}