+++ /dev/null
-/*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__;
-}
projects / opencv / blobdiff