Ejemplo n.º 1
0
/*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;

    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 */
    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 );
        }

        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 );
    }
}
Ejemplo n.º 2
0
/* 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;
}