| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| #include "_cvaux.h" |
| #include "_cvvm.h" |
| |
| //#define REAL_ZERO(x) ( (x) < 1e-8 && (x) > -1e-8) |
| |
| static CvStatus |
| icvGetNormalVector3( CvMatrix3 * Matrix, float *v ) |
| { |
| /* return vector v that is any 3-vector perpendicular |
| to all the row vectors of Matrix */ |
| |
| double *solutions = 0; |
| double M[3 * 3]; |
| double B[3] = { 0.f, 0.f, 0.f }; |
| int i, j, res; |
| |
| if( Matrix == 0 || v == 0 ) |
| return CV_NULLPTR_ERR; |
| |
| for( i = 0; i < 3; i++ ) |
| { |
| for( j = 0; j < 3; j++ ) |
| M[i * 3 + j] = (double) (Matrix->m[i][j]); |
| } /* for */ |
| |
| res = icvGaussMxN( M, B, 3, 3, &solutions ); |
| |
| if( res == -1 ) |
| return CV_BADFACTOR_ERR; |
| |
| if( res > 0 && solutions ) |
| { |
| v[0] = (float) solutions[0]; |
| v[1] = (float) solutions[1]; |
| v[2] = (float) solutions[2]; |
| res = 0; |
| } |
| else |
| res = 1; |
| |
| if( solutions ) |
| cvFree( &solutions ); |
| |
| if( res ) |
| return CV_BADFACTOR_ERR; |
| else |
| return CV_NO_ERR; |
| |
| } /* icvgetNormalVector3 */ |
| |
| |
| /*=====================================================================================*/ |
| |
| static CvStatus |
| icvMultMatrixVector3( CvMatrix3 * m, float *src, float *dst ) |
| { |
| if( m == 0 || src == 0 || dst == 0 ) |
| return CV_NULLPTR_ERR; |
| |
| dst[0] = m->m[0][0] * src[0] + m->m[0][1] * src[1] + m->m[0][2] * src[2]; |
| dst[1] = m->m[1][0] * src[0] + m->m[1][1] * src[1] + m->m[1][2] * src[2]; |
| dst[2] = m->m[2][0] * src[0] + m->m[2][1] * src[1] + m->m[2][2] * src[2]; |
| |
| return CV_NO_ERR; |
| |
| } /* icvMultMatrixVector3 */ |
| |
| |
| /*=====================================================================================*/ |
| |
| static CvStatus |
| icvMultMatrixTVector3( CvMatrix3 * m, float *src, float *dst ) |
| { |
| if( m == 0 || src == 0 || dst == 0 ) |
| return CV_NULLPTR_ERR; |
| |
| dst[0] = m->m[0][0] * src[0] + m->m[1][0] * src[1] + m->m[2][0] * src[2]; |
| dst[1] = m->m[0][1] * src[0] + m->m[1][1] * src[1] + m->m[2][1] * src[2]; |
| dst[2] = m->m[0][2] * src[0] + m->m[1][2] * src[1] + m->m[2][2] * src[2]; |
| |
| return CV_NO_ERR; |
| |
| } /* icvMultMatrixTVector3 */ |
| |
| /*=====================================================================================*/ |
| |
| static CvStatus |
| icvCrossLines( float *line1, float *line2, float *cross_point ) |
| { |
| float delta; |
| |
| if( line1 == 0 && line2 == 0 && cross_point == 0 ) |
| return CV_NULLPTR_ERR; |
| |
| delta = line1[0] * line2[1] - line1[1] * line2[0]; |
| |
| if( REAL_ZERO( delta )) |
| return CV_BADFACTOR_ERR; |
| |
| cross_point[0] = (-line1[2] * line2[1] + line1[1] * line2[2]) / delta; |
| cross_point[1] = (-line1[0] * line2[2] + line1[2] * line2[0]) / delta; |
| cross_point[2] = 1; |
| |
| return CV_NO_ERR; |
| } /* icvCrossLines */ |
| |
| |
| |
| /*======================================================================================*/ |
| |
| static CvStatus |
| icvMakeScanlines( CvMatrix3 * matrix, |
| CvSize imgSize, |
| int *scanlines_1, int *scanlines_2, int *lens_1, int *lens_2, int *numlines ) |
| { |
| |
| CvStatus error; |
| |
| error = icvGetCoefficient( matrix, imgSize, scanlines_2, scanlines_1, numlines ); |
| |
| /* Make Length of scanlines */ |
| |
| if( scanlines_1 == 0 && scanlines_2 == 0 ) |
| return error; |
| |
| icvMakeScanlinesLengths( scanlines_1, *numlines, lens_1 ); |
| |
| icvMakeScanlinesLengths( scanlines_2, *numlines, lens_2 ); |
| |
| matrix = matrix; |
| return CV_NO_ERR; |
| |
| |
| } /* icvMakeScanlines */ |
| |
| |
| /*======================================================================================*/ |
| |
| CvStatus |
| icvMakeScanlinesLengths( int *scanlines, int numlines, int *lens ) |
| { |
| int index; |
| int x1, y1, x2, y2, dx, dy; |
| int curr; |
| |
| curr = 0; |
| |
| for( index = 0; index < numlines; index++ ) |
| { |
| |
| x1 = scanlines[curr++]; |
| y1 = scanlines[curr++]; |
| x2 = scanlines[curr++]; |
| y2 = scanlines[curr++]; |
| |
| dx = abs( x1 - x2 ) + 1; |
| dy = abs( y1 - y2 ) + 1; |
| |
| lens[index] = MAX( dx, dy ); |
| |
| } |
| return CV_NO_ERR; |
| } |
| |
| /*======================================================================================*/ |
| |
| static CvStatus |
| icvMakeAlphaScanlines( int *scanlines_1, |
| int *scanlines_2, |
| int *scanlines_a, int *lens, int numlines, float alpha ) |
| { |
| int index; |
| int x1, y1, x2, y2; |
| int curr; |
| int dx, dy; |
| int curr_len; |
| |
| curr = 0; |
| curr_len = 0; |
| for( index = 0; index < numlines; index++ ) |
| { |
| |
| x1 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha)); |
| |
| scanlines_a[curr++] = x1; |
| |
| y1 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha)); |
| |
| scanlines_a[curr++] = y1; |
| |
| x2 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha)); |
| |
| scanlines_a[curr++] = x2; |
| |
| y2 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha)); |
| |
| scanlines_a[curr++] = y2; |
| |
| dx = abs( x1 - x2 ) + 1; |
| dy = abs( y1 - y2 ) + 1; |
| |
| lens[curr_len++] = MAX( dx, dy ); |
| |
| } |
| |
| return CV_NO_ERR; |
| } |
| |
| /*======================================================================================*/ |
| |
| |
| |
| |
| |
| |
| |
| /* //////////////////////////////////////////////////////////////////////////////////// */ |
| |
| CvStatus |
| icvGetCoefficient( CvMatrix3 * matrix, |
| CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines ) |
| { |
| float l_epipole[3]; |
| float r_epipole[3]; |
| CvMatrix3 *F; |
| CvMatrix3 Ft; |
| CvStatus error; |
| int i, j; |
| |
| F = matrix; |
| |
| l_epipole[2] = -1; |
| r_epipole[2] = -1; |
| |
| if( F == 0 ) |
| { |
| error = icvGetCoefficientDefault( matrix, |
| imgSize, scanlines_1, scanlines_2, numlines ); |
| return error; |
| } |
| |
| |
| for( i = 0; i < 3; i++ ) |
| for( j = 0; j < 3; j++ ) |
| Ft.m[i][j] = F->m[j][i]; |
| |
| |
| error = icvGetNormalVector3( &Ft, l_epipole ); |
| if( error == CV_NO_ERR && !REAL_ZERO( l_epipole[2] ) && !REAL_ZERO( l_epipole[2] - 1 )) |
| { |
| |
| l_epipole[0] /= l_epipole[2]; |
| l_epipole[1] /= l_epipole[2]; |
| l_epipole[2] = 1; |
| } /* if */ |
| |
| error = icvGetNormalVector3( F, r_epipole ); |
| if( error == CV_NO_ERR && !REAL_ZERO( r_epipole[2] ) && !REAL_ZERO( r_epipole[2] - 1 )) |
| { |
| |
| r_epipole[0] /= r_epipole[2]; |
| r_epipole[1] /= r_epipole[2]; |
| r_epipole[2] = 1; |
| } /* if */ |
| |
| if( REAL_ZERO( l_epipole[2] - 1 ) && REAL_ZERO( r_epipole[2] - 1 )) |
| { |
| error = icvGetCoefficientStereo( matrix, |
| imgSize, |
| l_epipole, |
| r_epipole, scanlines_1, scanlines_2, numlines ); |
| if( error == CV_NO_ERR ) |
| return CV_NO_ERR; |
| } |
| else |
| { |
| if( REAL_ZERO( l_epipole[2] ) && REAL_ZERO( r_epipole[2] )) |
| { |
| error = icvGetCoefficientOrto( matrix, |
| imgSize, scanlines_1, scanlines_2, numlines ); |
| if( error == CV_NO_ERR ) |
| return CV_NO_ERR; |
| } |
| } |
| |
| |
| error = icvGetCoefficientDefault( matrix, imgSize, scanlines_1, scanlines_2, numlines ); |
| |
| return error; |
| |
| } /* icvlGetCoefficient */ |
| |
| /*===========================================================================*/ |
| CvStatus |
| icvGetCoefficientDefault( CvMatrix3 * matrix, |
| CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines ) |
| { |
| int curr; |
| int y; |
| |
| *numlines = imgSize.height; |
| |
| if( scanlines_1 == 0 && scanlines_2 == 0 ) |
| return CV_NO_ERR; |
| |
| curr = 0; |
| for( y = 0; y < imgSize.height; y++ ) |
| { |
| scanlines_1[curr] = 0; |
| scanlines_1[curr + 1] = y; |
| scanlines_1[curr + 2] = imgSize.width - 1; |
| scanlines_1[curr + 3] = y; |
| |
| scanlines_2[curr] = 0; |
| scanlines_2[curr + 1] = y; |
| scanlines_2[curr + 2] = imgSize.width - 1; |
| scanlines_2[curr + 3] = y; |
| |
| curr += 4; |
| } |
| |
| matrix = matrix; |
| return CV_NO_ERR; |
| |
| } /* icvlGetCoefficientDefault */ |
| |
| /*===========================================================================*/ |
| CvStatus |
| icvGetCoefficientOrto( CvMatrix3 * matrix, |
| CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines ) |
| { |
| float l_start_end[4], r_start_end[4]; |
| double a, b; |
| CvStatus error; |
| CvMatrix3 *F; |
| |
| F = matrix; |
| |
| if( F->m[0][2] * F->m[1][2] < 0 ) |
| { /* on left / */ |
| |
| if( F->m[2][0] * F->m[2][1] < 0 ) |
| { /* on right / */ |
| error = icvGetStartEnd1( F, imgSize, l_start_end, r_start_end ); |
| |
| |
| } |
| else |
| { /* on right \ */ |
| error = icvGetStartEnd2( F, imgSize, l_start_end, r_start_end ); |
| } /* if */ |
| |
| } |
| else |
| { /* on left \ */ |
| |
| if( F->m[2][0] * F->m[2][1] < 0 ) |
| { /* on right / */ |
| error = icvGetStartEnd3( F, imgSize, l_start_end, r_start_end ); |
| } |
| else |
| { /* on right \ */ |
| error = icvGetStartEnd4( F, imgSize, l_start_end, r_start_end ); |
| } /* if */ |
| } /* if */ |
| |
| if( error != CV_NO_ERR ) |
| return error; |
| |
| a = fabs( l_start_end[0] - l_start_end[2] ); |
| b = fabs( r_start_end[0] - r_start_end[2] ); |
| if( a > b ) |
| { |
| |
| error = icvBuildScanlineLeft( F, |
| imgSize, |
| scanlines_1, scanlines_2, l_start_end, numlines ); |
| |
| } |
| else |
| { |
| |
| error = icvBuildScanlineRight( F, |
| imgSize, |
| scanlines_1, scanlines_2, r_start_end, numlines ); |
| |
| } /* if */ |
| |
| return error; |
| |
| } /* icvlGetCoefficientOrto */ |
| |
| /*===========================================================================*/ |
| CvStatus |
| icvGetStartEnd1( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end ) |
| { |
| |
| CvMatrix3 *F; |
| int width, height; |
| float l_diagonal[3]; |
| float r_diagonal[3]; |
| float l_point[3], r_point[3], epiline[3]; |
| CvStatus error = CV_OK; |
| |
| F = matrix; |
| width = imgSize.width - 1; |
| height = imgSize.height - 1; |
| |
| l_diagonal[0] = (float) 1 / width; |
| l_diagonal[1] = (float) 1 / height; |
| l_diagonal[2] = -1; |
| |
| r_diagonal[0] = (float) 1 / width; |
| r_diagonal[1] = (float) 1 / height; |
| r_diagonal[2] = -1; |
| |
| r_point[0] = (float) width; |
| r_point[1] = 0; |
| 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[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 |
| { |
| |
| if( l_point[0] < 0 ) |
| { |
| |
| l_point[0] = 0; |
| 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; |
| |
| } |
| else |
| { /* if( l_point[0] > width ) */ |
| |
| l_point[0] = (float) width; |
| 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[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] = 0; |
| 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] = (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; |
| |
| } |
| else |
| { /* if( l_point[0] > width ) */ |
| |
| l_point[0] = (float) width; |
| 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; |
| } /* if */ |
| } /* if */ |
| |
| return error; |
| |
| } /* icvlGetStartEnd1 */ |
| |
| /*===========================================================================*/ |
| CvStatus |
| icvGetStartEnd2( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end ) |
| { |
| |
| |
| CvMatrix3 *F; |
| int width, height; |
| float l_diagonal[3]; |
| float r_diagonal[3]; |
| float l_point[3], r_point[3], epiline[3]; |
| CvStatus error = CV_OK; |
| |
| F = matrix; |
| |
| width = imgSize.width - 1; |
| height = imgSize.height - 1; |
| |
| l_diagonal[0] = (float) 1 / width; |
| l_diagonal[1] = (float) 1 / height; |
| l_diagonal[2] = -1; |
| |
| r_diagonal[0] = (float) height / width; |
| r_diagonal[1] = -1; |
| r_diagonal[2] = 0; |
| |
| r_point[0] = 0; |
| r_point[1] = 0; |
| 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[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 |
| { |
| |
| if( l_point[0] < 0 ) |
| { |
| |
| l_point[0] = 0; |
| 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; |
| |
| } |
| else |
| { /* if( l_point[0] > width ) */ |
| |
| l_point[0] = (float) width; |
| 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[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] = (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; |
| |
| } |
| else |
| { /* if( l_point[0] > width ) */ |
| |
| l_point[0] = (float) width; |
| 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; |
| } |
| } /* if */ |
| |
| return error; |
| |
| } /* icvlGetStartEnd2 */ |
| |
| /*===========================================================================*/ |
| CvStatus |
| icvGetStartEnd3( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end ) |
| { |
| |
| CvMatrix3 *F; |
| int width, height; |
| float l_diagonal[3]; |
| float r_diagonal[3]; |
| float l_point[3], r_point[3], epiline[3]; |
| CvStatus error = CV_OK; |
| |
| F = matrix; |
| |
| width = imgSize.width - 1; |
| height = imgSize.height - 1; |
| |
| l_diagonal[0] = (float) height / width; |
| l_diagonal[1] = -1; |
| l_diagonal[2] = 0; |
| |
| r_diagonal[0] = (float) 1 / width; |
| r_diagonal[1] = (float) 1 / height; |
| r_diagonal[2] = -1; |
| |
| r_point[0] = 0; |
| r_point[1] = 0; |
| 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[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 |
| { |
| |
| if( l_point[0] < 0 ) |
| { |
| |
| l_point[0] = 0; |
| 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; |
| |
| } |
| else |
| { /* if( l_point[0] > width ) */ |
| |
| l_point[0] = (float) width; |
| 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[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] = (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; |
| |
| } |
| else |
| { /* if( l_point[0] > width ) */ |
| |
| l_point[0] = (float) width; |
| 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; |
| } /* if */ |
| } /* if */ |
| |
| return error; |
| |
| } /* icvlGetStartEnd3 */ |
| |
| /*===========================================================================*/ |
| CvStatus |
| icvGetStartEnd4( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end ) |
| { |
| CvMatrix3 *F; |
| int width, height; |
| float l_diagonal[3]; |
| float r_diagonal[3]; |
| float l_point[3], r_point[3], epiline[3]; |
| CvStatus error; |
| |
| F = matrix; |
| |
| width = imgSize.width - 1; |
| height = imgSize.height - 1; |
| |
| l_diagonal[0] = (float) height / width; |
| l_diagonal[1] = -1; |
| l_diagonal[2] = 0; |
| |
| r_diagonal[0] = (float) height / width; |
| r_diagonal[1] = -1; |
| r_diagonal[2] = 0; |
| |
| r_point[0] = 0; |
| r_point[1] = 0; |
| r_point[2] = 1; |
| |
| icvMultMatrixVector3( F, r_point, epiline ); |
| error = icvCrossLines( l_diagonal, epiline, l_point ); |
| |
| if( error != CV_NO_ERR ) |
| return error; |
| |
| if( l_point[0] >= 0 && l_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 |
| { |
| |
| 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[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; |
| |
| } |
| 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[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]; |
| double a, b; |
| |
| assert( l_start_end != 0 ); |
| |
| a = fabs( l_start_end[2] - l_start_end[0] ); |
| b = fabs( l_start_end[3] - l_start_end[1] ); |
| prewarp_height = cvRound( MAX(a, b) ); |
| |
| *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]; |
| double a, b; |
| |
| assert( r_start_end != 0 ); |
| |
| a = fabs( r_start_end[2] - r_start_end[0] ); |
| b = fabs( r_start_end[3] - r_start_end[1] ); |
| prewarp_height = cvRound( MAX(a, b) ); |
| |
| *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 ! */ |
| |
| static 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; |
| } /* 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 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 )); |
| |
| /*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 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 )); |
| |
| /*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__; |
| } |
