cvscanlines.cpp.svn-base

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

    }                           /* if */

    if( r_start_end[0] == 2 )
    {

        l_point[0] = l_start_end[0] * width;
        l_point[1] = l_start_end[1] * height;
        l_point[2] = 1;

        icvMultMatrixTVector3( F, l_point, r_epiline );

        l_angle[0] = (float) atan2( l_start_end[1] * height - l_epipole[1],
                                    l_start_end[0] * width - l_epipole[0] );
        l_angle[1] = (float) atan2( l_start_end[3] * height - l_epipole[1],
                                    l_start_end[2] * width - l_epipole[0] );

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

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

        return error;

    }                           /* if */

    l_start_end[0] *= width;
    l_start_end[1] *= height;
    l_start_end[2] *= width;
    l_start_end[3] *= height;

    r_start_end[0] *= width;
    r_start_end[1] *= height;
    r_start_end[2] *= width;
    r_start_end[3] *= height;

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

    icvMultMatrixVector3( F, r_point, l_epiline );
    error = icvBuildScanline( imgSize, l_epiline, 0, &x, 0, &y );

    if( error == CV_NO_ERR )
    {

        l_angle[0] = (float) atan2( y - l_epipole[1], x - l_epipole[0] );

        r_angle[0] = (float) atan2( r_point[1] - r_epipole[1], r_point[0] - r_epipole[0] );

    }
    else
    {

        if( turn == 1 )
        {

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

        }
        else
        {

            l_point[0] = l_start_end[2];
            l_point[1] = l_start_end[3];
        }                       /* if */

        l_point[2] = 1;

        icvMultMatrixTVector3( F, l_point, r_epiline );
        error = icvBuildScanline( imgSize, r_epiline, 0, &x, 0, &y );

        if( error == CV_NO_ERR )
        {

            r_angle[0] = (float) atan2( y - r_epipole[1], x - r_epipole[0] );

            l_angle[0] = (float) atan2( l_point[1] - l_epipole[1], l_point[0] - l_epipole[0] );

        }
        else
            return CV_BADFACTOR_ERR;
    }                           /* if */

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

    icvMultMatrixVector3( F, r_point, l_epiline );
    error = icvBuildScanline( imgSize, l_epiline, 0, &x, 0, &y );

    if( error == CV_NO_ERR )
    {

        l_angle[1] = (float) atan2( y - l_epipole[1], x - l_epipole[0] );

        r_angle[1] = (float) atan2( r_point[1] - r_epipole[1], r_point[0] - r_epipole[0] );

    }
    else
    {

        if( turn == 1 )
        {

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

        }
        else
        {

            l_point[0] = l_start_end[0];
            l_point[1] = l_start_end[1];
        }                       /* if */

        l_point[2] = 1;

        icvMultMatrixTVector3( F, l_point, r_epiline );
        error = icvBuildScanline( imgSize, r_epiline, 0, &x, 0, &y );

        if( error == CV_NO_ERR )
        {

            r_angle[1] = (float) atan2( y - r_epipole[1], x - r_epipole[0] );

            l_angle[1] = (float) atan2( l_point[1] - l_epipole[1], l_point[0] - l_epipole[0] );

        }
        else
            return CV_BADFACTOR_ERR;
    }                           /* if */

    if( l_angle[0] > l_angle[1] )
    {

        swap = l_angle[0];
        l_angle[0] = l_angle[1];
        l_angle[1] = swap;
    }                           /* if */

    if( l_angle[1] - l_angle[0] > CV_PI )
    {

        swap = l_angle[0];
        l_angle[0] = l_angle[1];
        l_angle[1] = swap + (float) (CV_PI * 2);
    }                           /* if */

    if( r_angle[0] > r_angle[1] )
    {

        swap = r_angle[0];
        r_angle[0] = r_angle[1];
        r_angle[1] = swap;
    }                           /* if */

    if( r_angle[1] - r_angle[0] > CV_PI )
    {

        swap = r_angle[0];
        r_angle[0] = r_angle[1];
        r_angle[1] = swap + (float) (CV_PI * 2);
    }                           /* if */

    if( l_radius * (l_angle[1] - l_angle[0]) > r_radius * (r_angle[1] - r_angle[0]) )
        error = icvBuildScanlineLeftStereo( imgSize,
                                            matrix,
                                            l_epipole,
                                            l_angle,
                                            l_radius, scanlines_1, scanlines_2, numlines );

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


    return error;

}                               /* icvGetCoefficientStereo */

/*===========================================================================*/
CvStatus
icvBuildScanlineLeftStereo( CvSize imgSize,
                            CvMatrix3 * matrix,
                            float *l_epipole,
                            float *l_angle,
                            float l_radius, int *scanlines_1, int *scanlines_2, int *numlines )
{
    int image_width;
    int image_height;
    //int prewarp_width;
    int prewarp_height;
    float i;
    int offset;
    float height;
    float delta;
    float angle;
    float l_point[3];
    float l_epiline[3];
    float r_epiline[3];
    CvStatus error = CV_OK;
    CvMatrix3 *F;


    assert( l_angle != 0 && !REAL_ZERO( l_radius ));

    image_width = imgSize.width;

    image_height = imgSize.height;

    /*prewarp_width = (int) (sqrt( image_width * image_width +
                                 image_height * image_height ) + 1);*/

    prewarp_height = (int) (l_radius * (l_angle[1] - l_angle[0]));

    *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 = (l_angle[1] - l_angle[0]) / height;

    l_angle[0] += delta;
    l_angle[1] -= delta;

    delta = (l_angle[1] - l_angle[0]) / height;

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

        angle = l_angle[0] + i * delta;

        l_point[0] = l_epipole[0] + l_radius * (float) cos( angle );
        l_point[1] = l_epipole[1] + l_radius * (float) sin( angle );

        icvMultMatrixTVector3( F, l_point, r_epiline );

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


        l_epiline[0] = l_point[1] - l_epipole[1];
        l_epiline[1] = l_epipole[0] - l_point[0];
        l_epiline[2] = l_point[0] * l_epipole[1] - l_point[1] * l_epipole[0];

        if( Sgn( l_epiline[0] * r_epiline[0] + l_epiline[1] * r_epiline[1] ) < 0 )
        {

            l_epiline[0] = -l_epiline[0];
            l_epiline[1] = -l_epiline[1];
            l_epiline[2] = -l_epiline[2];
        }                       /* if */

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

    }                           /* for */

    *numlines = prewarp_height;

    return error;

}                               /* icvlBuildScanlineLeftStereo */

/*===========================================================================*/
CvStatus
icvBuildScanlineRightStereo( CvSize imgSize,
                             CvMatrix3 * matrix,
                             float *r_epipole,
                             float *r_angle,
                             float r_radius,
                             int *scanlines_1, int *scanlines_2, int *numlines )
{
    int image_width;
    int image_height;
    //int prewarp_width;
    int prewarp_height;
    float i;
    int offset;
    float height;
    float delta;
    float angle;
    float r_point[3];
    float l_epiline[3];
    float r_epiline[3];
    CvStatus error = CV_OK;
    CvMatrix3 *F;

    assert( r_angle != 0 && !REAL_ZERO( r_radius ));

    image_width = imgSize.width;

    image_height = imgSize.height;

    /*prewarp_width = (int) (sqrt( image_width * image_width +
                                 image_height * image_height ) + 1);*/

    prewarp_height = (int) (r_radius * (r_angle[1] - r_angle[0]));

    *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 = (r_angle[1] - r_angle[0]) / height;

    r_angle[0] += delta;
    r_angle[1] -= delta;

    delta = (r_angle[1] - r_angle[0]) / height;

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

        angle = r_angle[0] + i * delta;

        r_point[0] = r_epipole[0] + r_radius * (float) cos( angle );
        r_point[1] = r_epipole[1] + r_radius * (float) sin( angle );

        icvMultMatrixVector3( F, r_point, l_epiline );

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

        assert( error == CV_NO_ERR );

        r_epiline[0] = r_point[1] - r_epipole[1];
        r_epiline[1] = r_epipole[0] - r_point[0];
        r_epiline[2] = r_point[0] * r_epipole[1] - r_point[1] * r_epipole[0];

        if( Sgn( l_epiline[0] * r_epiline[0] + l_epiline[1] * r_epiline[1] ) < 0 )
        {

            r_epiline[0] = -r_epiline[0];
            r_epiline[1] = -r_epiline[1];
            r_epiline[2] = -r_epiline[2];
        }                       /* if */

        error = icvGetCrossEpilineFrame( imgSize, r_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;

}                               /* icvlBuildScanlineRightStereo */

/*===========================================================================*/
CvStatus
icvGetCrossEpilineFrame( CvSize imgSize, float *epiline, int *x1, int *y1, int *x2, int *y2 )
{
    int tx, ty;
    float point[2][2];
    int sign[4], i;
    float width, height;
    double tmpvalue;

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

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

    tmpvalue = epiline[2];
    sign[0] = SIGN( tmpvalue );

    tmpvalue = epiline[0] * width + epiline[2];
    sign[1] = SIGN( tmpvalue );

    tmpvalue = epiline[1] * height + epiline[2];
    sign[2] = SIGN( tmpvalue );

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

    i = 0;
    for( tx = 0; tx < 2; tx++ )
    {
        for( ty = 0; ty < 2; ty++ )
        {

            if( sign[ty * 2 + tx] == 0 )
            {

                point[i][0] = width * tx;
                point[i][1] = height * ty;
                i++;

            }                   /* if */
        }                       /* for */
    }                           /* for */

    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( (point[0][0] - point[1][0]) * epiline[1] +
        (point[1][1] - point[0][1]) * epiline[0] > 0 )
    {
        *x1 = (int) point[0][0];
        *y1 = (int) point[0][1];
        *x2 = (int) point[1][0];
        *y2 = (int) point[1][1];
    }
    else
    {
        *x1 = (int) point[1][0];
        *y1 = (int) point[1][1];
        *x2 = (int) point[0][0];
        *y2 = (int) point[0][1];
    }                           /* if */

    return CV_NO_ERR;
}                               /* icvlGetCrossEpilineFrame */

/*=====================================================================================*/

CV_IMPL void
cvMakeScanlines( const CvMatrix3* matrix, CvSize imgSize,
                 int *scanlines_1, int *scanlines_2,
                 int *lens_1, int *lens_2, int *numlines )
{
    CV_FUNCNAME( "cvMakeScanlines" );

    __BEGIN__;

    IPPI_CALL( icvMakeScanlines( (CvMatrix3*)matrix, imgSize, scanlines_1,
                                 scanlines_2, lens_1, lens_2, numlines ));
    __END__;
}

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: cvDeleteMoire
//    Purpose: The functions 
//    Context:
//    Parameters:  
//
//    Notes:  
//F*/
CV_IMPL void
cvMakeAlphaScanlines( int *scanlines_1,
                      int *scanlines_2,
                      int *scanlines_a, int *lens, int numlines, float alpha )
{
    CV_FUNCNAME( "cvMakeAlphaScanlines" );

    __BEGIN__;

    IPPI_CALL( icvMakeAlphaScanlines( scanlines_1, scanlines_2, scanlines_a,
                                      lens, numlines, alpha ));

    __END__;
}