cvlinefit.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 686 行 · 第 1/2 页

    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 *) icvAlloc( count * sizeof( float ));
    r = (float *) icvAlloc( count * sizeof( float ));

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

    ret = icvFitLine2D_wods( points, count, 0, _line );
    for( i = 0; i < 100; i++ )
    {
        if( ret != CV_NO_ERR )
        {
            icvFree( &w );
            icvFree( &r );
            return ret;
        }

        if( first )
        {
            first = 0;
        }
        else
        {
            if( fabs( acos( (float) (_line[0] * _lineprev[0] + _line[1] * _lineprev[1]) ))
                < 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 )
                {
                    // Return...
                    memcpy( line, _line, 4 * sizeof( float ));

                    icvFree( &w );
                    icvFree( &r );
                    return CV_NO_ERR;
                }

            }
        }
        /* calculate distances */
        icvCalcDist2D( points, count, _line, r );

        /* calculate weights */
        if( calc_weights )
        {
            calc_weights( r, count, w );
        }
        else if( calc_weights_param )
        {
            calc_weights_param( r, count, w, _param );
        }
        else
        {
            assert( 0 );
            return CV_BADFACTOR_ERR;
        }


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

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

    icvFree( &w );
    icvFree( &r );
    return CV_BADCONVERGENCE_ERR;
}


/* 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, CvDisType 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;
    float _line[6], _lineprev[6];
    int first = 1;
    float rdelta = reps != 0 ? reps : 1.0f;
    float adelta = aeps != 0 ? aeps : 0.01f;
    CvStatus ret;

    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 *) icvAlloc( count * sizeof( float ));
    r = (float *) icvAlloc( count * sizeof( float ));

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

    ret = icvFitLine3D_wods( points, count, 0, _line );
    for( i = 0; i < 100; i++ )
    {
        if( ret != CV_NO_ERR )
        {
            icvFree( &w );
            icvFree( &r );
            return ret;
        }

        if( first )
        {
            first = 0;
        }
        else
        {
            if( fabs
                ( acos
                  ( (float)
                    (_line[0] * _lineprev[0] + _line[1] * _lineprev[1] +
                     _line[2] * _lineprev[2]) )) < 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 )
                {
                    // Return...
                    memcpy( line, _line, 6 * sizeof( float ));

                    icvFree( &w );
                    icvFree( &r );
                    return CV_NO_ERR;
                }

            }
        }
        /* calculate distances */
        icvCalcDist3D( points, count, _line, r );

        /* calculate weights */
        if( calc_weights )
        {
            calc_weights( r, count, w );
        }
        else if( calc_weights_param )
        {
            calc_weights_param( r, count, w, _param );
        }
        else
        {
            assert( 0 );
            return CV_BADFACTOR_ERR;
        }

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

        /* Run again... */
        ret = icvFitLine3D_wods( points, count, w, _line );
    }

    icvFree( &w );
    icvFree( &r );
    return CV_BADCONVERGENCE_ERR;
}


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

    __BEGIN__;

    char* points = 0;
    CvContour contour_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) ||
            icvPixSize[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), icvPixSize[type], mat->data.ptr,
            mat->width + mat->height - 1, (CvSeq*)&contour_header, &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*icvPixSize[type] ));
        
        if( CV_MAT_DEPTH(type) == CV_32F )
        {
            CV_CALL( cvCvtSeqToArray( ptseq, points, CV_WHOLE_SEQ ));
        }
        else
        {
            /* convert sequence to 32f */
            float* fldata = (float*)points;
            CvSeqBlock* block = ptseq->first;

            do
            {
                int* idata = (int*)block->data;
                int i, count = block->count*CV_MAT_CN(type);
                for( i = 0; i < count; i++ )
                    fldata[i] = (float)idata[i];
                block = block->next;
                fldata += count;
            }
            while( block != ptseq->first );
        }
    }

    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( (void**)&buffer );
}

/* End of file. */