grimsonbg.cpp

人体运动跟踪 混合高斯模型＋GRISON方法

        	
	}
   else if(m_sdDataType==DATA_NORM_RGB){
    // Data is intensity normalised RGB
        double tot = 0 ;
        for(y_cnt=0 ; y_cnt<m_biHeight ; y_cnt+=m_biInrY){
            for(x_cnt=0 ; x_cnt<m_biWidth ; x_cnt+=m_biInrX, p_cnt++, pix_ptr++){

                d_off = (y_cnt * m_biWidth + x_cnt) * 3 ;

                red_d = (double)data[d_off] ;
                tot= red_d ;
                green_d = (double)data[d_off+1] ;
                tot+= green_d ;
                blue_d = (double)data[d_off] ;
                tot+= blue_d ;

                data_d[0] = red_d / tot ;
                data_d[1] = green_d / tot ;
                data_d[2] = blue_d / tot ;

                if(!InMixture(data_d, pix_ptr,true)){
                    fg[(y_cnt * m_biWidth + x_cnt)] = 255 ;
                }
                else{
                    fg[(y_cnt * m_biWidth + x_cnt)] = 0 ;
                }
            }
        }
    }
   fg_ptr=fg;
   //形态学滤波
    //VerticalMorp(fg_ptr,1);
  //利用区域连通标识图像
//	CImage img(m_biWidth,m_biHeight,fg_ptr);
//	img.MarkImage(255);
//	img.CreateShowMark();
//	memcpy(fg_ptr,img.m_lpShowMark,sizeof(unsigned char)*m_biWidth*m_biHeight);
}                                                                               

inline void GrimsonBG::UpdateMixture(double *data, PixelInfo *pi)
{
// Updates the mixture m_pModel using data

    Gaussian   **g ;
    Gaussian   **g_ptr ;
    Gaussian   *g_ptr_i ;
    Gaussian   *g_ptr_i2 ;
    int g_cnt ;
    double       p ;
    double       p_i ;
    double       w_tot = 0 ;
    double       w_cum = 0 ;
    register double manh_min = HUGE_VAL ;
    register double manh_d ;
    bool         fits_m = true;
    double       *d_ptr ;
    double       *m_ptr   ;
    register double new_val ;
	int div =0;
    g = pi->gaussians ;

    g_ptr = g ;
    if(pi->frames_since_fg==1)
	{
       fits_m = false; 
	}
    if(!fits_m){
    // Does not fit any mixture, replace last mixture with one with mean
    // at data
        g_ptr_i = g[m_biGaussiansPerMix-1] ;
        g_ptr_i->mean[0] = data[0] ;
        g_ptr_i->mean[1] = data[1] ;
        g_ptr_i->mean[2] = data[2] ;
        g_ptr_i->weight = INITIAL_WEIGHT ;
        g_ptr_i->covariance = INIT_VAR ;
        for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix ; g_cnt++){
            w_tot += g[g_cnt]->weight ;
        }
    }
    else{
    // Update mixture
    // Update weights
    for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix; g_cnt++, g_ptr++)
	{
        manh_d = MahalanobisDist(data, *g_ptr) ;
        if(manh_d<MANH_THRESH_SQ&&manh_d<manh_min)
		{
			manh_min = manh_d ;
            g_ptr_i = *g_ptr ;
			pi->belong[g_cnt]++;
			div = pi->belong[g_cnt];
		}
	}
// if(g_ptr_i->weight < MAX_MIXTURE_PROB  )
 {
//    if(frames<INITFRAME)
//	 {
//       	 g_ptr = g ;
//        for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix ; g_cnt++, g_ptr++)
//		{
//           g_ptr_i2 = *g_ptr ;
//                if(g_ptr_i2 == g_ptr_i )
//				{
//                    new_val = g_ptr_i2->weight +(1-g_ptr_i2->weight)/(frames+1);
//                    g_ptr_i2->weight = new_val ;
//					  // Update mean
//                    d_ptr = data ;
//                    m_ptr = g_ptr_i->mean ;
//					new_val = m_ptr[0]+ (d_ptr[0]-m_ptr[0])/div ;
//					m_ptr[0] = new_val ; 
//					new_val = m_ptr[1] + (d_ptr[1]- m_ptr[1] )/div ;
//					m_ptr[1]= new_val ; 
//					new_val = m_ptr[2] +  (d_ptr[2]-m_ptr[2])/div ;
//					m_ptr[2] = new_val ; 
//                    g_ptr_i->covariance +=  (g_ptr_i->ss-g_ptr_i->covariance)/div;					
//                }
//                else
//				{
//                    g_ptr_i2->weight -= g_ptr_i2->weight/(frames+1) ;
//                }
//			 w_tot += g_ptr_i2->weight ;
//		}
//	}
//  else
//  {
    if(g_ptr_i->weight < /*MAX_MIXTURE_PROB*/1  ){
            g_ptr = g ;
            for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix ; g_cnt++, g_ptr++){
                g_ptr_i2 = *g_ptr ;
                if(g_ptr_i2 == g_ptr_i)
				{
                    if(g_cnt>pi->no_in_bg)
					{
                      m_dAlpha=0.001;
					  m_dAlphai = 1-m_dAlpha;
					}
					new_val = g_ptr_i2->weight * m_dAlphai + m_dAlpha ;
                    g_ptr_i2->weight = new_val ;
                }
                else{
                    g_ptr_i2->weight *= m_dAlphai ;
                }
                w_tot += g_ptr_i2->weight ;
            }
      }
        else{
           w_tot = 1.0 ;
      }
        g_ptr = g ;       
        // Calc update factor
        p = m_dAlpha * CalculateGaussian(data, g_ptr_i) ;
        if(p>0.1) p=0.1 ; 
        p_i = 1.0 - p ;

        // Update mean
        d_ptr = data ;
        m_ptr = g_ptr_i->mean ;

        new_val = ((*m_ptr) * p_i) + (p * *(d_ptr++)) ;
        *(m_ptr++) = new_val ; 
        new_val = ((*m_ptr) * p_i) + (p * *(d_ptr++)) ;
        *(m_ptr++) = new_val ; 
        new_val = ((*m_ptr) * p_i) + (p * *d_ptr) ;
        *m_ptr = new_val ; 

        // Update Covariance
        g_ptr_i->covariance *= p_i ;
        g_ptr_i->covariance += p * g_ptr_i->ss ;
    }
     for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix ; g_cnt++)
	{
          g[g_cnt]->weight /= w_tot ; 
			
	}
    SortGaussians(g) ;
    for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix ; g_cnt++){
        g[g_cnt]->weight /= w_tot ; 
        w_cum += g[g_cnt]->weight ;
        if(w_cum>m_dBgdProp){
            pi->no_in_bg = g_cnt+1 ;
            break ;
        }
    }
}

}

bool GrimsonBG::InMixture(double *data, PixelInfo *pi,bool step)
{
// Updates the mixture m_pModel using data
    Gaussian   **g ;
    Gaussian   **g_ptr ;
    int g_cnt ;
	int number;
    g = pi->gaussians ;
    g_ptr = g ;	
    if(step == true )
    {
		number =pi->no_in_bg;
	}
	else
	{
		number = m_biGaussiansPerMix;
	}
	for(g_cnt=0 ; g_cnt<number ; g_cnt++, g_ptr++)
	{
		if(MahalanobisDist(data, *g_ptr) < MANH_THRESH_SQ)
		{ 

			return true ;
		}
	}
    return false ;
}

void GrimsonBG::PlotBestMixture(unsigned char* data)
{
// Plots best mixture to data

     int x_cnt ;
     int y_cnt ;
    unsigned int p_cnt = 0 ;
    unsigned int d_off ;
    PixelInfo    *pix_ptr ;

    pix_ptr = m_pModel ;

    // Data is raw RGB
    for(y_cnt=0 ; y_cnt<m_biHeight ; y_cnt+=m_biInrY){
        for(x_cnt=0 ; x_cnt<m_biWidth ; x_cnt+=m_biInrX, p_cnt++, pix_ptr++){

            d_off = (y_cnt * m_biWidth + x_cnt) * 3 ;

            data[d_off]   = (unsigned char)(pix_ptr->gaussians)[0]->mean[0]  ;
            data[d_off+1] = (unsigned char)(pix_ptr->gaussians)[0]->mean[1] ;
            data[d_off+2] = (unsigned char)(pix_ptr->gaussians)[0]->mean[2]  ;
        }
    }

}

inline double GrimsonBG::MahalanobisDist(double *d, Gaussian *g)
{
// Returns the _square_ of the mahalanobis distance
    register double m_biInrX ;
    register double m_biInrY ;
    register double dz ;
    register double sum_sq ;
    register double *mn ;
    register double *dd ;

    mn = g->mean ;
    dd = d ;

    m_biInrX = *(dd++) - *(mn++) ;
    m_biInrX *= m_biInrX ;

    m_biInrY = *(dd++) - *(mn++) ;
    m_biInrY *= m_biInrY ;

    dz = *dd - *mn ;
    dz *= dz ;

    sum_sq = (m_biInrX + m_biInrY + dz) ;
    g->ss = sum_sq ;

    return (sum_sq / g->covariance) ;
}

void GrimsonBG::SetAlpha(double a)
{
// Sets the value of the update constant
    m_dAlpha = a ;
    m_dAlphai = 1.0 - m_dAlpha ;
}

bool GrimsonBG::isShadow(double *data, PixelInfo *pi)
{
	Gaussian   **g ;
    Gaussian   **g_ptr ;
    double data1[3],data2[3];
    g = pi->gaussians ;
	g_ptr = g ;
	if(m_bShadow==true)
	   m_bShadow=false;
    data1[0]=data[2]+data[1]+data[0];
	data1[1]=data[2]/data1[0];
	data1[2]=data[1]/data1[0];
	data2[0]=(*g_ptr)->mean[2]+(*g_ptr)->mean[1]+(*g_ptr)->mean[0];
	data2[1]=(*g_ptr)->mean[2]/data2[0];
	data2[2]=(*g_ptr)->mean[1]/data2[0];
	if(data1[0]/data2[0]>=0.6&&data1[0]/data2[0]<1&&abs(data1[1]-data2[1])<0.005&&abs(data1[2]-data2[2])<0.005&&data[0]/(*g_ptr)->mean[0]>data[1]/(*g_ptr)->mean[1]
		&&data[0]/(*g_ptr)->mean[0]>data[1]/(*g_ptr)->mean[1])
	{
		m_bShadow=true;
		return true;
	}
	else
		return false;
}
int GrimsonBG::GetAverage(unsigned char *data,int height,int width)
{
	int x,y,sum[3],pixelnum;
	 pixelnum = height*width;
	for( x =0;x<3;x++)
	{
		sum[x]=0;
	}
	for( x=0; x<width; x++ )
	{
		for( y=0; y<height; y++ )
		{
          sum[0]+=*(data++);
		  sum[1]+= *(data++);
		  sum[2]+= *(data);
		}
		data++;
	}
    sum[0]/=pixelnum;
	sum[1]/=pixelnum;
	sum[2]/=pixelnum;
   return (sum[0]+sum[1]+sum[2])/3;
}

void GrimsonBG::VerticalMorp(unsigned char *Source,bool flag)
{
   //对每一帧视频图像做形态学操作
	unsigned char Morp_Temp[320*240];
	IplImage *source1, *destination1;
	IplImage *img;
	img = iplCreateImageHeader(1,0,8,"GRAY","GRAY",  IPL_DATA_ORDER_PIXEL,     
                                  IPL_ORIGIN_BL, IPL_ALIGN_DWORD, m_biWidth, m_biHeight, 
								  0, 0,0,0);             
    img->imageData=img->imageDataOrigin=(char*)Source;
	source1 = img;
	img = iplCreateImageHeader(1,0,8,"GRAY","GRAY",  IPL_DATA_ORDER_PIXEL,     
                                  IPL_ORIGIN_BL, IPL_ALIGN_DWORD, m_biWidth, m_biHeight, 
								  0, 0,0,0);             
    img->imageData=img->imageDataOrigin=(char*)Morp_Temp;
	destination1 = img;
	   iplErode( source1,destination1, 1);
		iplDilate(destination1,source1,1);


	iplDeallocate(source1, IPL_IMAGE_HEADER);
	iplDeallocate(destination1, IPL_IMAGE_HEADER);

}



void GrimsonBG::Label(BYTE *Source, BYTE *Result, int w, int h, short *region_info, CRect *position)
{
		int jj, segindex, ce;
	int i, j, aa, bb, cc, collx;
	unsigned int TempBuffer[352][288], TempBuffer1[352][288];

	BOOL	recorded=FALSE;
	WORD	coll[1000][2];
	WORD	ttt;
	segindex=1;
	collx=0;

	//////// if the value > threshold , set it 1
	for (j=0; j<m_biHeight; j++)
	{
		for (int i=0; i<m_biWidth; i++)
		{
			if (Source[i+j*m_biWidth] >=1) 
				TempBuffer1[i][j]=1;
			else 
				TempBuffer1[i][j]=0;
		}
	}
//////////////////////////////////////////////////////////////////////////////////////////////////
// Morphological operation
//		a
//		b
//		c   (b = 0 ) , (a !=0) , (c!=0)
//////////////////////////////////////////////////////////////////////////////////////////////////

		memcpy(TempBuffer, TempBuffer1, sizeof(TempBuffer1));
		
		for (j=1; j<m_biHeight; j++)
			for (i=1; i<m_biWidth; i++)
			{
				//		a
				//	b	c
				aa=TempBuffer[i][j-1];//InsVfw.Dis_buff[x+y-w+2];
				bb=TempBuffer[i-1][j];//bb=InsVfw.Dis_buff[x-3+y+2];				
				cc=TempBuffer[i][j];//cc=InsVfw.Dis_buff[x+y+2];					
			
				if ((i==1)&&(bb!=0))
				{
					if (TempBuffer[0][j-1]==0) 
					{
						segindex=segindex+1;
						TempBuffer[0][j]=(SHORT)segindex;
					}
					else TempBuffer[0][j]=TempBuffer[0][j-1];
				}
				
				if (cc != 0) 
				{
						if ((aa+bb) ==0)
						{
							segindex++;
							TempBuffer[i][j]=segindex;//InsVfw.Dis_buff[x+y+2]=segindex;
						}
						else if ( (aa+bb) !=0)		
						{
							if (aa ==0) TempBuffer[i][j]=TempBuffer[i-1][j];//InsVfw.Dis_buff[x+y+2]=InsVfw.Dis_buff[x-3+y+2];
							else if (bb==0) TempBuffer[i][j]=TempBuffer[i][j-1];//InsVfw.Dis_buff[x+y+2]=InsVfw.Dis_buff[x+y-w+2];

							else if ( (aa!=bb) && (aa!=0) && (bb!=0))
							{
								
								TempBuffer[i][j]=TempBuffer[i][j-1];
								if (TempBuffer[i-1][j] < TempBuffer[i][j-1])//if ( InsVfw.Dis_buff[x-3+y+2] < InsVfw.Dis_buff[x+y-w+2])
								{
										recorded=FALSE;
										if (collx !=0)
										{
											for (ce=0; ce<collx; ce++) if ((coll[ce][0]==TempBuffer[i][j-1]) && (coll[ce][1]==TempBuffer[i-1][j])) recorded=TRUE;//InsVfw.Dis_buff[x+y-w+2]) recorded=TRUE;
										}
										if (recorded==FALSE) 
										{
											//coll[collx][0]=InsVfw.Dis_buff[x+y-w+2]; 
											//coll[collx][1]=InsVfw.Dis_buff[x-3, y+2];
											coll[collx][0]=TempBuffer[i][j-1];
											coll[collx][1]=TempBuffer[i-1][j];
											collx++;
										}