cvlinefit.cpp

opencv库在TI DM6437上的移植,目前包括两个库cv.lib和cxcore.lib的工程

    int first = 1;
    float rdelta = reps != 0 ? reps : 1.0f;
    float adelta = aeps != 0 ? aeps : 0.01f;

    memset( line, 0, 4*sizeof(line[0]) );

    switch (dist)
    {
    case CV_DIST_L2:
        return icvFitLine2D_wods( points, count, 0, line );

    case CV_DIST_L1:
        calc_weights = icvWeightL1;
        break;

    case CV_DIST_L12:
        calc_weights = icvWeightL12;
        break;

    case CV_DIST_FAIR:
        calc_weights_param = icvWeightFair;
        break;

    case CV_DIST_WELSCH:
        calc_weights_param = icvWeightWelsch;
        break;

    case CV_DIST_HUBER:
        calc_weights_param = icvWeightHuber;
        break;

    /*case CV_DIST_USER:
        calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
        break;*/

    default:
        return CV_BADFACTOR_ERR;
    }

    w = (float *) cvAlloc( count * sizeof( float ));
    r = (float *) cvAlloc( count * sizeof( float ));

    for( i = 0; i < count; i++ )
        w[i] = 1.0f;

    icvFitLine2D_wods( points, count, 0, _line );
    for( i = 0; i < 100; i++ )
    {
        double sum_w = 0;

        if( first )
        {
            first = 0;
        }
        else
        {
            double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1];
            t = MAX(t,-1.);
            t = MIN(t,1.);
            if( fabs(acos(t)) < adelta )
            {
                float x, y, d;

                x = (float) fabs( _line[2] - _lineprev[2] );
                y = (float) fabs( _line[3] - _lineprev[3] );

                d = x > y ? x : y;
                if( d < rdelta )
                    goto _exit_;
            }
        }
        /* calculate distances */
        if( icvCalcDist2D( points, count, _line, r ) < FLT_EPSILON*count )
            break;

        /* calculate weights */
        if( calc_weights )
        {
            calc_weights( r, count, w );
        }
        else if( calc_weights_param )
        {
            calc_weights_param( r, count, w, _param );
        }
        else
            goto _exit_;

        for( j = 0; j < count; j++ )
            sum_w += w[j];

        if( fabs(sum_w) > FLT_EPSILON )
        {
            sum_w = 1./sum_w;
            for( j = 0; j < count; j++ )
                w[j] = (float)(w[j]*sum_w);
        }
        else
        {
            for( j = 0; j < count; j++ )
                w[j] = 1.f;
        }

        /* save the line parameters */
        memcpy( _lineprev, _line, 4 * sizeof( float ));

        /* Run again... */
        icvFitLine2D_wods( points, count, w, _line );
    }

_exit_:
    memcpy( line, _line, 4 * sizeof(line[0]));
    cvFree( &w );
    cvFree( &r );
    return CV_OK;
}


/* Takes an array of 3D points, type of distance (including user-defined 
distance specified by callbacks, fills the array of four floats with line 
parameters A, B, C, D, E, F, where (A, B, C) is the normalized direction vector, 
(D, E, F) is the point that belongs to the line. */

static CvStatus
icvFitLine3D( CvPoint3D32f * points, int count, int dist,
              float _param, float reps, float aeps, float *line )
{
    void (*calc_weights) (float *, int, float *) = 0;
    void (*calc_weights_param) (float *, int, float *, float) = 0;
    float *w;                   /* weights */
    float *r;                   /* square distances */
    int i, j;
    float _line[6], _lineprev[6];
    int first = 1;
    float rdelta = reps != 0 ? reps : 1.0f;
    float adelta = aeps != 0 ? aeps : 0.01f;

    memset( line, 0, 6*sizeof(line[0]) );

    switch (dist)
    {
    case CV_DIST_L2:
        return icvFitLine3D_wods( points, count, 0, line );

    case CV_DIST_L1:
        calc_weights = icvWeightL1;
        break;

    case CV_DIST_L12:
        calc_weights = icvWeightL12;
        break;

    case CV_DIST_FAIR:
        calc_weights_param = icvWeightFair;
        break;

    case CV_DIST_WELSCH:
        calc_weights_param = icvWeightWelsch;
        break;

    case CV_DIST_HUBER:
        calc_weights_param = icvWeightHuber;
        break;

    /*case CV_DIST_USER:
        _PFP.p = param;
        calc_weights = (void ( * )(float *, int, float *)) _PFP.fp;
        break;*/

    default:
        return CV_BADFACTOR_ERR;
    }

    w = (float *) cvAlloc( count * sizeof( float ));
    r = (float *) cvAlloc( count * sizeof( float ));

    for( i = 0; i < count; i++ )
        w[i] = 1.0f;

    icvFitLine3D_wods( points, count, 0, _line );
    for( i = 0; i < 100; i++ )
    {
        double sum_w = 0;
        
        if( first )
        {
            first = 0;
        }
        else
        {
            double t = _line[0] * _lineprev[0] + _line[1] * _lineprev[1] + _line[2] * _lineprev[2];
            t = MAX(t,-1.);
            t = MIN(t,1.);
            if( fabs(acos(t)) < adelta )
            {
                float x, y, z, ax, ay, az, dx, dy, dz, d;

                x = _line[3] - _lineprev[3];
                y = _line[4] - _lineprev[4];
                z = _line[5] - _lineprev[5];
                ax = _line[0] - _lineprev[0];
                ay = _line[1] - _lineprev[1];
                az = _line[2] - _lineprev[2];
                dx = (float) fabs( y * az - z * ay );
                dy = (float) fabs( z * ax - x * az );
                dz = (float) fabs( x * ay - y * ax );

                d = dx > dy ? (dx > dz ? dx : dz) : (dy > dz ? dy : dz);
                if( d < rdelta )
                    goto _exit_;
            }
        }
        /* calculate distances */
        if( icvCalcDist3D( points, count, _line, r ) < FLT_EPSILON*count )
            break;

        /* calculate weights */
        if( calc_weights )
        {
            calc_weights( r, count, w );
        }
        else if( calc_weights_param )
        {
            calc_weights_param( r, count, w, _param );
        }
        else
            goto _exit_;

        for( j = 0; j < count; j++ )
            sum_w += w[j];

        if( fabs(sum_w) > FLT_EPSILON )
        {
            sum_w = 1./sum_w;
            for( j = 0; j < count; j++ )
                w[j] = (float)(w[j]*sum_w);
        }
        else
        {
            for( j = 0; j < count; j++ )
                w[j] = 1.f;
        }

        /* save the line parameters */
        memcpy( _lineprev, _line, 6 * sizeof( float ));

        /* Run again... */
        icvFitLine3D_wods( points, count, w, _line );
    }
_exit_:
    // Return...
    memcpy( line, _line, 6 * sizeof(line[0]));
    cvFree( &w );
    cvFree( &r );
    return CV_OK;
}


CV_IMPL void
cvFitLine( const CvArr* array, int dist, double param,
           double reps, double aeps, float *line )
{
    char* buffer = 0;
    CV_FUNCNAME( "cvFitLine" );

    __BEGIN__;

    char* points = 0;
    union { CvContour contour; CvSeq seq; } header;
    CvSeqBlock block;
    CvSeq* ptseq = (CvSeq*)array;
    int type;

    if( !line )
        CV_ERROR( CV_StsNullPtr, "NULL pointer to line parameters" );

    if( CV_IS_SEQ(ptseq) )
    {
        type = CV_SEQ_ELTYPE(ptseq);
        if( ptseq->total == 0 )
            CV_ERROR( CV_StsBadSize, "The sequence has no points" );
        if( (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) ||
            CV_ELEM_SIZE(type) != ptseq->elem_size )
            CV_ERROR( CV_StsUnsupportedFormat,
                "Input sequence must consist of 2d points or 3d points" );
    }
    else
    {
        CvMat* mat = (CvMat*)array;
        type = CV_MAT_TYPE(mat->type);
        if( !CV_IS_MAT(mat))
            CV_ERROR( CV_StsBadArg, "Input array is not a sequence nor matrix" );

        if( !CV_IS_MAT_CONT(mat->type) ||
            (type!=CV_32FC2 && type!=CV_32FC3 && type!=CV_32SC2 && type!=CV_32SC3) ||
            (mat->width != 1 && mat->height != 1))
            CV_ERROR( CV_StsBadArg,
            "Input array must be 1d continuous array of 2d or 3d points" );

        CV_CALL( ptseq = cvMakeSeqHeaderForArray(
            CV_SEQ_KIND_GENERIC|type, sizeof(CvContour), CV_ELEM_SIZE(type), mat->data.ptr,
            mat->width + mat->height - 1, &header.seq, &block ));
    }

    if( reps < 0 || aeps < 0 )
        CV_ERROR( CV_StsOutOfRange, "Both reps and aeps must be non-negative" );

    if( CV_MAT_DEPTH(type) == CV_32F && ptseq->first->next == ptseq->first )
    {
        /* no need to copy data in this case */
        points = ptseq->first->data;
    }
    else
    {
        CV_CALL( buffer = points = (char*)cvAlloc( ptseq->total*CV_ELEM_SIZE(type) ));
        CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ ));

        if( CV_MAT_DEPTH(type) != CV_32F )
        {
            int i, total = ptseq->total*CV_MAT_CN(type);
            assert( CV_MAT_DEPTH(type) == CV_32S );

            for( i = 0; i < total; i++ )
                ((float*)points)[i] = (float)((int*)points)[i];
        }
    }

    if( dist == CV_DIST_USER )
        CV_ERROR( CV_StsBadArg, "User-defined distance is not allowed" );

    if( CV_MAT_CN(type) == 2 )
    {
        IPPI_CALL( icvFitLine2D( (CvPoint2D32f*)points, ptseq->total,
                                 dist, (float)param, (float)reps, (float)aeps, line ));
    }
    else
    {
        IPPI_CALL( icvFitLine3D( (CvPoint3D32f*)points, ptseq->total,
                                 dist, (float)param, (float)reps, (float)aeps, line ));
    }

    __END__;

    cvFree( &buffer );
}

/* End of file. */