/*F/////////////////////////////////////////////////////////////////////////////////////// // Name: cvConDensUpdateByTime // Purpose: Performing Time Update routine for ConDensation algorithm // Context: // Parameters: // Kalman - pointer to CvConDensation structure // Returns: // Notes: // //F*/ CV_IMPL void cvConDensUpdateByTime( CvConDensation * ConDens ) { int i, j; float Sum = 0; CV_FUNCNAME( "cvConDensUpdateByTime" ); __BEGIN__; if( !ConDens ) CV_ERROR( CV_StsNullPtr, "" ); /* Sets Temp to Zero */ icvSetZero_32f( ConDens->Temp, ConDens->DP, 1 ); /* Calculating the Mean */ for( i = 0; i < ConDens->SamplesNum; i++ ) { icvScaleVector_32f( ConDens->flSamples[i], ConDens->State, ConDens->DP, ConDens->flConfidence[i] ); icvAddVector_32f( ConDens->Temp, ConDens->State, ConDens->Temp, ConDens->DP ); Sum += ConDens->flConfidence[i]; ConDens->flCumulative[i] = Sum; } /* Taking the new vector from transformation of mean by dynamics matrix */ icvScaleVector_32f( ConDens->Temp, ConDens->Temp, ConDens->DP, 1.f / Sum ); icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP, ConDens->DP ); Sum = Sum / ConDens->SamplesNum; /* Updating the set of random samples */ //重采样,将新采样出来的粒子保存在flNewSamples中 for( i = 0; i < ConDens->SamplesNum; i++ ) { j = 0; while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1)) { j++; } icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] ); } /* Adding the random-generated vector to every vector in sample set */ for( i = 0; i < ConDens->SamplesNum; i++ ) { //为每个新粒子产生一个随即量 for( j = 0; j < ConDens->DP; j++ ) { cvbRand( ConDens->RandS + j, ConDens->RandomSample + j, 1 ); } //将flNewSamples加一个随即量,保存入flSamples中 icvTransformVector_32f( ConDens->DynamMatr, ConDens->flNewSamples[i], ConDens->flSamples[i], ConDens->DP, ConDens->DP ); icvAddVector_32f( ConDens->flSamples[i], ConDens->RandomSample, ConDens->flSamples[i], ConDens->DP ); } __END__; }
static CvStatus CV_STDCALL icvFindHandRegion( CvPoint3D32f * points, int count, CvSeq * indexs, float *line, CvSize2D32f size, int flag, CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers ) { /* IppmVect32f sub, cros; */ float *sub, *cros; CvSeqWriter writer; CvSeqReader reader; CvStatus status; int nbins = 20, i, l, i_point, left, right; int *bin_counts = 0; // pointer to the point's counter in the bickets int low_count; // low threshold CvPoint *tmp_number = 0, *pt; float value, vmin, vmax, vl, bsize, vc; float hand_length, hand_length2, hand_left, hand_right; float threshold, threshold2; float *vv = 0; float a[3]; status = CV_OK; hand_length = size.width; hand_length2 = hand_length / 2; threshold = (float) (size.height * 3 / 5.); threshold2 = threshold * threshold; /* low_count = count/nbins; */ low_count = (int) (count / 60.); assert( points != NULL && line != NULL ); if( points == NULL || line == NULL ) return CV_NULLPTR_ERR; assert( count > 5 ); if( count < 5 ) return CV_BADFLAG_ERR; assert( flag == 0 || flag == 1 ); if( flag != 0 && flag != 1 ) return CV_BADFLAG_ERR; /* create vectors */ sub = icvCreateVector_32f( 3 ); cros = icvCreateVector_32f( 3 ); if( sub == NULL || cros == NULL ) return CV_OUTOFMEM_ERR; /* alloc memory for the point's projections on the line */ vv = (float *) cvAlloc( count * sizeof( float )); if( vv == NULL ) return CV_OUTOFMEM_ERR; /* alloc memory for the point's counter in the bickets */ bin_counts = (int *) cvAlloc( nbins * sizeof( int )); if( bin_counts == NULL ) { status = CV_OUTOFMEM_ERR; goto M_END; } memset( bin_counts, 0, nbins * sizeof( int )); cvStartReadSeq( indexs, &reader, 0 ); /* alloc memory for the temporale point's numbers */ tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint )); if( tmp_number == NULL ) { status = CV_OUTOFMEM_ERR; goto M_END; } /* find min and max point's projection on the line */ vmin = 1000; vmax = -1000; i_point = 0; for( i = 0; i < count; i++ ) { /* icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3); icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros); */ sub[0] = points[i].x - line[3]; sub[1] = points[i].y - line[4]; sub[2] = points[i].z - line[5]; a[0] = sub[0] * line[1] - sub[1] * line[0]; a[1] = sub[1] * line[2] - sub[2] * line[1]; a[2] = sub[2] * line[0] - sub[0] * line[2]; /* if(IPPI_NORM_L22 ( cros ) < threshold2) */ if( _CV_NORM_L22( a ) < threshold2 ) { value = (float)icvDotProduct_32f( sub, &line[0], 3 ); if( value > vmax ) vmax = value; if( value < vmin ) vmin = value; vv[i_point] = value; pt = (CvPoint*)cvGetSeqElem( indexs, i ); tmp_number[i_point] = *pt; i_point++; } } /* compute the length of one bucket */ vl = vmax - vmin; bsize = vl / nbins; /* compute the number of points in each bucket */ for( i = 0; i < i_point; i++ ) { l = cvRound( (vv[i] - vmin) / bsize ); bin_counts[l]++; } *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage ); assert( numbers != 0 ); if( numbers == NULL ) { status = CV_OUTOFMEM_ERR; goto M_END; } cvStartAppendToSeq( *numbers, &writer ); if( flag == 0 ) { /* find the leftmost bucket */ for( l = 0; l < nbins; l++ ) { if( bin_counts[l] > low_count ) break; } left = l; /* compute center point of the left hand */ hand_left = vmin + left * bsize; vc = hand_left + hand_length2; hand_right = hand_left + hand_length; } else { /* find the rightmost bucket */ for( l = nbins - 1; l >= 0; l-- ) { if( bin_counts[l] > low_count ) break; } right = l; /* compute center point of the right hand */ hand_right = vmax - (nbins - right - 1) * bsize; vc = hand_right - hand_length2; hand_left = hand_right - hand_length; } icvScaleVector_32f( &line[0], sub, 3, vc ); icvAddVector_32f( &line[3], sub, (float *) center, 3 ); /* select hand's points and calculate mean value */ //ss.x = ss.y = ss.z = 0; for( l = 0; l < i_point; l++ ) { if( vv[l] >= hand_left && vv[l] <= hand_right ) { CV_WRITE_SEQ_ELEM( tmp_number[l], writer ); } } cvEndWriteSeq( &writer ); M_END: if( tmp_number != NULL ) cvFree( &tmp_number ); if( bin_counts != NULL ) cvFree( &bin_counts ); if( vv != NULL ) cvFree( &vv ); if( sub != NULL ) icvDeleteVector (sub); if( cros != NULL ) icvDeleteVector (cros); return status; }
static CvStatus CV_STDCALL icvFindHandRegionA( CvPoint3D32f * points, int count, CvSeq * indexs, float *line, CvSize2D32f size, int jc, CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers ) { /* IppmVect32f sub, cros; */ float *sub, *cros; float eps = (float) 0.01; CvSeqWriter writer; CvSeqReader reader; CvStatus status; float gor[3] = { 1, 0, 0 }; float ver[3] = { 0, 1, 0 }; int nbins = 20, i, l, i_point, left, right, jmin, jmax, jl; int j_left, j_right; int *bin_counts = 0; // pointer to the point's counter in the bickets // int *bin_countsj = 0; // pointer to the index's counter in the bickets int low_count; // low threshold CvPoint *tmp_number = 0, *pt; float value, vmin, vmax, vl, bsize, bsizej, vc, vcl, vcr; double v_ver, v_gor; float hand_length, hand_length2, hand_left, hand_right; float threshold, threshold2; float *vv = 0; float a[3]; char log; status = CV_OK; hand_length = size.width; hand_length2 = hand_length / 2; threshold = (float) (size.height * 3 / 5.); threshold2 = threshold * threshold; /* low_count = count/nbins; */ low_count = (int) (count / 60.); assert( points != NULL && line != NULL ); if( points == NULL || line == NULL ) return CV_NULLPTR_ERR; assert( count > 5 ); if( count < 5 ) return CV_BADFLAG_ERR; /* create vectors */ sub = icvCreateVector_32f( 3 ); cros = icvCreateVector_32f( 3 ); if( sub == NULL || cros == NULL ) return CV_OUTOFMEM_ERR; /* alloc memory for the point's projections on the line */ vv = (float *) cvAlloc( count * sizeof( float )); if( vv == NULL ) return CV_OUTOFMEM_ERR; /* alloc memory for the point's counter in the bickets */ bin_counts = (int *) cvAlloc( nbins * sizeof( int )); if( bin_counts == NULL ) { status = CV_OUTOFMEM_ERR; goto M_END; } memset( bin_counts, 0, nbins * sizeof( int )); /* alloc memory for the point's counter in the bickets */ // bin_countsj = (int*) icvAlloc(nbins*sizeof(int)); // if(bin_countsj == NULL) {status = CV_OUTOFMEM_ERR; goto M_END;} // memset(bin_countsj,0,nbins*sizeof(int)); cvStartReadSeq( indexs, &reader, 0 ); /* alloc memory for the temporale point's numbers */ tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint )); if( tmp_number == NULL ) { status = CV_OUTOFMEM_ERR; goto M_END; } /* find min and max point's projection on the line */ vmin = 1000; vmax = -1000; jmin = 1000; jmax = -1000; i_point = 0; for( i = 0; i < count; i++ ) { /* icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3); icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros); */ sub[0] = points[i].x - line[3]; sub[1] = points[i].y - line[4]; sub[2] = points[i].z - line[5]; // if(fabs(sub[0])<eps||fabs(sub[1])<eps||fabs(sub[2])<eps) continue; a[0] = sub[0] * line[1] - sub[1] * line[0]; a[1] = sub[1] * line[2] - sub[2] * line[1]; a[2] = sub[2] * line[0] - sub[0] * line[2]; v_gor = icvDotProduct_32f( gor, &line[0], 3 ); v_ver = icvDotProduct_32f( ver, &line[0], 3 ); if( v_ver > v_gor ) log = true; else log = false; /* if(IPPI_NORM_L22 ( cros ) < threshold2) */ /* if(fabs(a[0])<eps && fabs(a[1])<eps && fabs(a[2])<eps) { icvDotProduct_32f( sub, &line[0], 3, &value); if(value > vmax) vmax = value; if(value < vmin) vmin = value; vv[i_point] = value; pt = (CvPoint* )icvGetSeqElem ( indexs, i, 0); if(pt->x > jmax) jmax = pt->x; if(pt->x < jmin) jmin = pt->x; tmp_number[i_point] = *pt; i_point++; } else */ { if( _CV_NORM_L32( a ) < threshold2 ) { value = (float)icvDotProduct_32f( sub, &line[0], 3 ); if( value > vmax ) vmax = value; if( value < vmin ) vmin = value; vv[i_point] = value; pt = (CvPoint*)cvGetSeqElem( indexs, i ); if( !log ) { if( pt->x > jmax ) jmax = pt->x; if( pt->x < jmin ) jmin = pt->x; } else { if( pt->y > jmax ) jmax = pt->y; if( pt->y < jmin ) jmin = pt->y; } tmp_number[i_point] = *pt; i_point++; } } } /* compute the length of one bucket along the line */ vl = vmax - vmin; /* examining on the arm's existence */ if( vl < eps ) { *numbers = NULL; status = CV_OK; goto M_END; } bsize = vl / nbins; /* compute the number of points in each bucket along the line */ for( i = 0; i < i_point; i++ ) { l = cvRound( (vv[i] - vmin) / bsize ); bin_counts[l]++; } /* compute the length of one bucket along the X axe */ jl = jmax - jmin; if( jl <= 1 ) { *numbers = NULL; status = CV_OK; goto M_END; } bsizej = (float) (jl / (nbins + 0.)); /* compute the number of points in each bucket along the X axe */ // for(i=0;i<i_point;i++) // { // l = cvRound((tmp_number[i].x - jmin)/bsizej); // bin_countsj[l]++; // } left = right = -1; /* find the leftmost and the rightmost buckets */ for( l = 0; l < nbins; l++ ) { if( bin_counts[l] > low_count && left == -1 ) left = l; else if( bin_counts[l] > low_count && left >= 0 ) right = l; } /* compute center point of the left hand */ if( left == -1 && right == -1 ) { *numbers = NULL; status = CV_OK; goto M_END; } hand_left = vmin + left * bsize; j_left = (int) (jmin + left * bsizej); vcl = hand_left + hand_length2; /* compute center point of the right hand */ hand_right = vmax - (nbins - right - 1) * bsize; vcr = hand_right - hand_length2; j_right = (int) (jmax - (nbins - right - 1) * bsizej); j_left = abs( j_left - jc ); j_right = abs( j_right - jc ); if( j_left <= j_right ) { hand_right = hand_left + hand_length; vc = vcl; } else { hand_left = hand_right - hand_length; vc = vcr; } icvScaleVector_32f( &line[0], sub, 3, vc ); icvAddVector_32f( &line[3], sub, (float *) center, 3 ); /* select hand's points and calculate mean value */ *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage ); assert( *numbers != 0 ); if( *numbers == NULL ) { status = CV_OUTOFMEM_ERR; goto M_END; } cvStartAppendToSeq( *numbers, &writer ); for( l = 0; l < i_point; l++ ) { if( vv[l] >= hand_left && vv[l] <= hand_right ) { CV_WRITE_SEQ_ELEM( tmp_number[l], writer ); } } cvEndWriteSeq( &writer ); M_END: if( tmp_number != NULL ) cvFree( &tmp_number ); // if(bin_countsj != NULL) cvFree( &bin_countsj ); if( bin_counts != NULL ) cvFree( &bin_counts ); if( vv != NULL ) cvFree( &vv ); if( sub != NULL ) icvDeleteVector (sub); if( cros != NULL ) icvDeleteVector (cros); return status; }
CvStatus icvEstimate1DHMMStateParams(CvImgObsInfo** obs_info_array, int num_img, CvEHMM* hmm) { /* compute gamma, weights, means, vars */ int k, i, j, m; int counter = 0; int total = 0; int vect_len = obs_info_array[0]->obs_size; float start_log_var_val = LN2PI * vect_len; CvVect32f tmp_vect = icvCreateVector_32f( vect_len ); CvEHMMState* first_state = hmm->u.state; assert( sizeof(float) == sizeof(int) ); total+= hmm->num_states; /***************Gamma***********************/ /* initialize gamma */ for( i = 0; i < total; i++ ) { for (m = 0; m < first_state[i].num_mix; m++) { ((int*)(first_state[i].weight))[m] = 0; } } /* maybe gamma must be computed in mixsegm process ?? */ /* compute gamma */ counter = 0; for (k = 0; k < num_img; k++) { CvImgObsInfo* info = obs_info_array[k]; int num_obs = info->obs_y * info->obs_x; for (i = 0; i < num_obs; i++) { int state, mixture; state = info->state[i]; mixture = info->mix[i]; /* computes gamma - number of observations corresponding to every mixture of every state */ ((int*)(first_state[state].weight))[mixture] += 1; } } /***************Mean and Var***********************/ /* compute means and variances of every item */ /* initially variance placed to inv_var */ /* zero mean and variance */ for (i = 0; i < total; i++) { memset( (void*)first_state[i].mu, 0, first_state[i].num_mix * vect_len * sizeof(float) ); memset( (void*)first_state[i].inv_var, 0, first_state[i].num_mix * vect_len * sizeof(float) ); } /* compute sums */ for (i = 0; i < num_img; i++) { CvImgObsInfo* info = obs_info_array[i]; int total_obs = info->obs_x;// * info->obs_y; float* vector = info->obs; for (j = 0; j < total_obs; j++, vector+=vect_len ) { int state = info->state[j]; int mixture = info->mix[j]; CvVect32f mean = first_state[state].mu + mixture * vect_len; CvVect32f mean2 = first_state[state].inv_var + mixture * vect_len; icvAddVector_32f( mean, vector, mean, vect_len ); icvAddSquare_32f_C1IR( vector, vect_len * sizeof(float), mean2, vect_len * sizeof(float), cvSize(vect_len, 1) ); } } /*compute the means and variances */ /* assume gamma already computed */ counter = 0; for (i = 0; i < total; i++) { CvEHMMState* state = &(first_state[i]); for (m = 0; m < state->num_mix; m++) { int k; CvVect32f mu = state->mu + m * vect_len; CvVect32f invar = state->inv_var + m * vect_len; if ( ((int*)state->weight)[m] > 1) { float inv_gamma = 1.f/((int*)(state->weight))[m]; icvScaleVector_32f( mu, mu, vect_len, inv_gamma); icvScaleVector_32f( invar, invar, vect_len, inv_gamma); } icvMulVectors_32f(mu, mu, tmp_vect, vect_len); icvSubVector_32f( invar, tmp_vect, invar, vect_len); /* low bound of variance - 0.01 (Ara's experimental result) */ for( k = 0; k < vect_len; k++ ) { invar[k] = (invar[k] > 0.01f) ? invar[k] : 0.01f; } /* compute log_var */ state->log_var_val[m] = start_log_var_val; for( k = 0; k < vect_len; k++ ) { state->log_var_val[m] += (float)log( invar[k] ); } state->log_var_val[m] *= 0.5; /* compute inv_var = 1/sqrt(2*variance) */ icvScaleVector_32f(invar, invar, vect_len, 2.f ); icvbInvSqrt_32f(invar, invar, vect_len ); } } /***************Weights***********************/ /* normilize gammas - i.e. compute mixture weights */ //compute weights for (i = 0; i < total; i++) { int gamma_total = 0; float norm; for (m = 0; m < first_state[i].num_mix; m++) { gamma_total += ((int*)(first_state[i].weight))[m]; } norm = gamma_total ? (1.f/(float)gamma_total) : 0.f; for (m = 0; m < first_state[i].num_mix; m++) { first_state[i].weight[m] = ((int*)(first_state[i].weight))[m] * norm; } } icvDeleteVector( tmp_vect); 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; }