cvscanlines.cpp.svn-base

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

            l_point[0] = (float) width;
            l_point[1] = (float) height;
            l_point[2] = 1;

            icvMultMatrixTVector3( F, l_point, epiline );
            error = icvCrossLines( r_diagonal, epiline, r_point );
            assert( error == CV_NO_ERR );

            if( r_point[0] >= 0 && r_point[0] <= width )
            {

                l_start_end[0] = l_point[0];
                l_start_end[1] = l_point[1];

                r_start_end[0] = r_point[0];
                r_start_end[1] = r_point[1];
            }
            else
                return CV_BADFACTOR_ERR;
        }                       /* if */
    }                           /* if */

    r_point[0] = (float) width;
    r_point[1] = (float) height;
    r_point[2] = 1;

    icvMultMatrixVector3( F, r_point, epiline );
    error = icvCrossLines( l_diagonal, epiline, l_point );
    assert( error == CV_NO_ERR );

    if( l_point[0] >= 0 && l_point[0] <= width )
    {

        l_start_end[2] = l_point[0];
        l_start_end[3] = l_point[1];

        r_start_end[2] = r_point[0];
        r_start_end[3] = r_point[1];

    }
    else
    {

        if( l_point[0] < 0 )
        {

            l_point[0] = 0;
            l_point[1] = 0;
            l_point[2] = 1;

            icvMultMatrixTVector3( F, l_point, epiline );
            error = icvCrossLines( r_diagonal, epiline, r_point );
            assert( error == CV_NO_ERR );

            if( r_point[0] >= 0 && r_point[0] <= width )
            {

                l_start_end[2] = l_point[0];
                l_start_end[3] = l_point[1];

                r_start_end[2] = r_point[0];
                r_start_end[3] = r_point[1];
            }
            else
                return CV_BADFACTOR_ERR;

        }
        else
        {                       /* if( l_point[0] > width ) */

            l_point[0] = (float) width;
            l_point[1] = (float) height;
            l_point[2] = 1;

            icvMultMatrixTVector3( F, l_point, epiline );
            error = icvCrossLines( r_diagonal, epiline, r_point );
            assert( error == CV_NO_ERR );

            if( r_point[0] >= 0 && r_point[0] <= width )
            {

                l_start_end[2] = l_point[0];
                l_start_end[3] = l_point[1];

                r_start_end[2] = r_point[0];
                r_start_end[3] = r_point[1];
            }
            else
                return CV_BADFACTOR_ERR;
        }                       /* if */
    }                           /* if */

    return CV_NO_ERR;

}                               /* icvlGetStartEnd4 */

/*===========================================================================*/
CvStatus
icvBuildScanlineLeft( CvMatrix3 * matrix,
                      CvSize imgSize,
                      int *scanlines_1, int *scanlines_2, float *l_start_end, int *numlines )
{
    int prewarp_height;
    float l_point[3];
    float r_point[3];
    float height;
    float delta_x;
    float delta_y;
    CvStatus error = CV_OK;
    CvMatrix3 *F;
    float i;
    int offset;
    float epiline[3];

    assert( l_start_end != 0 );

    prewarp_height = (int) (MAX( fabs( l_start_end[2] - l_start_end[0] ),
                                 fabs( l_start_end[3] - l_start_end[1] )));

    *numlines = prewarp_height;

    if( scanlines_1 == 0 && scanlines_2 == 0 )
        return CV_NO_ERR;

    F = matrix;


    l_point[2] = 1;
    height = (float) prewarp_height;

    delta_x = (l_start_end[2] - l_start_end[0]) / height;

    l_start_end[0] += delta_x;
    l_start_end[2] -= delta_x;

    delta_x = (l_start_end[2] - l_start_end[0]) / height;
    delta_y = (l_start_end[3] - l_start_end[1]) / height;

    l_start_end[1] += delta_y;
    l_start_end[3] -= delta_y;

    delta_y = (l_start_end[3] - l_start_end[1]) / height;

    for( i = 0, offset = 0; i < height; i++, offset += 4 )
    {

        l_point[0] = l_start_end[0] + i * delta_x;
        l_point[1] = l_start_end[1] + i * delta_y;

        icvMultMatrixTVector3( F, l_point, epiline );

        error = icvGetCrossEpilineFrame( imgSize, epiline,
                                         scanlines_2 + offset,
                                         scanlines_2 + offset + 1,
                                         scanlines_2 + offset + 2, scanlines_2 + offset + 3 );



        assert( error == CV_NO_ERR );

        r_point[0] = -(float) (*(scanlines_2 + offset));
        r_point[1] = -(float) (*(scanlines_2 + offset + 1));
        r_point[2] = -1;

        icvMultMatrixVector3( F, r_point, epiline );

        error = icvGetCrossEpilineFrame( imgSize, epiline,
                                         scanlines_1 + offset,
                                         scanlines_1 + offset + 1,
                                         scanlines_1 + offset + 2, scanlines_1 + offset + 3 );

        assert( error == CV_NO_ERR );
    }                           /* for */

    *numlines = prewarp_height;

    return error;

} /*icvlBuildScanlineLeft */

/*===========================================================================*/
CvStatus
icvBuildScanlineRight( CvMatrix3 * matrix,
                       CvSize imgSize,
                       int *scanlines_1, int *scanlines_2, float *r_start_end, int *numlines )
{
    int prewarp_height;
    float l_point[3];
    float r_point[3];
    float height;
    float delta_x;
    float delta_y;
    CvStatus error = CV_OK;
    CvMatrix3 *F;
    float i;
    int offset;
    float epiline[3];

    assert( r_start_end != 0 );

    prewarp_height = (int) (MAX( fabs( r_start_end[2] - r_start_end[0] ),
                                 fabs( r_start_end[3] - r_start_end[1] )));

    *numlines = prewarp_height;

    if( scanlines_1 == 0 && scanlines_2 == 0 )
        return CV_NO_ERR;

    F = matrix;

    r_point[2] = 1;
    height = (float) prewarp_height;

    delta_x = (r_start_end[2] - r_start_end[0]) / height;

    r_start_end[0] += delta_x;
    r_start_end[2] -= delta_x;

    delta_x = (r_start_end[2] - r_start_end[0]) / height;
    delta_y = (r_start_end[3] - r_start_end[1]) / height;

    r_start_end[1] += delta_y;
    r_start_end[3] -= delta_y;

    delta_y = (r_start_end[3] - r_start_end[1]) / height;

    for( i = 0, offset = 0; i < height; i++, offset += 4 )
    {

        r_point[0] = r_start_end[0] + i * delta_x;
        r_point[1] = r_start_end[1] + i * delta_y;

        icvMultMatrixVector3( F, r_point, epiline );

        error = icvGetCrossEpilineFrame( imgSize, epiline,
                                         scanlines_1 + offset,
                                         scanlines_1 + offset + 1,
                                         scanlines_1 + offset + 2, scanlines_1 + offset + 3 );


        assert( error == CV_NO_ERR );

        l_point[0] = -(float) (*(scanlines_1 + offset));
        l_point[1] = -(float) (*(scanlines_1 + offset + 1));

        l_point[2] = -1;

        icvMultMatrixTVector3( F, l_point, epiline );
        error = icvGetCrossEpilineFrame( imgSize, epiline,
                                         scanlines_2 + offset,
                                         scanlines_2 + offset + 1,
                                         scanlines_2 + offset + 2, scanlines_2 + offset + 3 );


        assert( error == CV_NO_ERR );
    }                           /* for */

    *numlines = prewarp_height;

    return error;

} /*icvlBuildScanlineRight */

/*===========================================================================*/
#define Abs(x)              ( (x)<0 ? -(x):(x) )
#define Sgn(x)              ( (x)<0 ? -1:1 )    /* Sgn(0) = 1 ! */

CvStatus
icvBuildScanline( CvSize imgSize, float *epiline, float *kx, float *cx, float *ky, float *cy )
{
    float point[4][2], d;
    int sign[4], i;

    float width, height;

    if( REAL_ZERO( epiline[0] ) && REAL_ZERO( epiline[1] ))
        return CV_BADFACTOR_ERR;

    width = (float) imgSize.width - 1;
    height = (float) imgSize.height - 1;

    sign[0] = Sgn( epiline[2] );
    sign[1] = Sgn( epiline[0] * width + epiline[2] );
    sign[2] = Sgn( epiline[1] * height + epiline[2] );
    sign[3] = Sgn( epiline[0] * width + epiline[1] * height + epiline[2] );

    i = 0;

    if( sign[0] * sign[1] < 0 )
    {

        point[i][0] = -epiline[2] / epiline[0];
        point[i][1] = 0;
        i++;
    }                           /* if */

    if( sign[0] * sign[2] < 0 )
    {

        point[i][0] = 0;
        point[i][1] = -epiline[2] / epiline[1];
        i++;
    }                           /* if */

    if( sign[1] * sign[3] < 0 )
    {

        point[i][0] = width;
        point[i][1] = -(epiline[0] * width + epiline[2]) / epiline[1];
        i++;
    }                           /* if */

    if( sign[2] * sign[3] < 0 )
    {

        point[i][0] = -(epiline[1] * height + epiline[2]) / epiline[0];
        point[i][1] = height;
    }                           /* if */

    if( sign[0] == sign[1] && sign[0] == sign[2] && sign[0] == sign[3] )
        return CV_BADFACTOR_ERR;

    if( !kx && !ky && !cx && !cy )
        return CV_BADFACTOR_ERR;

    if( kx && ky )
    {

        *kx = -epiline[1];
        *ky = epiline[0];

        d = (float) MAX( Abs( *kx ), Abs( *ky ));

        *kx /= d;
        *ky /= d;
    }                           /* if */

    if( cx && cy )
    {

        if( (point[0][0] - point[1][0]) * epiline[1] +
            (point[1][1] - point[0][1]) * epiline[0] > 0 )
        {

            *cx = point[0][0];
            *cy = point[0][1];

        }
        else
        {

            *cx = point[1][0];
            *cy = point[1][1];
        }                       /* if */
    }                           /* if */

    return CV_NO_ERR;

}                               /* icvlBuildScanline */

/*===========================================================================*/
CvStatus
icvGetCoefficientStereo( CvMatrix3 * matrix,
                         CvSize imgSize,
                         float *l_epipole,
                         float *r_epipole, int *scanlines_1, int *scanlines_2, int *numlines )
{
    int i, j, turn;
    float width, height;
    float l_angle[2], r_angle[2];
    float l_radius, r_radius;
    float r_point[3], l_point[3];
    float l_epiline[3], r_epiline[3], x, y;
    float swap;

    float radius1, radius2, radius3, radius4;

    float l_start_end[4], r_start_end[4];
    CvMatrix3 *F;
    CvStatus error;
    float Region[3][3][4] = {
       {{0.f, 0.f, 1.f, 1.f}, {0.f, 1.f, 1.f, 1.f}, {0.f, 1.f, 1.f, 0.f}},
        {{0.f, 0.f, 0.f, 1.f}, {2.f, 2.f, 2.f, 2.f}, {1.f, 1.f, 1.f, 0.f}},
        {{1.f, 0.f, 0.f, 1.f}, {1.f, 0.f, 0.f, 0.f}, {1.f, 1.f, 0.f, 0.f}}
    };


    width = (float) imgSize.width - 1;
    height = (float) imgSize.height - 1;

    F = matrix;

    if( F->m[0][0] * F->m[1][1] - F->m[1][0] * F->m[0][1] > 0 )
        turn = 1;
    else
        turn = -1;

    if( l_epipole[0] < 0 )
        i = 0;
    else if( l_epipole[0] < width )
        i = 1;
    else
        i = 2;

    if( l_epipole[1] < 0 )
        j = 2;
    else if( l_epipole[1] < height )
        j = 1;
    else
        j = 0;

    l_start_end[0] = Region[j][i][0];
    l_start_end[1] = Region[j][i][1];
    l_start_end[2] = Region[j][i][2];
    l_start_end[3] = Region[j][i][3];

    if( r_epipole[0] < 0 )
        i = 0;
    else if( r_epipole[0] < width )
        i = 1;
    else
        i = 2;

    if( r_epipole[1] < 0 )
        j = 2;
    else if( r_epipole[1] < height )
        j = 1;
    else
        j = 0;

    r_start_end[0] = Region[j][i][0];
    r_start_end[1] = Region[j][i][1];
    r_start_end[2] = Region[j][i][2];
    r_start_end[3] = Region[j][i][3];

    radius1 = l_epipole[0] * l_epipole[0] + (l_epipole[1] - height) * (l_epipole[1] - height);

    radius2 = (l_epipole[0] - width) * (l_epipole[0] - width) +
        (l_epipole[1] - height) * (l_epipole[1] - height);

    radius3 = l_epipole[0] * l_epipole[0] + l_epipole[1] * l_epipole[1];

    radius4 = (l_epipole[0] - width) * (l_epipole[0] - width) + l_epipole[1] * l_epipole[1];


    l_radius = (float) sqrt( (double)MAX( MAX( radius1, radius2 ), MAX( radius3, radius4 )));

    radius1 = r_epipole[0] * r_epipole[0] + (r_epipole[1] - height) * (r_epipole[1] - height);

    radius2 = (r_epipole[0] - width) * (r_epipole[0] - width) +
        (r_epipole[1] - height) * (r_epipole[1] - height);

    radius3 = r_epipole[0] * r_epipole[0] + r_epipole[1] * r_epipole[1];

    radius4 = (r_epipole[0] - width) * (r_epipole[0] - width) + r_epipole[1] * r_epipole[1];


    r_radius = (float) sqrt( (double)MAX( MAX( radius1, radius2 ), MAX( radius3, radius4 )));

    if( l_start_end[0] == 2 && r_start_end[0] == 2 )

        if( l_radius > r_radius )
        {

            l_angle[0] = 0.0f;
            l_angle[1] = (float) CV_PI;

            error = icvBuildScanlineLeftStereo( imgSize,
                                                matrix,
                                                l_epipole,
                                                l_angle,
                                                l_radius, scanlines_1, scanlines_2, numlines );

            return error;

        }
        else
        {

            r_angle[0] = 0.0f;
            r_angle[1] = (float) CV_PI;

            error = icvBuildScanlineRightStereo( imgSize,
                                                 matrix,
                                                 r_epipole,
                                                 r_angle,
                                                 r_radius,
                                                 scanlines_1, scanlines_2, numlines );

            return error;
        }                       /* if */

    if( l_start_end[0] == 2 )
    {

        r_angle[0] = (float) atan2( r_start_end[1] * height - r_epipole[1],
                                    r_start_end[0] * width - r_epipole[0] );
        r_angle[1] = (float) atan2( r_start_end[3] * height - r_epipole[1],
                                    r_start_end[2] * width - r_epipole[0] );

        if( r_angle[0] > r_angle[1] )
            r_angle[1] += (float) (CV_PI * 2);

        error = icvBuildScanlineRightStereo( imgSize,
                                             matrix,
                                             r_epipole,
                                             r_angle,
                                             r_radius, scanlines_1, scanlines_2, numlines );

        return error;