cvhmm.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 1,633 行 · 第 1/4 页

    /* clear */
    memset( num_samples, 0 , total*sizeof(int) );
    memset( counter, 0 , total*sizeof(int) );
    
    
    /* for every state the number of vectors which belong to it is computed (smth. like histogram) */
    for (k = 0; k < num_img; k++)
    {  
        CvImgObsInfo* obs = obs_info_array[k];
        int count = 0;
        
        for (i = 0; i < obs->obs_y; i++)
        {
            for (j = 0; j < obs->obs_x; j++, count++)
            {
                int state = obs->state[ 2 * count + 1];
                num_samples[state] += 1;
            }
        }
    } 
    
    /* for every state int* is allocated */
    a_class = (int**)icvAlloc( total*sizeof(int*) );
    
    for (i = 0; i < total; i++)
    {
        a_class[i] = (int*)icvAlloc( num_samples[i] * sizeof(int) );
        samples[i] = (CvVect32f*)icvAlloc( num_samples[i] * sizeof(CvVect32f) );
        samples_mix[i] = (int**)icvAlloc( num_samples[i] * sizeof(int*) );
    }
    
    /* for every state vectors which belong to state are gathered */
    for (k = 0; k < num_img; k++)
    {  
        CvImgObsInfo* obs = obs_info_array[k];
        int num_obs = ( obs->obs_x ) * ( obs->obs_y );
        float* vector = obs->obs;

        for (i = 0; i < num_obs; i++, vector+=obs->obs_size )
        {
            int state = obs->state[2*i+1];
            
            samples[state][counter[state]] = vector;
            samples_mix[state][counter[state]] = &(obs->mix[i]);
            counter[state]++;            
        }
    } 
    
    /* clear counters */
    memset( counter, 0, total*sizeof(int) );
    
    /* do the actual clustering using the K Means algorithm */
    for (i = 0; i < total; i++)
    {
        if ( first_state[i].num_mix == 1)
        {   
            for (k = 0; k < num_samples[i]; k++)
            {  
                /* all vectors belong to one mixture */
                a_class[i][k] = 0;
            }
        }      
        else if( num_samples[i] )
        {
            /* clusterize vectors  */
            cvKMeans( first_state[i].num_mix, samples[i], num_samples[i], 
                      obs_info_array[0]->obs_size, criteria, a_class[i] );
        } 
    }
    
    /* for every vector number of mixture is assigned */
    for( i = 0; i < total; i++ )
    {
        for (j = 0; j < num_samples[i]; j++)
        {
            samples_mix[i][j][0] = a_class[i][j];
        }
    }
    
    for (i = 0; i < total; i++)
    {
        icvFree( &(a_class[i]) );
        icvFree( &(samples[i]) );
        icvFree( &(samples_mix[i]) );
    }

    icvFree( &a_class );
    icvFree( &samples );
    icvFree( &samples_mix );
    icvFree( &counter );
    icvFree( &num_samples );  
    
    return CV_NO_ERR;
}

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: ComputeUniModeGauss
//    Purpose: The function computes the Gaussian pdf for a sample vector 
//    Context:
//    Parameters:  obsVeq - pointer to the sample vector
//                 mu - pointer to the mean vector of the Gaussian pdf
//                 var - pointer to the variance vector of the Gaussian pdf
//                 VecSize - the size of sample vector
//                 
//    Returns: the pdf of the sample vector given the specified Gaussian 
//
//    Notes: 
//F*/
float icvComputeUniModeGauss(CvVect32f vect, CvVect32f mu, 
                              CvVect32f inv_var, float log_var_val, int vect_size)           
{
    int n; 
    double tmp;
    double prob;

    prob = -log_var_val;

    for (n = 0; n < vect_size; n++)
    {
        tmp = (vect[n] - mu[n]) * inv_var[n];
        prob = prob - tmp * tmp;
   }
   //prob *= 0.5f;
  
   return (float)prob;
}                        

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: ComputeGaussMixture
//    Purpose: The function computes the mixture Gaussian pdf of a sample vector. 
//    Context:
//    Parameters:  obsVeq - pointer to the sample vector
//                 mu  - two-dimensional pointer to the mean vector of the Gaussian pdf;
//                       the first dimension is indexed over the number of mixtures and 
//                       the second dimension is indexed along the size of the mean vector
//                 var - two-dimensional pointer to the variance vector of the Gaussian pdf;
//                       the first dimension is indexed over the number of mixtures and 
//                       the second dimension is indexed along the size of the variance vector
//                 VecSize - the size of sample vector
//                 weight - pointer to the wights of the Gaussian mixture
//                 NumMix - the number of Gaussian mixtures
//                 
//    Returns: the pdf of the sample vector given the specified Gaussian mixture.  
//
//    Notes: 
//F*/
/* Calculate probability of observation at state in logarithmic scale*/
float icvComputeGaussMixture( CvVect32f vect, float* mu, 
                                float* inv_var, float* log_var_val, 
                                int vect_size, float* weight, int num_mix )
{       
    double prob, l_prob;
    
    prob = 0.0f; 

    if (num_mix == 1)
    {
        return icvComputeUniModeGauss( vect, mu, inv_var, log_var_val[0], vect_size);    
    }
    else
    {
        int m;
        for (m = 0; m < num_mix; m++)
        {
            if ( weight[m] > 0.0)
            { 
                l_prob = icvComputeUniModeGauss(vect, mu + m*vect_size, 
                                                        inv_var + m * vect_size,
                                                        log_var_val[m], 
                                                        vect_size); 

                prob = prob + weight[m]*exp((double)l_prob);
            }
        } 
        prob = log(prob);    
    }                        
    return (float)prob;   
}                            


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: EstimateObsProb
//    Purpose: The function computes the probability of every observation in every state 
//    Context:
//    Parameters:  obs_info - observations
//                 hmm      - hmm
//    Returns: error status  
//
//    Notes: 
//F*/
IPCVAPI_IMPL(CvStatus, icvEstimateObsProb, (CvImgObsInfo* obs_info, CvEHMM* hmm ))
{
    int i, j;
    int total_states = 0;

    /* check if matrix exist and check current size
       if not sufficient - realloc */
    int status = 0; /* 1 - not allocated, 2 - allocated but small size, 
                       3 - size is enough, but distribution is bad, 0 - all ok */

    for( j = 0; j < hmm->num_states; j++ )
    {
       total_states += hmm->u.ehmm[j].num_states;
    }

    if ( hmm->obsProb == NULL ) 
    {
        /* allocare memory */
        int need_size = ( obs_info->obs_x * obs_info->obs_y * total_states * sizeof(float) +
                          obs_info->obs_y * hmm->num_states * sizeof( CvMatr32f) );

        int* buffer = (int*)icvAlloc( need_size + 3 * sizeof(int) );
        buffer[0] = need_size;
        buffer[1] = obs_info->obs_y;
        buffer[2] = obs_info->obs_x;
        hmm->obsProb = (float**) (buffer + 3);
        status = 3;
        
    }
    else
    {   
        /* check current size */
        int* total= (int*)(((int*)(hmm->obsProb)) - 3);
        int need_size = ( obs_info->obs_x * obs_info->obs_y * total_states * sizeof(float) +
                          obs_info->obs_y * hmm->num_states * sizeof( CvMatr32f/*(float*)*/ ) );

        assert( sizeof(float*) == sizeof(int) );

        if ( need_size > (*total) ) 
        {
            int* buffer = ((int*)(hmm->obsProb)) - 3;
            icvFree( &buffer);
            buffer = (int*)icvAlloc( need_size + 3 * sizeof(int));
            buffer[0] = need_size;
            buffer[1] = obs_info->obs_y;
            buffer[2] = obs_info->obs_x;

            hmm->obsProb = (float**)(buffer + 3);
            
            status = 3;
        }          
    }
    if (!status)
    {
        int* obsx = ((int*)(hmm->obsProb)) - 1;
        int* obsy = ((int*)(hmm->obsProb)) - 2;
                
        assert( (*obsx > 0) && (*obsy > 0) );

        /* is good distribution? */
        if ( (obs_info->obs_x > (*obsx) ) || (obs_info->obs_y > (*obsy) ) ) 
            status = 3;        
    }
    
    /* if bad status - do reallocation actions */
    assert( (status == 0) || (status == 3) );

    if ( status )
    {
        float** tmp = hmm->obsProb;
        float*  tmpf;

        /* distribute pointers of ehmm->obsProb */
        for( i = 0; i < hmm->num_states; i++ )
        {
            hmm->u.ehmm[i].obsProb = tmp; 
            tmp += obs_info->obs_y;
        }

        tmpf = (float*)tmp;

        /* distribute pointers of ehmm->obsProb[j] */
        for( i = 0; i < hmm->num_states; i++ )
        {
            CvEHMM* ehmm = &( hmm->u.ehmm[i] );
            
            for( j = 0; j < obs_info->obs_y; j++ )
            {
                ehmm->obsProb[j] = tmpf;
                tmpf += ehmm->num_states * obs_info->obs_x;
            }           
        }
    }/* end of pointer distribution */ 

#if 1    
    {
#define MAX_BUF_SIZE  1200
        float  local_log_mix_prob[MAX_BUF_SIZE];
        double local_mix_prob[MAX_BUF_SIZE];
        int    vect_size = obs_info->obs_size;
        CvStatus res = CV_NO_ERR;

        float*  log_mix_prob = local_log_mix_prob;
        double* mix_prob = local_mix_prob;
        
        int  max_size = 0;
        int  obs_x = obs_info->obs_x;

        /* calculate temporary buffer size */
        for( i = 0; i < hmm->num_states; i++ )
        {
            CvEHMM* ehmm = &(hmm->u.ehmm[i]);
            CvEHMMState* state = ehmm->u.state;

            int max_mix = 0;
            for( j = 0; j < ehmm->num_states; j++ )
            {
                int t = state[j].num_mix;
                if( max_mix < t ) max_mix = t;
            }
            max_mix *= ehmm->num_states;
            if( max_size < max_mix ) max_size = max_mix;
        }

        max_size *= obs_x * vect_size;
        
        /* allocate buffer */
        if( max_size > MAX_BUF_SIZE )
        {
            log_mix_prob = (float*)icvAlloc( max_size*(sizeof(float) + sizeof(double)));
            if( !log_mix_prob ) return CV_OUTOFMEM_ERR;
            mix_prob = (double*)(log_mix_prob + max_size);
        }

        memset( log_mix_prob, 0, max_size*sizeof(float));

        /*****************computing probabilities***********************/
        
        /* loop through external states */
        for( i = 0; i < hmm->num_states; i++ )
        {
            CvEHMM* ehmm = &(hmm->u.ehmm[i]);
            CvEHMMState* state = ehmm->u.state;
            
            int max_mix = 0;
            int n_states = ehmm->num_states;

            /* determine maximal number of mixtures (again) */
            for( j = 0; j < ehmm->num_states; j++ )
            {
                int t = state[j].num_mix;
                if( max_mix < t ) max_mix = t;
            }

            /* loop through rows of the observation matrix */
            for( j = 0; j < obs_info->obs_y; j++ )
            {
                int  m, n;
                       
                float* obs = obs_info->obs + j * obs_x * vect_size;
                float* log_mp = max_mix > 1 ? log_mix_prob : ehmm->obsProb[j];
                double* mp = mix_prob;
            
                /* several passes are done below */
                
                /* 1. calculate logarithms of probabilities for each mixture */

                /* loop through mixtures */
                for( m = 0; m < max_mix; m++ )
                {
                    /* set pointer to first observation in the line */
                    float* vect = obs;

                    /* cycles through obseravtions in the line */
                    for( n = 0; n < obs_x; n++, vect += vect_size, log_mp += n_states )
                    {
                        int k, l;
                        for( l = 0; l < n_states; l++ )
                        {
                            if( state[l].num_mix > m )
                            {
                                float* mu = state[l].mu + m*vect_size;
                                float* inv_var = state[l].inv_var + m*vect_size;
                                double prob = -state[l].log_var_val[m];
                                for( k = 0; k < vect_size; k++ )
                                {
                                    double t = (vect[k] - mu[k])*inv_var[k];
                                    prob -= t*t;
                                }
                                log_mp[l] = MAX( (float)prob, -500 );
                            }
                        }
                    }
                }

                /* skip the rest if there is a single mixture */
                if( max_mix == 1 ) continue;

                /* 2. calculate exponent of log_mix_prob
                      (i.e. probability for each mixture) */
                res = icvbExp_32f64f( log_mix_prob, mix_prob,
                                        max_mix * obs_x * n_states );
                if( res < 0 ) goto processing_exit;

                /* 3. sum all mixtures with weights */
                /* 3a. first mixture - simply scale by weight */
                for( n = 0; n < obs_x; n++, mp += n_states )
                {
                    int l;
                    for( l = 0; l < n_states; l++ )
                    {
                        mp[l] *= state[l].weight[0];
                    }
                }

                /* 3b. add other mixtures */
                for( m = 1; m < max_mix; m++ )
                {
                    int ofs = -m*obs_x*n_states;