grimsonbg.cpp

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

/*****************************************************************************
 *
 * File :       GrimsonBG.cpp
 *
 * Module :     visualise_data
 *
 * Author :     Derek Magee, School of Computing, Leeds University.
 *
 * Created :    18th September 2001
 *
 *****************************************************************************
 *
 * Source code for CogVis
 *
 * The author, Derek Magee, gives permission for this code to be copied,
 * modified and distributed within the University of Leeds subject to the
 * following conditions:-
 *
 * - The code is not to be used for commercial gain.
 * - The code and use thereof will be attributed to the author where
 *   appropriate (inluding demonstrations which rely on it's use).
 * - All modified, distributions of the source files will retain this header.
 *
 *****************************************************************************
 *
 * Description:
 *
 * Methods for class GrimsonBG which implements the Stauffer / Grimson 
 * background m_pModel (based on mixtures of gaussians at each pixel).
 *
 * See:
 *
 * C. Stauffer, W.E.L. Grimson "Adaptive background mixture m_pModels for real-time
 * tracking", CVPR 99. 
 *
 *****************************************************************************
 *
 * Revision History:
 *
 * Date         By              Revision
 *
 * 01/08/01     DRM             Created.
 *
 ****************************************************************************/  
#include "stdafx.h"
#include "GrimsonBG.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>


#define DATA_RANGE 255
#define MANH_THRESH 2
#define MANH_THRESH_SQ (MANH_THRESH*MANH_THRESH) 
#define INITIAL_WEIGHT 0.05
#define MIN_COV  0.000005
//#define INIT_VAR 0.00001 /* For normalised colourspace */
#define INIT_VAR 0.005 * DATA_RANGE * DATA_RANGE /* For RGB colourspace */
//#define INIT_VAR 0.01 * DATA_RANGE * DATA_RANGE
#define MAX_MIXTURE_PROB 0.5
#define  M_PI 3.14159
#define  INITFRAME 200
GrimsonBG::GrimsonBG()
{
	AVIFileInit();	
   
	m_biGaussiansPerMix=3;
    m_dProportion=0.8;
    m_fUpdateAlpha=0.3;
	
	m_iNum=0;
	m_iNumAudioStreams=0;
	m_iNumVideoStreams=0;	
	m_biWidth=0;
	m_biHeight=0;
	m_wBitCount=0;
	m_iCurrentTrack=0;
	m_pGf = NULL;
	m_bAviOpened=FALSE;
	m_bStreamInit=FALSE;
	m_bGetFrame=FALSE;
	m_lCurrentFrame=0;
	m_iMinRegionSize=9;
	m_iMinObjectSize=600; 
	
}
GrimsonBG::BackgroundInit(unsigned int gauss_per_mix,
                     double       m_fProportion,
                     double       m_fUpdateM_dAlpha,
                     unsigned int size_x,
					 unsigned int size_y,
                     unsigned int x_inc,    // Increment for low-res m_pModel
                     unsigned int y_inc,
                     DataType     type)
{
// Constructor: Allocates memory etc.
    unsigned int p_cnt ;
    Gaussian   *new_g ;
 
    m_bInitOk = true ;

    m_biGaussiansPerMix = m_biGaussiansPerMix ;
    m_dBgdProp = m_fProportion ;
    m_dAlpha = m_fUpdateAlpha ;
    m_dAlphai = 1.0 - m_fUpdateAlpha ;
    m_biInrX = x_inc ;
    m_biInrY = y_inc ;
    m_sdDataType = type ;
    m_bUpdateEveryFrame = true ;
	m_biWidth=size_x;
    m_biHeight	= size_y;
     int i;
    m_biTotPixels = (320 / m_biInrX) * (240 / m_biInrY) ; // Total pixels in use 
	if((m_pModel =  new PixelInfo[m_biTotPixels]) == NULL) m_bInitOk = false ;
    for(p_cnt=0 ; p_cnt<m_biTotPixels && m_bInitOk ; p_cnt++){
        m_pModel[p_cnt].frames_since_fg = 0 ;
        m_pModel[p_cnt].frames_since_ud = INT_MAX ;
		m_pModel[p_cnt].no_in_bg = m_biGaussiansPerMix;
		for(i = 0; i<m_biGaussiansPerMix; i++)
		{
			m_pModel[p_cnt].belong[i]=0;
		}
		
        if((new_g = new Gaussian[m_biGaussiansPerMix]) == NULL)
                                                                m_bInitOk = false ;
        else if((m_pModel[p_cnt].gaussians = 
                            new Gaussian*[m_biGaussiansPerMix]) == NULL)
                                                                m_bInitOk = false ;
        else InitialiseMixtures(new_g,
                                 m_pModel[p_cnt].gaussians) ;
		m_bShadow= false;
    }

}

GrimsonBG::~GrimsonBG()
{
	if (m_bGetFrame==TRUE) 	AVIStreamGetFrameClose(m_pGf);
	if (m_bStreamInit==TRUE)	AVIStreamRelease(m_pStream);
	if (m_bAviOpened==TRUE)	AVIFileRelease(m_pAviFile);
	AVIFileExit();

}

void GrimsonBG::InitialiseMixtures(Gaussian *g,
                                    Gaussian **gps) 
{
// Randomly initialise mixture means, set weights equal and set covs to large
// values

           int g_cnt ; 
    double       wt ;
   

    wt = 1.0 / (double)m_biGaussiansPerMix ;

    for(g_cnt=0 ; g_cnt<m_biGaussiansPerMix ; g_cnt++){
        gps[g_cnt] = &(g[g_cnt]) ; 
        g[g_cnt].weight = wt ;
        g[g_cnt].mean[0] = ((double)rand() / (double)RAND_MAX) * DATA_RANGE ; 
        g[g_cnt].mean[1] =((double)rand() / (double)RAND_MAX) * DATA_RANGE ; 
        g[g_cnt].mean[2] = ((double)rand() / (double)RAND_MAX) * DATA_RANGE ; 
        g[g_cnt].covariance = INIT_VAR ;
	
    }
}

void GrimsonBG::SortGaussians(Gaussian **g)
{
// Sorts Gaussians into decending weight order

     int g_cnt ;
     int g_cnt2 ;
    Gaussian   **g_ptr ;
    Gaussian   **g_ptr2 ;
    Gaussian   *g_ptr_i ;
    Gaussian   *g_ptr_i2 ;

    for(g_cnt=0,g_ptr=g ; g_cnt<m_biGaussiansPerMix ; g_cnt++,g_ptr++){
        g_ptr_i = *g_ptr ;
        for(g_cnt2=g_cnt+1,g_ptr2=g+g_cnt+1 ; g_cnt2<m_biGaussiansPerMix ; 
                                                            g_cnt2++,g_ptr2++){
        // if gaussian[g_cnt2] weight > gaussian[g_cnt] weight then swap
            g_ptr_i2 = *g_ptr2 ;
            if(g_ptr_i2->weight > g_ptr_i->weight){
                *g_ptr = g_ptr_i2 ;
                *g_ptr2 = g_ptr_i ;
            }
        }
    }
}       

inline double GrimsonBG::CalculateGaussian(double *d,
                                            Gaussian *g)
{
/*
 * Method returns the value of an n-dimensional gaussian with mean 'mean_vector' * and covariance 'cov_matrix' at the point in_vector.
 *
 *    _   ____   ___
 * G( x , mean , cov) = exp( -0.5 * (x-mean)T * inv(cov) * (x-mean) )
 *                      ---------------------------------------------
 *                               (2*PI)^(n/2) * sqrt(|cov|)
 *
 *        _     ____                     ___
 *  where x and mean are 1xn vectors and cov is an nxn matrix.
 *  |cov| = determinant of the covariance matrix.
 */
    register double       ret_val ;
    register double       cov_determinant ;
    register double       m_biInrX ;
    register double       m_biInrY ;
    register double       dz ;
    register double       cov ;
    register double       *dptr ;
    register double       *mptr ;

    cov = g->covariance ;
 
    cov_determinant = cov * cov * cov ; 

    dptr = d ;
    mptr = g->mean ; 
    m_biInrX = *(dptr++) - *(mptr++) ;
    m_biInrX *= m_biInrX ;
    m_biInrY = *(dptr++) - *(mptr++) ;
    m_biInrY *= m_biInrY ;
    dz = *dptr - *mptr ;
    dz *= dz ;
 
    ret_val = -0.5 * (m_biInrX + m_biInrY + dz) / cov ;
    ret_val = 
    ret_val = exp(ret_val) ;
 
 //   ret_val /= pow(2*M_PI,1.5) ;
  //  ret_val /= sqrt(cov_determinant) ;
 
    return ret_val ;                                    

}

void GrimsonBG::AddNewImage(unsigned char *data)
{
// Updates m_pModel with new data.

     int x_cnt ;
     int y_cnt ;
    unsigned int p_cnt ;
    unsigned int d_off ;
    double       red_d ;
    double       green_d ;
    double       blue_d ;
    double       data_d[3] ;
    PixelInfo    *pix_ptr ;
    unsigned char   *d_ptr ;      
    unsigned int l_inc ;
    unsigned int p_inc ;
    register double *dd_ptr ;

    l_inc = m_biWidth * (m_biInrY-1) * 3 ;
    p_inc = (m_biInrX-1) * 3 ;

    pix_ptr = m_pModel ;

    d_ptr = data ;

    if(m_sdDataType==DATA_RGB){
    // Data is raw RGB
        for(y_cnt=0 ; y_cnt<m_biHeight ; y_cnt+=m_biInrY, d_ptr+=l_inc){
            for(x_cnt=0 ; x_cnt<m_biWidth ; x_cnt+=m_biInrX,p_cnt++,pix_ptr++,d_ptr+=p_inc){
                dd_ptr = data_d ;
                *(dd_ptr++) = (double)*(d_ptr++) ;
                *(dd_ptr++) = (double)*(d_ptr++) ;
                *dd_ptr = (double)*(d_ptr++) ;
                UpdateMixture(data_d, pix_ptr) ;

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

                UpdateMixture(data_d, pix_ptr) ;
            }
        }
    }
}



void GrimsonBG::CalculateUpdate(unsigned char *data)
{
// Updates m_pModel with new data.

    int x_cnt ;
    int y_cnt ;
    unsigned int p_cnt ;
    double       data_d[3] ;
    PixelInfo    *pix_ptr ;
    unsigned char          *d_ptr ;
    unsigned char          *fg_ptr ;
    unsigned int l_inc ;
    unsigned int p_inc ;
    unsigned int l_inc_fg ;
    unsigned int p_inc_fg ;
    l_inc = m_biWidth * (m_biInrY-1) * 3 ;
    p_inc = (m_biInrX-1) * 3 ;
    l_inc_fg = m_biWidth * (m_biInrY-1) ;
    p_inc_fg = m_biInrX ;
    pix_ptr = m_pModel ;
    d_ptr = data ;
    if(m_sdDataType==DATA_RGB){
    // Data is raw RGB
        for(y_cnt=0 ; y_cnt<m_biHeight ; y_cnt+=m_biInrY, d_ptr+=l_inc, fg_ptr+=l_inc_fg){
            for(x_cnt=0 ; x_cnt<m_biWidth ; x_cnt+=m_biInrX,p_cnt++,pix_ptr++,d_ptr+=p_inc,
                                                              fg_ptr+=p_inc_fg){
                data_d[0] = (double)*(d_ptr++) ;
                data_d[1] = (double)*(d_ptr++) ;
                data_d[2] = (double)*(d_ptr++) ;

                if(!InMixture(data_d, pix_ptr,false)){
    //                *fg_ptr = 255 ;
                    pix_ptr->frames_since_fg = 1 ;
                }
                else{
    //               *fg_ptr = 0 ;
                    pix_ptr->frames_since_fg =0 ;
                }

            }
        }
    }
    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)){
//                    fg[(y_cnt * m_biWidth + x_cnt)] = 255 ;
//                }
//                else{
//                    fg[(y_cnt * m_biWidth + x_cnt)] = 0 ;
//                }
//            }
//        }
    }

}

void GrimsonBG::CalculateForeground(unsigned char *data,
                                     unsigned char *fg)
{
// Updates m_pModel with new data.

     int x_cnt ;
     int y_cnt ;
    unsigned int p_cnt ;
    unsigned int d_off ;
    double       red_d ;
    double       green_d ;
    double       blue_d ;
    double       data_d[3] ;
    PixelInfo    *pix_ptr ;
    unsigned char          *d_ptr ;
    unsigned char          *fg_ptr ;
    unsigned int l_inc ;
    unsigned int p_inc ;
    unsigned int l_inc_fg ;
    unsigned int p_inc_fg ;
    l_inc = m_biWidth * (m_biInrY-1) * 3 ;
    p_inc = (m_biInrX-1) * 3 ;
    l_inc_fg = m_biWidth * (m_biInrY-1) ;
    p_inc_fg = m_biInrX ;
    pix_ptr = m_pModel ;

    d_ptr = data ;
    fg_ptr = fg ;

    if(m_sdDataType==DATA_RGB){
    // Data is raw RGB
        for(y_cnt=0 ; y_cnt<m_biHeight ; y_cnt+=m_biInrY,fg_ptr+=l_inc_fg){
            for(x_cnt=0 ; x_cnt<m_biWidth ; x_cnt+=m_biInrX,p_cnt++,pix_ptr++,d_ptr+=p_inc
                                                              ){

                data_d[0] = (double)*(d_ptr++) ;
                 data_d[1] = (double)*(d_ptr++) ;
                data_d[2] = (double)*(d_ptr++) ;

                if(!InMixture(data_d, pix_ptr,true))
				{
                   if(isShadow(data_d, pix_ptr))
				   {
					fg_ptr[(y_cnt * m_biWidth + x_cnt)] =255;

				   }
				   else
				   {
					fg_ptr[(y_cnt * m_biWidth + x_cnt)] =255;
   
				   }
                } 
                else
				{   
					 fg_ptr[(y_cnt * m_biWidth + x_cnt)] =0;
                }
            }
        }