CvStatus icvForward1DHMM( int num_states, int num_obs, CvMatr64d A, CvMatr64d B, double* scales) { // assume that observation and transition // probabilities already computed int m_HMMType = _CV_CAUSAL; double* m_pi = icvAlloc( num_states* sizeof( double) ); /* alpha is matrix rows throuhg states columns through time */ double* alpha = icvAlloc( num_states*num_obs * sizeof( double ) ); /* All calculations will be in non-logarithmic domain */ /* Initialization */ /* set initial state probabilities */ m_pi[0] = 1; for (i = 1; i < num_states; i++) { m_pi[i] = 0.0; } for (i = 0; i < num_states; i++) { alpha[i] = m_pi[i] * m_b[ i]; } /******************************************************************/ /* Induction */ if ( m_HMMType == _CV_ERGODIC ) { int t; for (t = 1 ; t < num_obs; t++) { for (j = 0; j < num_states; j++) { double sum = 0.0; int i; for (i = 0; i < num_states; i++) { sum += alpha[(t - 1) * num_states + i] * A[i * num_states + j]; } alpha[(t - 1) * num_states + j] = sum * B[t * num_states + j]; /* add computed alpha to scale factor */ sum_alpha += alpha[(t - 1) * num_states + j]; } double scale = 1/sum_alpha; /* scale alpha */ for (j = 0; j < num_states; j++) { alpha[(t - 1) * num_states + j] *= scale; } scales[t] = scale; } } #endif //*F/////////////////////////////////////////////////////////////////////////////////////// // Name: icvCreateObsInfo // Purpose: The function allocates memory for CvImgObsInfo structure // and its inner stuff // Context: // Parameters: obs_info - addres of pointer to CvImgObsInfo structure // num_hor_obs - number of horizontal observation vectors // num_ver_obs - number of horizontal observation vectors // obs_size - length of observation vector // // Returns: error status // // Notes: //F*/ /*CvStatus icvCreateObsInfo( CvImgObsInfo** obs_info, CvSize num_obs, int obs_size ) { int total = num_obs.height * num_obs.width; CvImgObsInfo* obs = (CvImgObsInfo*)icvAlloc( sizeof( CvImgObsInfo) ); obs->obs_x = num_obs.width; obs->obs_y = num_obs.height; obs->obs = (float*)icvAlloc( total * obs_size * sizeof(float) ); obs->state = (int*)icvAlloc( 2 * total * sizeof(int) ); obs->mix = (int*)icvAlloc( total * sizeof(int) ); obs->obs_size = obs_size; obs_info[0] = obs; return CV_NO_ERR; }*/ /*CvStatus icvReleaseObsInfo( CvImgObsInfo** p_obs_info ) { CvImgObsInfo* obs_info = p_obs_info[0]; icvFree( &(obs_info->obs) ); icvFree( &(obs_info->mix) ); icvFree( &(obs_info->state) ); icvFree( &(obs_info) ); p_obs_info[0] = NULL; return CV_NO_ERR; } */ //*F/////////////////////////////////////////////////////////////////////////////////////// // Name: icvCreate1DHMM // Purpose: The function allocates memory for 1-dimensional HMM // and its inner stuff // Context: // Parameters: hmm - addres of pointer to CvEHMM structure // state_number - number of states in HMM // num_mix - number of gaussian mixtures in HMM states // size of array is defined by previous parameter // obs_size - length of observation vectors // // Returns: error status // Notes: //F*/ CvStatus icvCreate1DHMM( CvEHMM** this_hmm, int state_number, int* num_mix, int obs_size ) { int i; int real_states = state_number; CvEHMMState* all_states; CvEHMM* hmm; int total_mix = 0; float* pointers; /* allocate memory for hmm */ hmm = (CvEHMM*)icvAlloc( sizeof(CvEHMM) ); /* set number of superstates */ hmm->num_states = state_number; hmm->level = 0; /* allocate memory for all states */ all_states = (CvEHMMState *)icvAlloc( real_states * sizeof( CvEHMMState ) ); /* assign number of mixtures */ for( i = 0; i < real_states; i++ ) { all_states[i].num_mix = num_mix[i]; } /* compute size of inner of all real states */ for( i = 0; i < real_states; i++ ) { total_mix += num_mix[i]; } /* allocate memory for states stuff */ pointers = (float*)icvAlloc( total_mix * (2/*for mu invvar */ * obs_size + 2/*for weight and log_var_val*/ ) * sizeof( float) ); /* organize memory */ for( i = 0; i < real_states; i++ ) { all_states[i].mu = pointers; pointers += num_mix[i] * obs_size; all_states[i].inv_var = pointers; pointers += num_mix[i] * obs_size; all_states[i].log_var_val = pointers; pointers += num_mix[i]; all_states[i].weight = pointers; pointers += num_mix[i]; } hmm->u.state = all_states; hmm->transP = icvCreateMatrix_32f( hmm->num_states, hmm->num_states ); hmm->obsProb = NULL; /* if all ok - return pointer */ *this_hmm = hmm; return CV_NO_ERR; }
/* for now this function works bad with singular cases You can see in the code, that when some troubles with matrices or some variables occur - box filled with zero values is returned. However in general function works fine. */ static CvStatus icvFitEllipse_32f( CvSeq* points, CvBox2D* box ) { CvStatus status = CV_OK; float u[6]; CvMatr32f D = 0; float S[36]; /* S = D' * D */ float C[36]; float INVQ[36]; /* transposed eigenvectors */ float INVEIGV[36]; /* auxulary matrices */ float TMP1[36]; float TMP2[36]; int i, index = -1; float eigenvalues[6]; float a, b, c, d, e, f; float offx, offy; float *matr; int n = points->total; CvSeqReader reader; int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2; CvMat _S, _EIGVECS, _EIGVALS; /* create matrix D of input points */ D = icvCreateMatrix_32f( 6, n ); offx = offy = 0; cvStartReadSeq( points, &reader ); /* shift all points to zero */ for( i = 0; i < n; i++ ) { if( !is_float ) { offx += (float)((CvPoint*)reader.ptr)->x; offy += (float)((CvPoint*)reader.ptr)->y; } else { offx += ((CvPoint2D32f*)reader.ptr)->x; offy += ((CvPoint2D32f*)reader.ptr)->y; } CV_NEXT_SEQ_ELEM( points->elem_size, reader ); } c = 1.f / n; offx *= c; offy *= c; /* fill matrix rows as (x*x, x*y, y*y, x, y, 1 ) */ matr = D; for( i = 0; i < n; i++ ) { float x, y; if( !is_float ) { x = (float)((CvPoint*)reader.ptr)->x - offx; y = (float)((CvPoint*)reader.ptr)->y - offy; } else { x = ((CvPoint2D32f*)reader.ptr)->x - offx; y = ((CvPoint2D32f*)reader.ptr)->y - offy; } CV_NEXT_SEQ_ELEM( points->elem_size, reader ); matr[0] = x * x; matr[1] = x * y; matr[2] = y * y; matr[3] = x; matr[4] = y; matr[5] = 1.f; matr += 6; } /* compute S */ icvMulTransMatrixR_32f( D, 6, n, S ); /* fill matrix C */ icvSetZero_32f( C, 6, 6 ); C[2] = 2.f; //icvSetElement_32f( C, 6, 6, 0, 2, 2.f ); C[7] = -1.f; //icvSetElement_32f( C, 6, 6, 1, 1, -1.f ); C[12] = 2.f; //icvSetElement_32f( C, 6, 6, 2, 0, 2.f ); /* find eigenvalues */ //status1 = icvJacobiEigens_32f( S, INVEIGV, eigenvalues, 6, 0.f ); //assert( status1 == CV_OK ); _S = cvMat( 6, 6, CV_32F, S ); _EIGVECS = cvMat( 6, 6, CV_32F, INVEIGV ); _EIGVALS = cvMat( 6, 1, CV_32F, eigenvalues ); cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 ); //avoid troubles with small negative values for( i = 0; i < 6; i++ ) eigenvalues[i] = (float)fabs(eigenvalues[i]); cvbSqrt( eigenvalues, eigenvalues, 6 ); cvbInvSqrt( eigenvalues, eigenvalues, 6 ); for( i = 0; i < 6; i++ ) icvScaleVector_32f( &INVEIGV[i * 6], &INVEIGV[i * 6], 6, eigenvalues[i] ); // INVQ = transp(INVEIGV) * INVEIGV icvMulTransMatrixR_32f( INVEIGV, 6, 6, INVQ ); /* create matrix INVQ*C*INVQ */ icvMulMatrix_32f( INVQ, 6, 6, C, 6, 6, TMP1 ); icvMulMatrix_32f( TMP1, 6, 6, INVQ, 6, 6, TMP2 ); /* find its eigenvalues and vectors */ //status1 = icvJacobiEigens_32f( TMP2, INVEIGV, eigenvalues, 6, 0.f ); //assert( status1 == CV_OK ); _S = cvMat( 6, 6, CV_32F, TMP2 ); cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 ); /* search for positive eigenvalue */ for( i = 0; i < 3; i++ ) { if( eigenvalues[i] > 0 ) { index = i; break; } } /* only 3 eigenvalues must be not zero and only one of them must be positive if it is not true - return zero result */ if( index == -1 ) { box->center.x = box->center.y = box->size.width = box->size.height = box->angle = 0.f; goto error; } /* now find truthful eigenvector */ icvTransformVector_32f( INVQ, &INVEIGV[index * 6], u, 6, 6 ); /* extract vector components */ a = u[0]; b = u[1]; c = u[2]; d = u[3]; e = u[4]; f = u[5]; { /* extract ellipse axes from above values */ /* 1) find center of ellipse it satisfy equation | a b/2 | * | x0 | + | d/2 | = |0 | | b/2 c | | y0 | | e/2 | |0 | */ float x0, y0; float idet = 1.f / (a * c - b * b * 0.25f); /* we must normalize (a b c d e f ) to fit (4ac-b^2=1) */ float scale = cvSqrt( 0.25f * idet ); if (!scale) { box->center.x = box->center.y = box->size.width = box->size.height = box->angle = 0.f; goto error; } a *= scale; b *= scale; c *= scale; d *= scale; e *= scale; f *= scale; //x0 = box->center.x = (-d * c * 0.5f + e * b * 0.25f) * 4.f; //y0 = box->center.y = (-a * e * 0.5f + d * b * 0.25f) * 4.f; x0 = box->center.x = (-d * c + e * b * 0.5f) * 2.f; y0 = box->center.y = (-a * e + d * b * 0.5f) * 2.f; /* offset ellipse to (x0,y0) */ /* new f == F(x0,y0) */ f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0; if (!f) { box->center.x = box->center.y = box->size.width = box->size.height = box->angle = 0.f; goto error; } scale = -1.f / f; /* normalize to f = 1 */ a *= scale; b *= scale; c *= scale; } /* recover center */ box->center.x += offx; box->center.y += offy; /* extract axis of ellipse */ /* one more eigenvalue operation */ TMP1[0] = a; TMP1[1] = TMP1[2] = b * 0.5f; TMP1[3] = c; //status1 = icvJacobiEigens_32f( TMP1, INVEIGV, eigenvalues, 2, 0.f ); //assert( status1 == CV_OK ); _S = cvMat( 2, 2, CV_32F, TMP1 ); _EIGVECS = cvMat( 2, 2, CV_32F, INVEIGV ); _EIGVALS = cvMat( 2, 1, CV_32F, eigenvalues ); cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 ); /* exteract axis length from eigenvectors */ box->size.height = 2 * cvInvSqrt( eigenvalues[0] ); box->size.width = 2 * cvInvSqrt( eigenvalues[1] ); if ( !(box->size.height && box->size.width) ) { assert(0); } /* calc angle */ box->angle = cvFastArctan( INVEIGV[3], INVEIGV[2] ); error: if( D ) icvDeleteMatrix( D ); return status; }