git.maemo.org Git - opencv/blob - cvaux/src/cvhmm1d.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42
  43 #include "_cvaux.h"
  44
  45 #if 0
  46
  47 #define LN2PI 1.837877f
  48 #define BIG_FLT 1.e+10f
  49
  50
  51 #define _CV_ERGODIC 1
  52 #define _CV_CAUSAL 2
  53
  54 #define _CV_LAST_STATE 1
  55 #define _CV_BEST_STATE 2
  56
  57 //*F///////////////////////////////////////////////////////////////////////////////////////
  58 //    Name: icvForward1DHMM
  59 //    Purpose: The function performs baum-welsh algorithm
  60 //    Context:
  61 //    Parameters: obs_info - addres of pointer to CvImgObsInfo structure
  62 //                num_hor_obs - number of horizontal observation vectors
  63 //                num_ver_obs - number of horizontal observation vectors
  64 //                obs_size - length of observation vector
  65 //
  66 //    Returns: error status
  67 //
  68 //    Notes:
  69 //F*/
  70 #if 0
  71 CvStatus icvForward1DHMM( int num_states, int num_obs, CvMatr64d A,
  72                           CvMatr64d B,
  73                           double* scales)
  74 {
  75     // assume that observation and transition
  76     // probabilities already computed
  77     int m_HMMType  = _CV_CAUSAL;
  78     double* m_pi = icvAlloc( num_states* sizeof( double) );
  79
  80     /* alpha is matrix
  81        rows throuhg states
  82        columns through time
  83     */
  84     double* alpha = icvAlloc( num_states*num_obs * sizeof( double ) );
  85
  86     /* All calculations will be in non-logarithmic domain */
  87
  88     /* Initialization */
  89     /* set initial state probabilities */
  90     m_pi[0] = 1;
  91     for (i = 1; i < num_states; i++)
  92     {
  93         m_pi[i] = 0.0;
  94     }
  95
  96     for  (i = 0; i < num_states; i++)
  97     {
  98         alpha[i] = m_pi[i] * m_b[ i];
  99     }
 100
 101     /******************************************************************/
 102     /*   Induction                                                    */
 103
 104     if ( m_HMMType == _CV_ERGODIC )
 105     {
 106         int t;
 107         for (t = 1 ; t < num_obs; t++)
 108         {
 109             for (j = 0; j < num_states; j++)
 110             {
 111                double sum = 0.0;
 112                int i;
 113
 114                 for (i = 0; i < num_states; i++)
 115                 {
 116                      sum += alpha[(t - 1) * num_states + i] * A[i * num_states + j];
 117                 }
 118
 119                 alpha[(t - 1) * num_states + j] = sum * B[t * num_states + j];
 120
 121                 /* add computed alpha to scale factor */
 122                 sum_alpha += alpha[(t - 1) * num_states + j];
 123             }
 124
 125             double scale = 1/sum_alpha;
 126
 127             /* scale alpha */
 128             for (j = 0; j < num_states; j++)
 129             {
 130                 alpha[(t - 1) * num_states + j] *= scale;
 131             }
 132
 133             scales[t] = scale;
 134
 135         }
 136     }
 137
 138 #endif
 139
 140
 141
 142 //*F///////////////////////////////////////////////////////////////////////////////////////
 143 //    Name: icvCreateObsInfo
 144 //    Purpose: The function allocates memory for CvImgObsInfo structure
 145 //             and its inner stuff
 146 //    Context:
 147 //    Parameters: obs_info - addres of pointer to CvImgObsInfo structure
 148 //                num_hor_obs - number of horizontal observation vectors
 149 //                num_ver_obs - number of horizontal observation vectors
 150 //                obs_size - length of observation vector
 151 //
 152 //    Returns: error status
 153 //
 154 //    Notes:
 155 //F*/
 156 /*CvStatus icvCreateObsInfo( CvImgObsInfo** obs_info,
 157                               CvSize num_obs, int obs_size )
 158 {
 159     int total = num_obs.height * num_obs.width;
 160
 161     CvImgObsInfo* obs = (CvImgObsInfo*)icvAlloc( sizeof( CvImgObsInfo) );
 162
 163     obs->obs_x = num_obs.width;
 164     obs->obs_y = num_obs.height;
 165
 166     obs->obs = (float*)icvAlloc( total * obs_size * sizeof(float) );
 167
 168     obs->state = (int*)icvAlloc( 2 * total * sizeof(int) );
 169     obs->mix = (int*)icvAlloc( total * sizeof(int) );
 170
 171     obs->obs_size = obs_size;
 172
 173     obs_info[0] = obs;
 174
 175     return CV_NO_ERR;
 176 }*/
 177
 178 /*CvStatus icvReleaseObsInfo( CvImgObsInfo** p_obs_info )
 179 {
 180     CvImgObsInfo* obs_info = p_obs_info[0];
 181
 182     icvFree( &(obs_info->obs) );
 183     icvFree( &(obs_info->mix) );
 184     icvFree( &(obs_info->state) );
 185     icvFree( &(obs_info) );
 186
 187     p_obs_info[0] = NULL;
 188
 189     return CV_NO_ERR;
 190 } */
 191
 192
 193 //*F///////////////////////////////////////////////////////////////////////////////////////
 194 //    Name: icvCreate1DHMM
 195 //    Purpose: The function allocates memory for 1-dimensional HMM
 196 //             and its inner stuff
 197 //    Context:
 198 //    Parameters: hmm - addres of pointer to CvEHMM structure
 199 //                state_number - number of states in HMM
 200 //                num_mix - number of gaussian mixtures in HMM states
 201 //                          size of array is defined by previous parameter
 202 //                obs_size - length of observation vectors
 203 //
 204 //    Returns: error status
 205 //    Notes:
 206 //F*/
 207 CvStatus icvCreate1DHMM( CvEHMM** this_hmm,
 208                          int state_number, int* num_mix, int obs_size )
 209 {
 210     int i;
 211     int real_states = state_number;
 212
 213     CvEHMMState* all_states;
 214     CvEHMM* hmm;
 215     int total_mix = 0;
 216     float* pointers;
 217
 218     /* allocate memory for hmm */
 219     hmm = (CvEHMM*)icvAlloc( sizeof(CvEHMM) );
 220
 221     /* set number of superstates */
 222     hmm->num_states = state_number;
 223     hmm->level = 0;
 224
 225     /* allocate memory for all states */
 226     all_states = (CvEHMMState *)icvAlloc( real_states * sizeof( CvEHMMState ) );
 227
 228     /* assign number of mixtures */
 229     for( i = 0; i < real_states; i++ )
 230     {
 231         all_states[i].num_mix = num_mix[i];
 232     }
 233
 234     /* compute size of inner of all real states */
 235     for( i = 0; i < real_states; i++ )
 236     {
 237         total_mix += num_mix[i];
 238     }
 239     /* allocate memory for states stuff */
 240     pointers = (float*)icvAlloc( total_mix * (2/*for mu invvar */ * obs_size +
 241                                  2/*for weight and log_var_val*/ ) * sizeof( float) );
 242
 243     /* organize memory */
 244     for( i = 0; i < real_states; i++ )
 245     {
 246         all_states[i].mu      = pointers; pointers += num_mix[i] * obs_size;
 247         all_states[i].inv_var = pointers; pointers += num_mix[i] * obs_size;
 248
 249         all_states[i].log_var_val = pointers; pointers += num_mix[i];
 250         all_states[i].weight      = pointers; pointers += num_mix[i];
 251     }
 252     hmm->u.state = all_states;
 253
 254     hmm->transP = icvCreateMatrix_32f( hmm->num_states, hmm->num_states );
 255     hmm->obsProb = NULL;
 256
 257     /* if all ok - return pointer */
 258     *this_hmm = hmm;
 259     return CV_NO_ERR;
 260 }
 261
 262 CvStatus icvRelease1DHMM( CvEHMM** phmm )
 263 {
 264     CvEHMM* hmm = phmm[0];
 265     icvDeleteMatrix( hmm->transP );
 266
 267     if (hmm->obsProb != NULL)
 268     {
 269         int* tmp = ((int*)(hmm->obsProb)) - 3;
 270         icvFree( &(tmp)  );
 271     }
 272
 273     icvFree( &(hmm->u.state->mu) );
 274     icvFree( &(hmm->u.state) );
 275
 276     phmm[0] = NULL;
 277
 278     return CV_NO_ERR;
 279 }
 280
 281 /*can be used in CHMM & DHMM */
 282 CvStatus icvUniform1DSegm( Cv1DObsInfo* obs_info, CvEHMM* hmm )
 283 {
 284     /* implementation is very bad */
 285     int  i;
 286     CvEHMMState* first_state;
 287
 288     /* check arguments */
 289     if ( !obs_info || !hmm ) return CV_NULLPTR_ERR;
 290
 291     first_state = hmm->u.state;
 292
 293     for (i = 0; i < obs_info->obs_x; i++)
 294     {
 295         //bad line (division )
 296         int state = (i * hmm->num_states)/obs_info->obs_x;
 297         obs_info->state[i] = state;
 298     }
 299     return CV_NO_ERR;
 300 }
 301
 302
 303
 304 /*F///////////////////////////////////////////////////////////////////////////////////////
 305 //    Name: InitMixSegm
 306 //    Purpose: The function implements the mixture segmentation of the states of the embedded HMM
 307 //    Context: used with the Viterbi training of the embedded HMM
 308 //             Function uses K-Means algorithm for clustering
 309 //
 310 //    Parameters:  obs_info_array - array of pointers to image observations
 311 //                 num_img - length of above array
 312 //                 hmm - pointer to HMM structure
 313 //
 314 //    Returns: error status
 315 //
 316 //    Notes:
 317 //F*/
 318 CvStatus icvInit1DMixSegm(Cv1DObsInfo** obs_info_array, int num_img, CvEHMM* hmm)
 319 {
 320     int  k, i, j;
 321     int* num_samples; /* number of observations in every state */
 322     int* counter;     /* array of counters for every state */
 323
 324     int**  a_class;   /* for every state - characteristic array */
 325
 326     CvVect32f** samples; /* for every state - pointer to observation vectors */
 327     int***  samples_mix;   /* for every state - array of pointers to vectors mixtures */
 328
 329     CvTermCriteria criteria = cvTermCriteria( CV_TERMCRIT_EPS|CV_TERMCRIT_ITER,
 330                                               1000,    /* iter */
 331                                               0.01f ); /* eps  */
 332
 333     int total = hmm->num_states;
 334     CvEHMMState* first_state = hmm->u.state;
 335
 336     /* for every state integer is allocated - number of vectors in state */
 337     num_samples = (int*)icvAlloc( total * sizeof(int) );
 338
 339     /* integer counter is allocated for every state */
 340     counter = (int*)icvAlloc( total * sizeof(int) );
 341
 342     samples = (CvVect32f**)icvAlloc( total * sizeof(CvVect32f*) );
 343     samples_mix = (int***)icvAlloc( total * sizeof(int**) );
 344
 345     /* clear */
 346     memset( num_samples, 0 , total*sizeof(int) );
 347     memset( counter, 0 , total*sizeof(int) );
 348
 349
 350     /* for every state the number of vectors which belong to it is computed (smth. like histogram) */
 351     for (k = 0; k < num_img; k++)
 352     {
 353         CvImgObsInfo* obs = obs_info_array[k];
 354
 355         for (i = 0; i < obs->obs_x; i++)
 356         {
 357             int state = obs->state[ i ];
 358             num_samples[state] += 1;
 359         }
 360     }
 361
 362     /* for every state int* is allocated */
 363     a_class = (int**)icvAlloc( total*sizeof(int*) );
 364
 365     for (i = 0; i < total; i++)
 366     {
 367         a_class[i] = (int*)icvAlloc( num_samples[i] * sizeof(int) );
 368         samples[i] = (CvVect32f*)icvAlloc( num_samples[i] * sizeof(CvVect32f) );
 369         samples_mix[i] = (int**)icvAlloc( num_samples[i] * sizeof(int*) );
 370     }
 371
 372     /* for every state vectors which belong to state are gathered */
 373     for (k = 0; k < num_img; k++)
 374     {
 375         CvImgObsInfo* obs = obs_info_array[k];
 376         int num_obs = obs->obs_x;
 377         float* vector = obs->obs;
 378
 379         for (i = 0; i < num_obs; i++, vector+=obs->obs_size )
 380         {
 381             int state = obs->state[i];
 382
 383             samples[state][counter[state]] = vector;
 384             samples_mix[state][counter[state]] = &(obs->mix[i]);
 385             counter[state]++;
 386         }
 387     }
 388
 389     /* clear counters */
 390     memset( counter, 0, total*sizeof(int) );
 391
 392     /* do the actual clustering using the K Means algorithm */
 393     for (i = 0; i < total; i++)
 394     {
 395         if ( first_state[i].num_mix == 1)
 396         {
 397             for (k = 0; k < num_samples[i]; k++)
 398             {
 399                 /* all vectors belong to one mixture */
 400                 a_class[i][k] = 0;
 401             }
 402         }
 403         else if( num_samples[i] )
 404         {
 405             /* clusterize vectors  */
 406             icvKMeans( first_state[i].num_mix, samples[i], num_samples[i],
 407                 obs_info_array[0]->obs_size, criteria, a_class[i] );
 408         }
 409     }
 410
 411     /* for every vector number of mixture is assigned */
 412     for( i = 0; i < total; i++ )
 413     {
 414         for (j = 0; j < num_samples[i]; j++)
 415         {
 416             samples_mix[i][j][0] = a_class[i][j];
 417         }
 418     }
 419
 420    for (i = 0; i < total; i++)
 421     {
 422         icvFree( &(a_class[i]) );
 423         icvFree( &(samples[i]) );
 424         icvFree( &(samples_mix[i]) );
 425     }
 426
 427     icvFree( &a_class );
 428     icvFree( &samples );
 429     icvFree( &samples_mix );
 430     icvFree( &counter );
 431     icvFree( &num_samples );
 432
 433
 434     return CV_NO_ERR;
 435 }
 436
 437 /*F///////////////////////////////////////////////////////////////////////////////////////
 438 //    Name: ComputeUniModeGauss
 439 //    Purpose: The function computes the Gaussian pdf for a sample vector
 440 //    Context:
 441 //    Parameters:  obsVeq - pointer to the sample vector
 442 //                 mu - pointer to the mean vector of the Gaussian pdf
 443 //                 var - pointer to the variance vector of the Gaussian pdf
 444 //                 VecSize - the size of sample vector
 445 //
 446 //    Returns: the pdf of the sample vector given the specified Gaussian
 447 //
 448 //    Notes:
 449 //F*/
 450 /*float icvComputeUniModeGauss(CvVect32f vect, CvVect32f mu,
 451                               CvVect32f inv_var, float log_var_val, int vect_size)
 452 {
 453     int n;
 454     double tmp;
 455     double prob;
 456
 457     prob = -log_var_val;
 458
 459     for (n = 0; n < vect_size; n++)
 460     {
 461         tmp = (vect[n] - mu[n]) * inv_var[n];
 462         prob = prob - tmp * tmp;
 463    }
 464    //prob *= 0.5f;
 465
 466    return (float)prob;
 467 }*/
 468
 469 /*F///////////////////////////////////////////////////////////////////////////////////////
 470 //    Name: ComputeGaussMixture
 471 //    Purpose: The function computes the mixture Gaussian pdf of a sample vector.
 472 //    Context:
 473 //    Parameters:  obsVeq - pointer to the sample vector
 474 //                 mu  - two-dimensional pointer to the mean vector of the Gaussian pdf;
 475 //                       the first dimension is indexed over the number of mixtures and
 476 //                       the second dimension is indexed along the size of the mean vector
 477 //                 var - two-dimensional pointer to the variance vector of the Gaussian pdf;
 478 //                       the first dimension is indexed over the number of mixtures and
 479 //                       the second dimension is indexed along the size of the variance vector
 480 //                 VecSize - the size of sample vector
 481 //                 weight - pointer to the wights of the Gaussian mixture
 482 //                 NumMix - the number of Gaussian mixtures
 483 //
 484 //    Returns: the pdf of the sample vector given the specified Gaussian mixture.
 485 //
 486 //    Notes:
 487 //F*/
 488 /* Calculate probability of observation at state in logarithmic scale*/
 489 /*float icvComputeGaussMixture( CvVect32f vect, float* mu,
 490                                 float* inv_var, float* log_var_val,
 491                                 int vect_size, float* weight, int num_mix )
 492 {
 493     double prob, l_prob;
 494
 495     prob = 0.0f;
 496
 497     if (num_mix == 1)
 498     {
 499         return icvComputeUniModeGauss( vect, mu, inv_var, log_var_val[0], vect_size);
 500     }
 501     else
 502     {
 503         int m;
 504         for (m = 0; m < num_mix; m++)
 505         {
 506             if ( weight[m] > 0.0)
 507             {
 508                 l_prob = icvComputeUniModeGauss(vect, mu + m*vect_size,
 509                                                         inv_var + m * vect_size,
 510                                                         log_var_val[m],
 511                                                         vect_size);
 512
 513                 prob = prob + weight[m]*exp((double)l_prob);
 514             }
 515         }
 516         prob = log(prob);
 517     }
 518     return (float)prob;
 519 }
 520 */
 521
 522 /*F///////////////////////////////////////////////////////////////////////////////////////
 523 //    Name: EstimateObsProb
 524 //    Purpose: The function computes the probability of every observation in every state
 525 //    Context:
 526 //    Parameters:  obs_info - observations
 527 //                 hmm      - hmm
 528 //    Returns: error status
 529 //
 530 //    Notes:
 531 //F*/
 532 CvStatus icvEstimate1DObsProb(CvImgObsInfo* obs_info, CvEHMM* hmm )
 533 {
 534     int j;
 535     int total_states = 0;
 536
 537     /* check if matrix exist and check current size
 538        if not sufficient - realloc */
 539     int status = 0; /* 1 - not allocated, 2 - allocated but small size,
 540                        3 - size is enough, but distribution is bad, 0 - all ok */
 541
 542     /*for( j = 0; j < hmm->num_states; j++ )
 543     {
 544        total_states += hmm->u.ehmm[j].num_states;
 545     }*/
 546     total_states = hmm->num_states;
 547
 548     if ( hmm->obsProb == NULL )
 549     {
 550         /* allocare memory */
 551         int need_size = ( obs_info->obs_x /* * obs_info->obs_y*/ * total_states * sizeof(float) /* +
 552                           obs_info->obs_y * hmm->num_states * sizeof( CvMatr32f) */);
 553
 554         int* buffer = (int*)icvAlloc( need_size + 3 * sizeof(int) );
 555         buffer[0] = need_size;
 556         buffer[1] = obs_info->obs_y;
 557         buffer[2] = obs_info->obs_x;
 558         hmm->obsProb = (float**) (buffer + 3);
 559         status = 3;
 560
 561     }
 562     else
 563     {
 564         /* check current size */
 565         int* total= (int*)(((int*)(hmm->obsProb)) - 3);
 566         int need_size = ( obs_info->obs_x /* * obs_info->obs_y*/ * total_states * sizeof(float) /* +
 567                            obs_info->obs_y * hmm->num_states * sizeof( CvMatr32f(float*)  )*/ );
 568
 569         assert( sizeof(float*) == sizeof(int) );
 570
 571         if ( need_size > (*total) )
 572         {
 573             int* buffer = ((int*)(hmm->obsProb)) - 3;
 574             icvFree( &buffer);
 575             buffer = (int*)icvAlloc( need_size + 3);
 576             buffer[0] = need_size;
 577             buffer[1] = obs_info->obs_y;
 578             buffer[2] = obs_info->obs_x;
 579
 580             hmm->obsProb = (float**)(buffer + 3);
 581
 582             status = 3;
 583         }
 584     }
 585     if (!status)
 586     {
 587         int* obsx = ((int*)(hmm->obsProb)) - 1;
 588         //int* obsy = ((int*)(hmm->obsProb)) - 2;
 589
 590         assert( /*(*obsy > 0) &&*/ (*obsx > 0) );
 591
 592         /* is good distribution? */
 593         if ( (obs_info->obs_x > (*obsx) ) /* || (obs_info->obs_y > (*obsy) ) */ )
 594             status = 3;
 595     }
 596
 597     assert( (status == 0) || (status == 3) );
 598     /* if bad status - do reallocation actions */
 599     if ( status )
 600     {
 601         float** tmp = hmm->obsProb;
 602         //float*  tmpf;
 603
 604         /* distribute pointers of ehmm->obsProb */
 605 /*        for( i = 0; i < hmm->num_states; i++ )
 606         {
 607             hmm->u.ehmm[i].obsProb = tmp;
 608             tmp += obs_info->obs_y;
 609         }
 610 */
 611         //tmpf = (float*)tmp;
 612
 613         /* distribute pointers of ehmm->obsProb[j] */
 614 /*      for( i = 0; i < hmm->num_states; i++ )
 615         {
 616             CvEHMM* ehmm = &( hmm->u.ehmm[i] );
 617
 618             for( j = 0; j < obs_info->obs_y; j++ )
 619             {
 620                 ehmm->obsProb[j] = tmpf;
 621                 tmpf += ehmm->num_states * obs_info->obs_x;
 622             }
 623         }
 624 */
 625         hmm->obsProb = tmp;
 626
 627     }/* end of pointer distribution */
 628
 629 #if 1
 630     {
 631 #define MAX_BUF_SIZE  1200
 632         float  local_log_mix_prob[MAX_BUF_SIZE];
 633         double local_mix_prob[MAX_BUF_SIZE];
 634         int    vect_size = obs_info->obs_size;
 635         CvStatus res = CV_NO_ERR;
 636
 637         float*  log_mix_prob = local_log_mix_prob;
 638         double* mix_prob = local_mix_prob;
 639
 640         int  max_size = 0;
 641         int  obs_x = obs_info->obs_x;
 642
 643         /* calculate temporary buffer size */
 644         //for( i = 0; i < hmm->num_states; i++ )
 645         //{
 646         //    CvEHMM* ehmm = &(hmm->u.ehmm[i]);
 647             CvEHMMState* state = hmm->u.state;
 648
 649             int max_mix = 0;
 650             for( j = 0; j < hmm->num_states; j++ )
 651             {
 652                 int t = state[j].num_mix;
 653                 if( max_mix < t ) max_mix = t;
 654             }
 655             max_mix *= hmm->num_states;
 656             /*if( max_size < max_mix )*/ max_size = max_mix;
 657         //}
 658
 659         max_size *= obs_x * vect_size;
 660
 661         /* allocate buffer */
 662         if( max_size > MAX_BUF_SIZE )
 663         {
 664             log_mix_prob = (float*)icvAlloc( max_size*(sizeof(float) + sizeof(double)));
 665             if( !log_mix_prob ) return CV_OUTOFMEM_ERR;
 666             mix_prob = (double*)(log_mix_prob + max_size);
 667         }
 668
 669         memset( log_mix_prob, 0, max_size*sizeof(float));
 670
 671         /*****************computing probabilities***********************/
 672
 673         /* loop through external states */
 674         //for( i = 0; i < hmm->num_states; i++ )
 675         {
 676         //    CvEHMM* ehmm = &(hmm->u.ehmm[i]);
 677             CvEHMMState* state = hmm->u.state;
 678
 679             int max_mix = 0;
 680             int n_states = hmm->num_states;
 681
 682             /* determine maximal number of mixtures (again) */
 683             for( j = 0; j < hmm->num_states; j++ )
 684             {
 685                 int t = state[j].num_mix;
 686                 if( max_mix < t ) max_mix = t;
 687             }
 688
 689             /* loop through rows of the observation matrix */
 690             //for( j = 0; j < obs_info->obs_y; j++ )
 691             {
 692                 int  m, n;
 693
 694                 float* obs = obs_info->obs;/* + j * obs_x * vect_size; */
 695                 float* log_mp = max_mix > 1 ? log_mix_prob : (float*)(hmm->obsProb);
 696                 double* mp = mix_prob;
 697
 698                 /* several passes are done below */
 699
 700                 /* 1. calculate logarithms of probabilities for each mixture */
 701
 702                 /* loop through mixtures */
 703     /*  !!!! */     for( m = 0; m < max_mix; m++ )
 704                 {
 705                     /* set pointer to first observation in the line */
 706                     float* vect = obs;
 707
 708                     /* cycles through obseravtions in the line */
 709                     for( n = 0; n < obs_x; n++, vect += vect_size, log_mp += n_states )
 710                     {
 711                         int k, l;
 712                         for( l = 0; l < n_states; l++ )
 713                         {
 714                             if( state[l].num_mix > m )
 715                             {
 716                                 float* mu = state[l].mu + m*vect_size;
 717                                 float* inv_var = state[l].inv_var + m*vect_size;
 718                                 double prob = -state[l].log_var_val[m];
 719                                 for( k = 0; k < vect_size; k++ )
 720                                 {
 721                                     double t = (vect[k] - mu[k])*inv_var[k];
 722                                     prob -= t*t;
 723                                 }
 724                                 log_mp[l] = MAX( (float)prob, -500 );
 725                             }
 726                         }
 727                     }
 728                 }
 729
 730                 /* skip the rest if there is a single mixture */
 731                 if( max_mix != 1 )
 732                 {
 733                     /* 2. calculate exponent of log_mix_prob
 734                           (i.e. probability for each mixture) */
 735                     res = icvbExp_32f64f( log_mix_prob, mix_prob,
 736                                             max_mix * obs_x * n_states );
 737                     if( res < 0 ) goto processing_exit;
 738
 739                     /* 3. sum all mixtures with weights */
 740                     /* 3a. first mixture - simply scale by weight */
 741                     for( n = 0; n < obs_x; n++, mp += n_states )
 742                     {
 743                         int l;
 744                         for( l = 0; l < n_states; l++ )
 745                         {
 746                             mp[l] *= state[l].weight[0];
 747                         }
 748                     }
 749
 750                     /* 3b. add other mixtures */
 751                     for( m = 1; m < max_mix; m++ )
 752                     {
 753                         int ofs = -m*obs_x*n_states;
 754                         for( n = 0; n < obs_x; n++, mp += n_states )
 755                         {
 756                             int l;
 757                             for( l = 0; l < n_states; l++ )
 758                             {
 759                                 if( m < state[l].num_mix )
 760                                 {
 761                                     mp[l + ofs] += mp[l] * state[l].weight[m];
 762                                 }
 763                             }
 764                         }
 765                     }
 766
 767                     /* 4. Put logarithms of summary probabilities to the destination matrix */
 768                     res = icvbLog_64f32f( mix_prob, (float*)(hmm->obsProb),//[j],
 769                                             obs_x * n_states );
 770                     if( res < 0 ) goto processing_exit;
 771                 }
 772             }
 773         }
 774
 775 processing_exit:
 776
 777         if( log_mix_prob != local_log_mix_prob ) icvFree( &log_mix_prob );
 778         return res;
 779 #undef MAX_BUF_SIZE
 780     }
 781 #else
 782 /*    for( i = 0; i < hmm->num_states; i++ )
 783     {
 784         CvEHMM* ehmm = &(hmm->u.ehmm[i]);
 785         CvEHMMState* state = ehmm->u.state;
 786
 787         for( j = 0; j < obs_info->obs_y; j++ )
 788         {
 789             int k,m;
 790
 791             int obs_index = j * obs_info->obs_x;
 792
 793             float* B = ehmm->obsProb[j];
 794
 795             // cycles through obs and states
 796             for( k = 0; k < obs_info->obs_x; k++ )
 797             {
 798                 CvVect32f vect = (obs_info->obs) + (obs_index + k) * vect_size;
 799
 800                 float* matr_line = B + k * ehmm->num_states;
 801
 802                 for( m = 0; m < ehmm->num_states; m++ )
 803                 {
 804                     matr_line[m] = icvComputeGaussMixture( vect, state[m].mu, state[m].inv_var,
 805                                                              state[m].log_var_val, vect_size, state[m].weight,
 806                                                              state[m].num_mix );
 807                 }
 808             }
 809         }
 810     }
 811 */
 812 #endif
 813 }
 814
 815
 816 /*F///////////////////////////////////////////////////////////////////////////////////////
 817 //    Name: EstimateTransProb
 818 //    Purpose: The function calculates the state and super state transition probabilities
 819 //             of the model given the images,
 820 //             the state segmentation and the input parameters
 821 //    Context:
 822 //    Parameters: obs_info_array - array of pointers to image observations
 823 //                num_img - length of above array
 824 //                hmm - pointer to HMM structure
 825 //    Returns: void
 826 //
 827 //    Notes:
 828 //F*/
 829 CvStatus icvEstimate1DTransProb( Cv1DObsInfo** obs_info_array,
 830                                  int num_seq,
 831                                  CvEHMM* hmm )
 832 {
 833     int    i, j, k;
 834
 835     /* as a counter we will use transP matrix */
 836
 837     /* initialization */
 838
 839     /* clear transP */
 840     icvSetZero_32f( hmm->transP, hmm->num_states, hmm->num_states );
 841
 842
 843     /* compute the counters */
 844     for (i = 0; i < num_seq; i++)
 845     {
 846         int counter = 0;
 847         Cv1DObsInfo* info = obs_info_array[i];
 848
 849         for (k = 0; k < info->obs_x; k++, counter++)
 850         {
 851             /* compute how many transitions from state to state
 852                occured */
 853             int state;
 854             int nextstate;
 855
 856             state = info->state[counter];
 857
 858             if (k < info->obs_x - 1)
 859             {
 860                 int transP_size = hmm->num_states;
 861
 862                 nextstate = info->state[counter+1];
 863                 hmm->transP[ state * transP_size + nextstate] += 1;
 864             }
 865         }
 866     }
 867
 868     /* estimate superstate matrix */
 869     for( i = 0; i < hmm->num_states; i++)
 870     {
 871         float total = 0;
 872         float inv_total;
 873         for( j = 0; j < hmm->num_states; j++)
 874         {
 875             total += hmm->transP[i * hmm->num_states + j];
 876         }
 877         //assert( total );
 878
 879         inv_total = total ? 1.f/total : 0;
 880
 881         for( j = 0; j < hmm->num_states; j++)
 882         {
 883             hmm->transP[i * hmm->num_states + j] =
 884                 hmm->transP[i * hmm->num_states + j] ?
 885                 (float)log( hmm->transP[i * hmm->num_states + j] * inv_total ) : -BIG_FLT;
 886         }
 887     }
 888
 889     return CV_NO_ERR;
 890 }
 891
 892
 893 /*F///////////////////////////////////////////////////////////////////////////////////////
 894 //    Name: MixSegmL2
 895 //    Purpose: The function implements the mixture segmentation of the states of the embedded HMM
 896 //    Context: used with the Viterbi training of the embedded HMM
 897 //
 898 //    Parameters:
 899 //             obs_info_array
 900 //             num_img
 901 //             hmm
 902 //    Returns: void
 903 //
 904 //    Notes:
 905 //F*/
 906 CvStatus icv1DMixSegmL2(CvImgObsInfo** obs_info_array, int num_img, CvEHMM* hmm )
 907 {
 908     int     k, i, m;
 909
 910     CvEHMMState* state = hmm->u.state;
 911
 912     for (k = 0; k < num_img; k++)
 913     {
 914         //int counter = 0;
 915         CvImgObsInfo* info = obs_info_array[k];
 916
 917         for (i = 0; i < info->obs_x; i++)
 918         {
 919             int e_state = info->state[i];
 920             float min_dist;
 921
 922             min_dist = icvSquareDistance((info->obs) + (i * info->obs_size),
 923                                                state[e_state].mu, info->obs_size);
 924             info->mix[i] = 0;
 925
 926             for (m = 1; m < state[e_state].num_mix; m++)
 927             {
 928                 float dist=icvSquareDistance( (info->obs) + (i * info->obs_size),
 929                                                state[e_state].mu + m * info->obs_size,
 930                                                info->obs_size);
 931                 if (dist < min_dist)
 932                 {
 933                     min_dist = dist;
 934                     /* assign mixture with smallest distance */
 935                     info->mix[i] = m;
 936                 }
 937             }
 938         }
 939     }
 940     return CV_NO_ERR;
 941 }
 942
 943 /*F///////////////////////////////////////////////////////////////////////////////////////
 944 //    Name: icvEViterbi
 945 //    Purpose: The function calculates the embedded Viterbi algorithm
 946 //             for 1 image
 947 //    Context:
 948 //    Parameters:
 949 //             obs_info - observations
 950 //             hmm      - HMM
 951 //
 952 //    Returns: the Embedded Viterbi probability (float)
 953 //             and do state segmentation of observations
 954 //
 955 //    Notes:
 956 //F*/
 957 float icvViterbi(Cv1DObsInfo* obs_info, CvEHMM* hmm)
 958 {
 959     int    i, counter;
 960     float  log_likelihood;
 961
 962     //CvEHMMState* first_state = hmm->u.state;
 963
 964     /* memory allocation for superB */
 965     /*CvMatr32f superB = picvCreateMatrix_32f(hmm->num_states, obs_info->obs_x );*/
 966
 967     /* memory allocation for q */
 968     int* super_q = (int*)icvAlloc( obs_info->obs_x * sizeof(int) );
 969
 970     /* perform Viterbi segmentation (process 1D HMM) */
 971     icvViterbiSegmentation( hmm->num_states, obs_info->obs_x,
 972                             hmm->transP, (float*)(hmm->obsProb), 0,
 973                             _CV_LAST_STATE, &super_q, obs_info->obs_x,
 974                              obs_info->obs_x, &log_likelihood );
 975
 976     log_likelihood /= obs_info->obs_x ;
 977
 978     counter = 0;
 979     /* assign new state to observation vectors */
 980     for (i = 0; i < obs_info->obs_x; i++)
 981     {
 982          int state = super_q[i];
 983          obs_info->state[i] = state;
 984     }
 985
 986     /* memory deallocation for superB */
 987     /*picvDeleteMatrix( superB );*/
 988     icvFree( &super_q );
 989
 990     return log_likelihood;
 991 }
 992
 993 CvStatus icvEstimate1DHMMStateParams(CvImgObsInfo** obs_info_array, int num_img, CvEHMM* hmm)
 994
 995 {
 996     /* compute gamma, weights, means, vars */
 997     int k, i, j, m;
 998     int counter = 0;
 999     int total = 0;
1000     int vect_len = obs_info_array[0]->obs_size;
1001
1002     float start_log_var_val = LN2PI * vect_len;
1003
1004     CvVect32f tmp_vect = icvCreateVector_32f( vect_len );
1005
1006     CvEHMMState* first_state = hmm->u.state;
1007
1008     assert( sizeof(float) == sizeof(int) );
1009
1010     total+= hmm->num_states;
1011
1012     /***************Gamma***********************/
1013     /* initialize gamma */
1014     for( i = 0; i < total; i++ )
1015     {
1016         for (m = 0; m < first_state[i].num_mix; m++)
1017         {
1018             ((int*)(first_state[i].weight))[m] = 0;
1019         }
1020     }
1021
1022     /* maybe gamma must be computed in mixsegm process ?? */
1023
1024     /* compute gamma */
1025     counter = 0;
1026     for (k = 0; k < num_img; k++)
1027     {
1028         CvImgObsInfo* info = obs_info_array[k];
1029         int num_obs = info->obs_y * info->obs_x;
1030
1031         for (i = 0; i < num_obs; i++)
1032         {
1033             int state, mixture;
1034             state = info->state[i];
1035             mixture = info->mix[i];
1036             /* computes gamma - number of observations corresponding
1037                to every mixture of every state */
1038             ((int*)(first_state[state].weight))[mixture] += 1;
1039         }
1040     }
1041     /***************Mean and Var***********************/
1042     /* compute means and variances of every item */
1043     /* initially variance placed to inv_var */
1044     /* zero mean and variance */
1045     for (i = 0; i < total; i++)
1046     {
1047         memset( (void*)first_state[i].mu, 0, first_state[i].num_mix * vect_len *
1048                                                                          sizeof(float) );
1049         memset( (void*)first_state[i].inv_var, 0, first_state[i].num_mix * vect_len *
1050                                                                          sizeof(float) );
1051     }
1052
1053     /* compute sums */
1054     for (i = 0; i < num_img; i++)
1055     {
1056         CvImgObsInfo* info = obs_info_array[i];
1057         int total_obs = info->obs_x;// * info->obs_y;
1058
1059         float* vector = info->obs;
1060
1061         for (j = 0; j < total_obs; j++, vector+=vect_len )
1062         {
1063             int state = info->state[j];
1064             int mixture = info->mix[j];
1065
1066             CvVect32f mean  = first_state[state].mu + mixture * vect_len;
1067             CvVect32f mean2 = first_state[state].inv_var + mixture * vect_len;
1068
1069             icvAddVector_32f( mean, vector, mean, vect_len );
1070             icvAddSquare_32f_C1IR( vector, vect_len * sizeof(float),
1071                                     mean2, vect_len * sizeof(float), cvSize(vect_len, 1) );
1072         }
1073     }
1074
1075     /*compute the means and variances */
1076     /* assume gamma already computed */
1077     counter = 0;
1078     for (i = 0; i < total; i++)
1079     {
1080         CvEHMMState* state = &(first_state[i]);
1081
1082         for (m = 0; m < state->num_mix; m++)
1083         {
1084             int k;
1085             CvVect32f mu  = state->mu + m * vect_len;
1086             CvVect32f invar = state->inv_var + m * vect_len;
1087
1088             if ( ((int*)state->weight)[m] > 1)
1089             {
1090                 float inv_gamma = 1.f/((int*)(state->weight))[m];
1091
1092                 icvScaleVector_32f( mu, mu, vect_len, inv_gamma);
1093                 icvScaleVector_32f( invar, invar, vect_len, inv_gamma);
1094             }
1095
1096             icvMulVectors_32f(mu, mu, tmp_vect, vect_len);
1097             icvSubVector_32f( invar, tmp_vect, invar, vect_len);
1098
1099             /* low bound of variance - 0.01 (Ara's experimental result) */
1100             for( k = 0; k < vect_len; k++ )
1101             {
1102                 invar[k] = (invar[k] > 0.01f) ? invar[k] : 0.01f;
1103             }
1104
1105             /* compute log_var */
1106             state->log_var_val[m] = start_log_var_val;
1107             for( k = 0; k < vect_len; k++ )
1108             {
1109                 state->log_var_val[m] += (float)log( invar[k] );
1110             }
1111
1112             state->log_var_val[m] *= 0.5;
1113
1114             /* compute inv_var = 1/sqrt(2*variance) */
1115             icvScaleVector_32f(invar, invar, vect_len, 2.f );
1116             icvbInvSqrt_32f(invar, invar, vect_len );
1117         }
1118     }
1119
1120     /***************Weights***********************/
1121     /* normilize gammas - i.e. compute mixture weights */
1122
1123     //compute weights
1124     for (i = 0; i < total; i++)
1125     {
1126         int gamma_total = 0;
1127         float norm;
1128
1129         for (m = 0; m < first_state[i].num_mix; m++)
1130         {
1131             gamma_total += ((int*)(first_state[i].weight))[m];
1132         }
1133
1134         norm = gamma_total ? (1.f/(float)gamma_total) : 0.f;
1135
1136         for (m = 0; m < first_state[i].num_mix; m++)
1137         {
1138             first_state[i].weight[m] = ((int*)(first_state[i].weight))[m] * norm;
1139         }
1140     }
1141
1142     icvDeleteVector( tmp_vect);
1143     return CV_NO_ERR;
1144 }
1145
1146
1147
1148
1149
1150 #endif
1151