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;
}
