cvhmm.cpp.svn-base

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

                    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 */
                res = icvbLog_64f32f( mix_prob, ehmm->obsProb[j],
                                        obs_x * n_states );
                if( res < 0 ) goto processing_exit;
            }
        }

processing_exit:

        if( log_mix_prob != local_log_mix_prob ) icvFree( &log_mix_prob );
        return res;
#undef MAX_BUF_SIZE
    }
#else
    for( i = 0; i < hmm->num_states; i++ )
    {
        CvEHMM* ehmm = &(hmm->u.ehmm[i]);
        CvEHMMState* state = ehmm->u.state;

        for( j = 0; j < obs_info->obs_y; j++ )
        {
            int k,m;
                       
            int obs_index = j * obs_info->obs_x;

            float* B = ehmm->obsProb[j];
            
            /* cycles through obs and states */
            for( k = 0; k < obs_info->obs_x; k++ )
            {
                CvVect32f vect = (obs_info->obs) + (obs_index + k) * vect_size;
                
                float* matr_line = B + k * ehmm->num_states;

                for( m = 0; m < ehmm->num_states; m++ )
                {
                    matr_line[m] = icvComputeGaussMixture( vect, state[m].mu, state[m].inv_var, 
                                                             state[m].log_var_val, vect_size, state[m].weight,
                                                             state[m].num_mix );
                }
            }
        }
    }
#endif
}


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: EstimateTransProb
//    Purpose: The function calculates the state and super state transition probabilities 
//             of the model given the images, 
//             the state segmentation and the input parameters
//    Context:
//    Parameters: obs_info_array - array of pointers to image observations 
//                num_img - length of above array
//                hmm - pointer to HMM structure                 
//    Returns: void
//
//    Notes:   
//F*/
IPCVAPI_IMPL(CvStatus, icvEstimateTransProb, ( CvImgObsInfo** obs_info_array, 
                                               int num_img, 
                                               CvEHMM* hmm ))
{
    int    i, j, k;

    CvEHMMState* first_state = hmm->u.ehmm->u.state;
    /* as a counter we will use transP matrix */
    
    /* initialization */
    
    /* clear transP */
    icvSetZero_32f( hmm->transP, hmm->num_states, hmm->num_states );
    for (i = 0; i < hmm->num_states; i++ )
    {
        icvSetZero_32f( hmm->u.ehmm[i].transP , hmm->u.ehmm[i].num_states, hmm->u.ehmm[i].num_states );
    }
        
    /* compute the counters */
    for (i = 0; i < num_img; i++)
    {
        int counter = 0;
        CvImgObsInfo* info = obs_info_array[i];
        
        for (j = 0; j < info->obs_y; j++)
        {
            for (k = 0; k < info->obs_x; k++, counter++)
            {
                /* compute how many transitions from state to state
                   occured both in horizontal and vertical direction */ 
                int superstate, state;
                int nextsuperstate, nextstate;
                int begin_ind;

                superstate = info->state[2 * counter];
                begin_ind = hmm->u.ehmm[superstate].u.state - first_state;
                state = info->state[ 2 * counter + 1] - begin_ind; 
                
                if (j < info->obs_y - 1)
                {
                    int transP_size = hmm->num_states;
                    
                    nextsuperstate = info->state[ 2*(counter + info->obs_x) ];

                    hmm->transP[superstate * transP_size + nextsuperstate] += 1;
                }
                
                if (k < info->obs_x - 1)
                {   
                    int transP_size = hmm->u.ehmm[superstate].num_states;

                    nextstate = info->state[2*(counter+1) + 1] - begin_ind;
                    hmm->u.ehmm[superstate].transP[ state * transP_size + nextstate] += 1;
                }
            }
        }
    }
    /* estimate superstate matrix */
    for( i = 0; i < hmm->num_states; i++)
    {
        float total = 0;
        float inv_total;
        for( j = 0; j < hmm->num_states; j++)
        {
            total += hmm->transP[i * hmm->num_states + j];
        }
        //assert( total );

        inv_total = total ? 1.f/total : 0;
        
        for( j = 0; j < hmm->num_states; j++)
        {                   
            hmm->transP[i * hmm->num_states + j] = 
                hmm->transP[i * hmm->num_states + j] ? 
                (float)log( hmm->transP[i * hmm->num_states + j] * inv_total ) : -BIG_FLT;
        }
    }
    
    /* estimate other matrices */
    for( k = 0; k < hmm->num_states; k++ )
    {
        CvEHMM* ehmm = &(hmm->u.ehmm[k]);

        for( i = 0; i < ehmm->num_states; i++)
        {
            float total = 0;
            float inv_total;
            for( j = 0; j < ehmm->num_states; j++)
            {
                total += ehmm->transP[i*ehmm->num_states + j];
            }
            //assert( total );
            inv_total = total ? 1.f/total :  0;
            
            for( j = 0; j < ehmm->num_states; j++)
            {                   
                ehmm->transP[i * ehmm->num_states + j] = 
                    (ehmm->transP[i * ehmm->num_states + j]) ?
                    (float)log( ehmm->transP[i * ehmm->num_states + j] * inv_total) : -BIG_FLT ;
            }
        }
    }
    return CV_NO_ERR;
} 
                      

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: MixSegmL2
//    Purpose: The function implements the mixture segmentation of the states of the
//             embedded HMM
//    Context: used with the Viterbi training of the embedded HMM
//
//    Parameters:  
//             obs_info_array
//             num_img
//             hmm
//    Returns: void
//
//    Notes: 
//F*/
IPCVAPI_IMPL(CvStatus, icvMixSegmL2, (CvImgObsInfo** obs_info_array, int num_img,
                                      CvEHMM* hmm ))
{
    int     k, i, j, m;
       
    CvEHMMState* state = hmm->u.ehmm[0].u.state;
    
    
    for (k = 0; k < num_img; k++)
    {   
        int counter = 0;
        CvImgObsInfo* info = obs_info_array[k];

        for (i = 0; i < info->obs_y; i++)
        {
            for (j = 0; j < info->obs_x; j++, counter++)
            {
                int e_state = info->state[2 * counter + 1];
                float min_dist;
                                                
                min_dist = icvSquareDistance((info->obs) + (counter * info->obs_size), 
                                               state[e_state].mu, info->obs_size);
                info->mix[counter] = 0;  
                
                for (m = 1; m < state[e_state].num_mix; m++)
                {
                    float dist=icvSquareDistance( (info->obs) + (counter * info->obs_size),
                                                    state[e_state].mu + m * info->obs_size,
                                                    info->obs_size);
                    if (dist < min_dist)
                    {
                        min_dist = dist;
                        /* assign mixture with smallest distance */ 
                        info->mix[counter] = m;
                    }
                }
            }
        }
    }
    return CV_NO_ERR;
} 

/*
CvStatus icvMixSegmProb(CvImgObsInfo* obs_info, int num_img, CvEHMM* hmm )
{
    int     k, i, j, m;
       
    CvEHMMState* state = hmm->ehmm[0].state_info;
    
    
    for (k = 0; k < num_img; k++)
    {   
        int counter = 0;
        CvImgObsInfo* info = obs_info + k;

        for (i = 0; i < info->obs_y; i++)
        {
            for (j = 0; j < info->obs_x; j++, counter++)
            {
                int e_state = info->in_state[counter];
                float max_prob;
                                                
                max_prob = icvComputeUniModeGauss( info->obs[counter], state[e_state].mu[0], 
                                                    state[e_state].inv_var[0], 
                                                    state[e_state].log_var[0],
                                                    info->obs_size );
                info->mix[counter] = 0;  
                
                for (m = 1; m < state[e_state].num_mix; m++)
                {
                    float prob=icvComputeUniModeGauss(info->obs[counter], state[e_state].mu[m],
                                                       state[e_state].inv_var[m], 
                                                       state[e_state].log_var[m],
                                                       info->obs_size);
                    if (prob > max_prob)
                    {
                        max_prob = prob;
                        // assign mixture with greatest probability. 
                        info->mix[counter] = m;
                    }
                }
            }
        }
    } 

    return CV_NO_ERR;
} 
*/
IPCVAPI_IMPL( CvStatus, icvViterbiSegmentation, ( int num_states, int /*num_obs*/,
                                    CvMatr32f transP,
                                    CvMatr32f B, /*muDur[0], */
                                    int start_obs, int prob_type,
                                    int** q, int min_num_obs, int max_num_obs,
                                    float* prob  ) )
{
    // memory allocation 
    int i, j, last_obs;
    int m_xl;/* from which observation to start viterbi algorithm */
    int m_HMMType = _CV_ERGODIC; /* _CV_CAUSAL or _CV_ERGODIC */
    
    int m_ProbType   = prob_type; /* _CV_LAST_STATE or _CV_BEST_STATE */
    
    int m_minNumObs  = min_num_obs; /*??*/
    int m_maxNumObs  = max_num_obs; /*??*/
    
    int m_numStates  = num_states;
    
    float* m_pi = (float*)icvAlloc( num_states* sizeof(float) );
    CvMatr32f m_a = transP;

    // offset brobability matrix to starting observation 
    CvMatr32f m_b = B + start_obs * num_states;
    //so m_xl will not be used more

    m_xl = start_obs; 

    /*     if (muDur != NULL){ 
    m_d = new int[m_numStates];
    m_l = new double[m_numStates];
    for (i = 0; i < m_numStates; i++){
    m_l[i] = muDur[i]; 
    }
    } 
    else{
    m_d = NULL;
    m_l = NULL;
    }
    */
    
    CvMatr32f m_Gamma = icvCreateMatrix_32f( num_states, m_maxNumObs );
    int* m_csi = (int*)icvAlloc( num_states * m_maxNumObs * sizeof(int) );
    
    //stores maximal result for every ending observation */
    CvVect32f   m_MaxGamma = prob;
    

//    assert( m_xl + max_num_obs <= num_obs );

    /*??m_q          = new int*[m_maxNumObs - m_minNumObs];
      ??for (i = 0; i < m_maxNumObs - m_minNumObs; i++)
      ??     m_q[i] = new int[m_minNumObs + i + 1];
    */

    /******************************************************************/
    /*    Viterbi initialization                                      */
    /* set initial state probabilities, in logarithmic scale */
    for (i = 0; i < m_numStates; i++)
    {
        m_pi[i] = -BIG_FLT;
    }
    m_pi[0] = 0.0f;
    
    for  (i = 0; i < num_states; i++)
    {
        m_Gamma[0 * num_states + i] = m_pi[i] + m_b[0 * num_states + i];
        m_csi[0 * num_states + i] = 0;   
    }
    
    /******************************************************************/
    /*    Viterbi recursion                                           */
    
    if ( m_HMMType == _CV_CAUSAL ) //causal model
    {
        int t,j;
        
        for (t = 1 ; t < m_maxNumObs; t++)
        {
            // evaluate self-to-self transition for state 0
            m_Gamma[t * num_states + 0] = m_Gamma[(t-1) * num_states + 0] + m_a[0];
            m_csi[t * num_states + 0] = 0;
            
            for (j = 1; j < num_states; j++)
            {  
                float self = m_Gamma[ (t-1) * num_states + j] + m_a[ j * num_states + j];
                float prev = m_Gamma[ (t-1) * num_states +(j-1)] + m_a[ (j-1) * num_states + j];
                
                if ( prev > self )
                {
                    m_csi[t * num_states + j] = j-1;
                    m_Gamma[t * num_states + j] = prev;
                }
                else
                {
                    m_csi[t * num_states + j] = j;
                    m_Gamma[t * num_states + j] = self;
                }
                
                m_Gamma[t * num_states + j] = m_Gamma[t * num_states + j] + m_b[t * num_states + j];   
            }                                                                    
        }
    }
    else if ( m_HMMType == _CV_ERGODIC ) //ergodic model 
    { 
        int t;
        for (t = 1 ; t < m_maxNumObs; t++)
        {     
            for (j = 0; j < num_states; j++)
            {   
                int i;
                m_Gamma[ t*num_states + j] = m_Gamma[(t-1) * num_states + 0] + m_a[0*num_states+j];
                m_csi[t *num_states + j] = 0;
                
                for (i = 1; i < num_states; i++)
                {
                    float currGamma = m_Gamma[(t-1) *num_states + i] + m_a[i *num_states + j];       
                    if (currGamma > m_Gamma[t *num_states + j])
                    { 
                        m_Gamma[t * num_states + j] = currGamma;
                        m_csi[t * num_states + j] = i;
                    }
                } 
                m_Gamma[t *num_states + j] = m_Gamma[t *num_states + j] + m_b[t * num_states + j];
            }             
        }                  
    }