cvhmm1d.cpp.svn-base

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

            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  */
            icvKMeans( 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*/
CvStatus icvEstimate1DObsProb(CvImgObsInfo* obs_info, CvEHMM* hmm )
{
    int 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;
    }*/
    total_states = hmm->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);
            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( /*(*obsy > 0) &&*/ (*obsx > 0) );

        /* is good distribution? */
        if ( (obs_info->obs_x > (*obsx) ) /* || (obs_info->obs_y > (*obsy) ) */ ) 
            status = 3;        
    }
    
    assert( (status == 0) || (status == 3) );
    /* if bad status - do reallocation actions */
    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;
            }
        }
*/
        hmm->obsProb = tmp;

    }/* 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 = hmm->u.state;

            int max_mix = 0;
            for( j = 0; j < hmm->num_states; j++ )
            {
                int t = state[j].num_mix;
                if( max_mix < t ) max_mix = t;
            }
            max_mix *= hmm->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 = hmm->u.state;
            
            int max_mix = 0;
            int n_states = hmm->num_states;

            /* determine maximal number of mixtures (again) */
            for( j = 0; j < hmm->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 : (float*)(hmm->obsProb);
                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 ) 
                {
                    /* 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;
                        for( n = 0; n < obs_x; n++, mp += n_states )
                        {
                            int l;
                            for( l = 0; l < n_states; l++ )
                            {
                                if( m < state[l].num_mix )
                                {
                                    mp[l + ofs] += mp[l] * state[l].weight[m];
                                }
                            }
                        }
                    }

                    /* 4. Put logarithms of summary probabilities to the destination matrix */