+++ /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 "cvtypes.h"
-#include <float.h>
-#include <limits.h>
-#include "cv.h"
-
-/* Valery Mosyagin */
-
-#undef quad
-
-#define EPS64D 1e-9
-
-int cvComputeEssentialMatrix( CvMatr32f rotMatr,
- CvMatr32f transVect,
- CvMatr32f essMatr);
-
-int cvConvertEssential2Fundamental( CvMatr32f essMatr,
- CvMatr32f fundMatr,
- CvMatr32f cameraMatr1,
- CvMatr32f cameraMatr2);
-
-int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
- CvPoint3D32f* epipole1,
- CvPoint3D32f* epipole2);
-
-void icvTestPoint( CvPoint2D64d testPoint,
- CvVect64d line1,CvVect64d line2,
- CvPoint2D64d basePoint,
- int* result);
-
-
-
-int icvGetSymPoint3D( CvPoint3D64d pointCorner,
- CvPoint3D64d point1,
- CvPoint3D64d point2,
- CvPoint3D64d *pointSym2)
-{
- double len1,len2;
- double alpha;
- icvGetPieceLength3D(pointCorner,point1,&len1);
- if( len1 < EPS64D )
- {
- return CV_BADARG_ERR;
- }
- icvGetPieceLength3D(pointCorner,point2,&len2);
- alpha = len2 / len1;
-
- pointSym2->x = pointCorner.x + alpha*(point1.x - pointCorner.x);
- pointSym2->y = pointCorner.y + alpha*(point1.y - pointCorner.y);
- pointSym2->z = pointCorner.z + alpha*(point1.z - pointCorner.z);
- return CV_NO_ERR;
-}
-
-/* author Valery Mosyagin */
-
-/* Compute 3D point for scanline and alpha betta */
-int icvCompute3DPoint( double alpha,double betta,
- CvStereoLineCoeff* coeffs,
- CvPoint3D64d* point)
-{
-
- double partX;
- double partY;
- double partZ;
- double partAll;
- double invPartAll;
-
- double alphabetta = alpha*betta;
-
- partAll = alpha - betta;
- if( fabs(partAll) > 0.00001 ) /* alpha must be > betta */
- {
-
- partX = coeffs->Xcoef + coeffs->XcoefA *alpha +
- coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta;
-
- partY = coeffs->Ycoef + coeffs->YcoefA *alpha +
- coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta;
-
- partZ = coeffs->Zcoef + coeffs->ZcoefA *alpha +
- coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta;
-
- invPartAll = 1.0 / partAll;
-
- point->x = partX * invPartAll;
- point->y = partY * invPartAll;
- point->z = partZ * invPartAll;
- return CV_NO_ERR;
- }
- else
- {
- return CV_BADFACTOR_ERR;
- }
-}
-
-/*--------------------------------------------------------------------------------------*/
-
-/* Compute rotate matrix and trans vector for change system */
-int icvCreateConvertMatrVect( CvMatr64d rotMatr1,
- CvMatr64d transVect1,
- CvMatr64d rotMatr2,
- CvMatr64d transVect2,
- CvMatr64d convRotMatr,
- CvMatr64d convTransVect)
-{
- double invRotMatr2[9];
- double tmpVect[3];
-
-
- icvInvertMatrix_64d(rotMatr2,3,invRotMatr2);
- /* Test for error */
-
- icvMulMatrix_64d( rotMatr1,
- 3,3,
- invRotMatr2,
- 3,3,
- convRotMatr);
-
- icvMulMatrix_64d( convRotMatr,
- 3,3,
- transVect2,
- 1,3,
- tmpVect);
-
- icvSubVector_64d(transVect1,tmpVect,convTransVect,3);
-
-
- return CV_NO_ERR;
-}
-
-/*--------------------------------------------------------------------------------------*/
-
-/* Compute point coordinates in other system */
-int icvConvertPointSystem(CvPoint3D64d M2,
- CvPoint3D64d* M1,
- CvMatr64d rotMatr,
- CvMatr64d transVect
- )
-{
- double tmpVect[3];
-
- icvMulMatrix_64d( rotMatr,
- 3,3,
- (double*)&M2,
- 1,3,
- tmpVect);
-
- icvAddVector_64d(tmpVect,transVect,(double*)M1,3);
-
- return CV_NO_ERR;
-}
-/*--------------------------------------------------------------------------------------*/
-int icvComputeCoeffForStereoV3( double quad1[4][2],
- double quad2[4][2],
- int numScanlines,
- CvMatr64d camMatr1,
- CvMatr64d rotMatr1,
- CvMatr64d transVect1,
- CvMatr64d camMatr2,
- CvMatr64d rotMatr2,
- CvMatr64d transVect2,
- CvStereoLineCoeff* startCoeffs,
- int* needSwapCamera)
-{
- /* For each pair */
- /* In this function we must define position of cameras */
-
- CvPoint2D64d point1;
- CvPoint2D64d point2;
- CvPoint2D64d point3;
- CvPoint2D64d point4;
-
- int currLine;
- *needSwapCamera = 0;
- for( currLine = 0; currLine < numScanlines; currLine++ )
- {
- /* Compute points */
- double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */
-
- point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
- point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];
-
- point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
- point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];
-
- point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
- point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];
-
- point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
- point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];
-
- /* We can compute coeffs for this line */
- icvComCoeffForLine( point1,
- point2,
- point3,
- point4,
- camMatr1,
- rotMatr1,
- transVect1,
- camMatr2,
- rotMatr2,
- transVect2,
- &startCoeffs[currLine],
- needSwapCamera);
- }
- return CV_NO_ERR;
-}
-/*--------------------------------------------------------------------------------------*/
-int icvComputeCoeffForStereoNew( double quad1[4][2],
- double quad2[4][2],
- int numScanlines,
- CvMatr32f camMatr1,
- CvMatr32f rotMatr1,
- CvMatr32f transVect1,
- CvMatr32f camMatr2,
- CvStereoLineCoeff* startCoeffs,
- int* needSwapCamera)
-{
- /* Convert data */
-
- double camMatr1_64d[9];
- double camMatr2_64d[9];
-
- double rotMatr1_64d[9];
- double transVect1_64d[3];
-
- double rotMatr2_64d[9];
- double transVect2_64d[3];
-
- icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
- icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
-
- icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
- icvCvt_32f_64d(transVect1,transVect1_64d,3);
-
- rotMatr2_64d[0] = 1;
- rotMatr2_64d[1] = 0;
- rotMatr2_64d[2] = 0;
- rotMatr2_64d[3] = 0;
- rotMatr2_64d[4] = 1;
- rotMatr2_64d[5] = 0;
- rotMatr2_64d[6] = 0;
- rotMatr2_64d[7] = 0;
- rotMatr2_64d[8] = 1;
-
- transVect2_64d[0] = 0;
- transVect2_64d[1] = 0;
- transVect2_64d[2] = 0;
-
- int status = icvComputeCoeffForStereoV3( quad1,
- quad2,
- numScanlines,
- camMatr1_64d,
- rotMatr1_64d,
- transVect1_64d,
- camMatr2_64d,
- rotMatr2_64d,
- transVect2_64d,
- startCoeffs,
- needSwapCamera);
-
-
- return status;
-
-}
-/*--------------------------------------------------------------------------------------*/
-int icvComputeCoeffForStereo( CvStereoCamera* stereoCamera)
-{
- double quad1[4][2];
- double quad2[4][2];
-
- int i;
- for( i = 0; i < 4; i++ )
- {
- quad1[i][0] = stereoCamera->quad[0][i].x;
- quad1[i][1] = stereoCamera->quad[0][i].y;
-
- quad2[i][0] = stereoCamera->quad[1][i].x;
- quad2[i][1] = stereoCamera->quad[1][i].y;
- }
-
- icvComputeCoeffForStereoNew( quad1,
- quad2,
- stereoCamera->warpSize.height,
- stereoCamera->camera[0]->matrix,
- stereoCamera->rotMatrix,
- stereoCamera->transVector,
- stereoCamera->camera[1]->matrix,
- stereoCamera->lineCoeffs,
- &(stereoCamera->needSwapCameras));
- return CV_OK;
-}
-
-
-
-/*--------------------------------------------------------------------------------------*/
-int icvComCoeffForLine( CvPoint2D64d point1,
- CvPoint2D64d point2,
- CvPoint2D64d point3,
- CvPoint2D64d point4,
- CvMatr64d camMatr1,
- CvMatr64d rotMatr1,
- CvMatr64d transVect1,
- CvMatr64d camMatr2,
- CvMatr64d rotMatr2,
- CvMatr64d transVect2,
- CvStereoLineCoeff* coeffs,
- int* needSwapCamera)
-{
- /* Get direction for all points */
- /* Direction for camera 1 */
-
- double direct1[3];
- double direct2[3];
- double camPoint1[3];
-
- double directS3[3];
- double directS4[3];
- double direct3[3];
- double direct4[3];
- double camPoint2[3];
-
- icvGetDirectionForPoint( point1,
- camMatr1,
- (CvPoint3D64d*)direct1);
-
- icvGetDirectionForPoint( point2,
- camMatr1,
- (CvPoint3D64d*)direct2);
-
- /* Direction for camera 2 */
-
- icvGetDirectionForPoint( point3,
- camMatr2,
- (CvPoint3D64d*)directS3);
-
- icvGetDirectionForPoint( point4,
- camMatr2,
- (CvPoint3D64d*)directS4);
-
- /* Create convertion for camera 2: two direction and camera point */
-
- double convRotMatr[9];
- double convTransVect[3];
-
- icvCreateConvertMatrVect( rotMatr1,
- transVect1,
- rotMatr2,
- transVect2,
- convRotMatr,
- convTransVect);
-
- double zeroVect[3];
- zeroVect[0] = zeroVect[1] = zeroVect[2] = 0.0;
- camPoint1[0] = camPoint1[1] = camPoint1[2] = 0.0;
-
- icvConvertPointSystem(*((CvPoint3D64d*)directS3),(CvPoint3D64d*)direct3,convRotMatr,convTransVect);
- icvConvertPointSystem(*((CvPoint3D64d*)directS4),(CvPoint3D64d*)direct4,convRotMatr,convTransVect);
- icvConvertPointSystem(*((CvPoint3D64d*)zeroVect),(CvPoint3D64d*)camPoint2,convRotMatr,convTransVect);
-
- double pointB[3];
-
- int postype = 0;
-
- /* Changed order */
- /* Compute point B: xB,yB,zB */
- icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct2),
- *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct3),
- (CvPoint3D64d*)pointB);
-
- if( pointB[2] < 0 )/* If negative use other lines for cross */
- {
- postype = 1;
- icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct1),
- *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct4),
- (CvPoint3D64d*)pointB);
- }
-
- CvPoint3D64d pointNewA;
- CvPoint3D64d pointNewC;
-
- pointNewA.x = pointNewA.y = pointNewA.z = 0;
- pointNewC.x = pointNewC.y = pointNewC.z = 0;
-
- if( postype == 0 )
- {
- icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1),
- *((CvPoint3D64d*)direct1),
- *((CvPoint3D64d*)pointB),
- &pointNewA);
-
- icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2),
- *((CvPoint3D64d*)direct4),
- *((CvPoint3D64d*)pointB),
- &pointNewC);
- }
- else
- {/* In this case we must change cameras */
- *needSwapCamera = 1;
- icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2),
- *((CvPoint3D64d*)direct3),
- *((CvPoint3D64d*)pointB),
- &pointNewA);
-
- icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1),
- *((CvPoint3D64d*)direct2),
- *((CvPoint3D64d*)pointB),
- &pointNewC);
- }
-
-
- double gamma;
-
- double x1,y1,z1;
-
- x1 = camPoint1[0];
- y1 = camPoint1[1];
- z1 = camPoint1[2];
-
- double xA,yA,zA;
- double xB,yB,zB;
- double xC,yC,zC;
-
- xA = pointNewA.x;
- yA = pointNewA.y;
- zA = pointNewA.z;
-
- xB = pointB[0];
- yB = pointB[1];
- zB = pointB[2];
-
- xC = pointNewC.x;
- yC = pointNewC.y;
- zC = pointNewC.z;
-
- double len1,len2;
- len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) );
- len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) );
- gamma = len2 / len1;
-
- icvComputeStereoLineCoeffs( pointNewA,
- *((CvPoint3D64d*)pointB),
- *((CvPoint3D64d*)camPoint1),
- gamma,
- coeffs);
-
- return CV_NO_ERR;
-}
-
-
-/*--------------------------------------------------------------------------------------*/
-
-int icvGetDirectionForPoint( CvPoint2D64d point,
- CvMatr64d camMatr,
- CvPoint3D64d* direct)
-{
- /* */
- double invMatr[9];
-
- /* Invert matrix */
-
- icvInvertMatrix_64d(camMatr,3,invMatr);
- /* TEST FOR ERRORS */
-
- double vect[3];
- vect[0] = point.x;
- vect[1] = point.y;
- vect[2] = 1;
-
- /* Mul matr */
- icvMulMatrix_64d( invMatr,
- 3,3,
- vect,
- 1,3,
- (double*)direct);
-
- return CV_NO_ERR;
-}
-
-/*--------------------------------------------------------------------------------------*/
-
-int icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,
- CvPoint3D64d point21,CvPoint3D64d point22,
- CvPoint3D64d* midPoint)
-{
- double xM,yM,zM;
- double xN,yN,zN;
-
- double xA,yA,zA;
- double xB,yB,zB;
-
- double xC,yC,zC;
- double xD,yD,zD;
-
- xA = point11.x;
- yA = point11.y;
- zA = point11.z;
-
- xB = point12.x;
- yB = point12.y;
- zB = point12.z;
-
- xC = point21.x;
- yC = point21.y;
- zC = point21.z;
-
- xD = point22.x;
- yD = point22.y;
- zD = point22.z;
-
- double a11,a12,a21,a22;
- double b1,b2;
-
- a11 = (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA);
- a12 = -(xD-xC)*(xB-xA)-(yD-yC)*(yB-yA)-(zD-zC)*(zB-zA);
- a21 = (xB-xA)*(xD-xC)+(yB-yA)*(yD-yC)+(zB-zA)*(zD-zC);
- a22 = -(xD-xC)*(xD-xC)-(yD-yC)*(yD-yC)-(zD-zC)*(zD-zC);
- b1 = -( (xA-xC)*(xB-xA)+(yA-yC)*(yB-yA)+(zA-zC)*(zB-zA) );
- b2 = -( (xA-xC)*(xD-xC)+(yA-yC)*(yD-yC)+(zA-zC)*(zD-zC) );
-
- double delta;
- double deltaA,deltaB;
- double alpha,betta;
-
- delta = a11*a22-a12*a21;
-
- if( fabs(delta) < EPS64D )
- {
- /*return ERROR;*/
- }
-
- deltaA = b1*a22-b2*a12;
- deltaB = a11*b2-b1*a21;
-
- alpha = deltaA / delta;
- betta = deltaB / delta;
-
- xM = xA+alpha*(xB-xA);
- yM = yA+alpha*(yB-yA);
- zM = zA+alpha*(zB-zA);
-
- xN = xC+betta*(xD-xC);
- yN = yC+betta*(yD-yC);
- zN = zC+betta*(zD-zC);
-
- /* Compute middle point */
- midPoint->x = (xM + xN) * 0.5;
- midPoint->y = (yM + yN) * 0.5;
- midPoint->z = (zM + zN) * 0.5;
-
- return CV_NO_ERR;
-}
-
-/*--------------------------------------------------------------------------------------*/
-
-int icvComputeStereoLineCoeffs( CvPoint3D64d pointA,
- CvPoint3D64d pointB,
- CvPoint3D64d pointCam1,
- double gamma,
- CvStereoLineCoeff* coeffs)
-{
- double x1,y1,z1;
-
- x1 = pointCam1.x;
- y1 = pointCam1.y;
- z1 = pointCam1.z;
-
- double xA,yA,zA;
- double xB,yB,zB;
-
- xA = pointA.x;
- yA = pointA.y;
- zA = pointA.z;
-
- xB = pointB.x;
- yB = pointB.y;
- zB = pointB.z;
-
- if( gamma > 0 )
- {
- coeffs->Xcoef = -x1 + xA;
- coeffs->XcoefA = xB + x1 - xA;
- coeffs->XcoefB = -xA - gamma * x1 + gamma * xA;
- coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;
-
- coeffs->Ycoef = -y1 + yA;
- coeffs->YcoefA = yB + y1 - yA;
- coeffs->YcoefB = -yA - gamma * y1 + gamma * yA;
- coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;
-
- coeffs->Zcoef = -z1 + zA;
- coeffs->ZcoefA = zB + z1 - zA;
- coeffs->ZcoefB = -zA - gamma * z1 + gamma * zA;
- coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
- }
- else
- {
- gamma = - gamma;
- coeffs->Xcoef = -( -x1 + xA);
- coeffs->XcoefB = -( xB + x1 - xA);
- coeffs->XcoefA = -( -xA - gamma * x1 + gamma * xA);
- coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA);
-
- coeffs->Ycoef = -( -y1 + yA);
- coeffs->YcoefB = -( yB + y1 - yA);
- coeffs->YcoefA = -( -yA - gamma * y1 + gamma * yA);
- coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA);
-
- coeffs->Zcoef = -( -z1 + zA);
- coeffs->ZcoefB = -( zB + z1 - zA);
- coeffs->ZcoefA = -( -zA - gamma * z1 + gamma * zA);
- coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA);
- }
-
-
-
- return CV_NO_ERR;
-}
-/*--------------------------------------------------------------------------------------*/
-
-
-/*---------------------------------------------------------------------------------------*/
-
-/* This function get minimum angle started at point which contains rect */
-int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2)
-{
- /* Get crosslines with image corners */
-
- /* Find four lines */
-
- CvPoint2D64d pa,pb,pc,pd;
-
- pa.x = 0;
- pa.y = 0;
-
- pb.x = imageSize.width-1;
- pb.y = 0;
-
- pd.x = imageSize.width-1;
- pd.y = imageSize.height-1;
-
- pc.x = 0;
- pc.y = imageSize.height-1;
-
- /* We can compute points for angle */
- /* Test for place section */
-
- if( startPoint.x < 0 )
- {/* 1,4,7 */
- if( startPoint.y < 0)
- {/* 1 */
- *point1 = pb;
- *point2 = pc;
- }
- else if( startPoint.y > imageSize.height-1 )
- {/* 7 */
- *point1 = pa;
- *point2 = pd;
- }
- else
- {/* 4 */
- *point1 = pa;
- *point2 = pc;
- }
- }
- else if ( startPoint.x > imageSize.width-1 )
- {/* 3,6,9 */
- if( startPoint.y < 0 )
- {/* 3 */
- *point1 = pa;
- *point2 = pd;
- }
- else if ( startPoint.y > imageSize.height-1 )
- {/* 9 */
- *point1 = pb;
- *point2 = pc;
- }
- else
- {/* 6 */
- *point1 = pb;
- *point2 = pd;
- }
- }
- else
- {/* 2,5,8 */
- if( startPoint.y < 0 )
- {/* 2 */
- if( startPoint.x < imageSize.width/2 )
- {
- *point1 = pb;
- *point2 = pa;
- }
- else
- {
- *point1 = pa;
- *point2 = pb;
- }
- }
- else if( startPoint.y > imageSize.height-1 )
- {/* 8 */
- if( startPoint.x < imageSize.width/2 )
- {
- *point1 = pc;
- *point2 = pd;
- }
- else
- {
- *point1 = pd;
- *point2 = pc;
- }
- }
- else
- {/* 5 - point in the image */
- return 2;
- }
- }
- return 0;
-}/* GetAngleLine */
-
-/*---------------------------------------------------------------------------------------*/
-
-void icvGetCoefForPiece( CvPoint2D64d p_start,CvPoint2D64d p_end,
- double *a,double *b,double *c,
- int* result)
-{
- double det;
- double detA,detB,detC;
-
- det = p_start.x*p_end.y+p_end.x+p_start.y-p_end.y-p_start.y*p_end.x-p_start.x;
- if( fabs(det) < EPS64D)/* Error */
- {
- *result = 0;
- return;
- }
-
- detA = p_start.y - p_end.y;
- detB = p_end.x - p_start.x;
- detC = p_start.x*p_end.y - p_end.x*p_start.y;
-
- double invDet = 1.0 / det;
- *a = detA * invDet;
- *b = detB * invDet;
- *c = detC * invDet;
-
- *result = 1;
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-
-/* Get common area of rectifying */
-void icvGetCommonArea( CvSize imageSize,
- CvPoint3D64d epipole1,CvPoint3D64d epipole2,
- CvMatr64d fundMatr,
- CvVect64d coeff11,CvVect64d coeff12,
- CvVect64d coeff21,CvVect64d coeff22,
- int* result)
-{
- int res = 0;
- CvPoint2D64d point11;
- CvPoint2D64d point12;
- CvPoint2D64d point21;
- CvPoint2D64d point22;
-
- double corr11[3];
- double corr12[3];
- double corr21[3];
- double corr22[3];
-
- double pointW11[3];
- double pointW12[3];
- double pointW21[3];
- double pointW22[3];
-
- double transFundMatr[3*3];
- /* Compute transpose of fundamental matrix */
- icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );
-
- CvPoint2D64d epipole1_2d;
- CvPoint2D64d epipole2_2d;
-
- if( fabs(epipole1.z) < 1e-8 )
- {/* epipole1 in infinity */
- *result = 0;
- return;
- }
- epipole1_2d.x = epipole1.x / epipole1.z;
- epipole1_2d.y = epipole1.y / epipole1.z;
-
- if( fabs(epipole2.z) < 1e-8 )
- {/* epipole2 in infinity */
- *result = 0;
- return;
- }
- epipole2_2d.x = epipole2.x / epipole2.z;
- epipole2_2d.y = epipole2.y / epipole2.z;
-
- int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12);
- if( stat == 2 )
- {
- /* No angle */
- *result = 0;
- return;
- }
-
- stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22);
- if( stat == 2 )
- {
- /* No angle */
- *result = 0;
- return;
- }
-
- /* ============= Computation for line 1 ================ */
- /* Find correspondence line for angle points11 */
- /* corr21 = Fund'*p1 */
-
- pointW11[0] = point11.x;
- pointW11[1] = point11.y;
- pointW11[2] = 1.0;
-
- icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
- pointW11,
- corr21,
- 3,3);
-
- /* Find crossing of line with image 2 */
- CvPoint2D64d start;
- CvPoint2D64d end;
- icvGetCrossRectDirect( imageSize,
- corr21[0],corr21[1],corr21[2],
- &start,&end,
- &res);
-
- if( res == 0 )
- {/* We have not cross */
- /* We must define new angle */
-
- pointW21[0] = point21.x;
- pointW21[1] = point21.y;
- pointW21[2] = 1.0;
-
- /* Find correspondence line for this angle points */
- /* We know point and try to get corr line */
- /* For point21 */
- /* corr11 = Fund * p21 */
-
- icvTransformVector_64d( fundMatr, /* !!! Modified */
- pointW21,
- corr11,
- 3,3);
-
- /* We have cross. And it's result cross for up line. Set result coefs */
-
- /* Set coefs for line 1 image 1 */
- coeff11[0] = corr11[0];
- coeff11[1] = corr11[1];
- coeff11[2] = corr11[2];
-
- /* Set coefs for line 1 image 2 */
- icvGetCoefForPiece( epipole2_2d,point21,
- &coeff21[0],&coeff21[1],&coeff21[2],
- &res);
- if( res == 0 )
- {
- *result = 0;
- return;/* Error */
- }
- }
- else
- {/* Line 1 cross image 2 */
- /* Set coefs for line 1 image 1 */
- icvGetCoefForPiece( epipole1_2d,point11,
- &coeff11[0],&coeff11[1],&coeff11[2],
- &res);
- if( res == 0 )
- {
- *result = 0;
- return;/* Error */
- }
-
- /* Set coefs for line 1 image 2 */
- coeff21[0] = corr21[0];
- coeff21[1] = corr21[1];
- coeff21[2] = corr21[2];
-
- }
-
- /* ============= Computation for line 2 ================ */
- /* Find correspondence line for angle points11 */
- /* corr22 = Fund*p2 */
-
- pointW12[0] = point12.x;
- pointW12[1] = point12.y;
- pointW12[2] = 1.0;
-
- icvTransformVector_64d( transFundMatr,
- pointW12,
- corr22,
- 3,3);
-
- /* Find crossing of line with image 2 */
- icvGetCrossRectDirect( imageSize,
- corr22[0],corr22[1],corr22[2],
- &start,&end,
- &res);
-
- if( res == 0 )
- {/* We have not cross */
- /* We must define new angle */
-
- pointW22[0] = point22.x;
- pointW22[1] = point22.y;
- pointW22[2] = 1.0;
-
- /* Find correspondence line for this angle points */
- /* We know point and try to get corr line */
- /* For point21 */
- /* corr2 = Fund' * p1 */
-
- icvTransformVector_64d( fundMatr,
- pointW22,
- corr12,
- 3,3);
-
-
- /* We have cross. And it's result cross for down line. Set result coefs */
-
- /* Set coefs for line 2 image 1 */
- coeff12[0] = corr12[0];
- coeff12[1] = corr12[1];
- coeff12[2] = corr12[2];
-
- /* Set coefs for line 1 image 2 */
- icvGetCoefForPiece( epipole2_2d,point22,
- &coeff22[0],&coeff22[1],&coeff22[2],
- &res);
- if( res == 0 )
- {
- *result = 0;
- return;/* Error */
- }
- }
- else
- {/* Line 2 cross image 2 */
- /* Set coefs for line 2 image 1 */
- icvGetCoefForPiece( epipole1_2d,point12,
- &coeff12[0],&coeff12[1],&coeff12[2],
- &res);
- if( res == 0 )
- {
- *result = 0;
- return;/* Error */
- }
-
- /* Set coefs for line 1 image 2 */
- coeff22[0] = corr22[0];
- coeff22[1] = corr22[1];
- coeff22[2] = corr22[2];
-
- }
-
- /* Now we know common area */
-
- return;
-
-}/* GetCommonArea */
-
-/*---------------------------------------------------------------------------------------*/
-
-/* Get cross for direction1 and direction2 */
-/* Result = 1 - cross */
-/* Result = 2 - parallel and not equal */
-/* Result = 3 - parallel and equal */
-
-void icvGetCrossDirectDirect( CvVect64d direct1,CvVect64d direct2,
- CvPoint2D64d *cross,int* result)
-{
- double det = direct1[0]*direct2[1] - direct2[0]*direct1[1];
- double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2];
-
- if( fabs(det) > EPS64D )
- {/* Have cross */
- cross->x = detx/det;
- cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det;
- *result = 1;
- }
- else
- {/* may be parallel */
- if( fabs(detx) > EPS64D )
- {/* parallel and not equal */
- *result = 2;
- }
- else
- {/* equals */
- *result = 3;
- }
- }
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-
-/* Get cross for piece p1,p2 and direction a,b,c */
-/* Result = 0 - no cross */
-/* Result = 1 - cross */
-/* Result = 2 - parallel and not equal */
-/* Result = 3 - parallel and equal */
-
-void icvGetCrossPieceDirect( CvPoint2D64d p_start,CvPoint2D64d p_end,
- double a,double b,double c,
- CvPoint2D64d *cross,int* result)
-{
-
- if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 )
- {/* Have cross */
- double det;
- double detxc,detyc;
-
- det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
-
- if( fabs(det) < EPS64D )
- {/* lines are parallel and may be equal or line is point */
- if( fabs(a*p_start.x + b*p_start.y + c) < EPS64D )
- {/* line is point or not diff */
- *result = 3;
- return;
- }
- else
- {
- *result = 2;
- }
- return;
- }
-
- detxc = b*(p_end.y*p_start.x - p_start.y*p_end.x) + c*(p_start.x - p_end.x);
- detyc = a*(p_end.x*p_start.y - p_start.x*p_end.y) + c*(p_start.y - p_end.y);
-
- cross->x = detxc / det;
- cross->y = detyc / det;
- *result = 1;
-
- }
- else
- {
- *result = 0;
- }
- return;
-}
-/*--------------------------------------------------------------------------------------*/
-
-void icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end,
- CvPoint2D64d p2_start,CvPoint2D64d p2_end,
- CvPoint2D64d* cross,
- int* result)
-{
- double ex1,ey1,ex2,ey2;
- double px1,py1,px2,py2;
- double del;
- double delA,delB,delX,delY;
- double alpha,betta;
-
- ex1 = p1_start.x;
- ey1 = p1_start.y;
- ex2 = p1_end.x;
- ey2 = p1_end.y;
-
- px1 = p2_start.x;
- py1 = p2_start.y;
- px2 = p2_end.x;
- py2 = p2_end.y;
-
- del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2);
- if( fabs(del) <= EPS64D )
- {/* May be they are parallel !!! */
- *result = 0;
- return;
- }
-
- delA = (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1);
- delB = (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1);
-
- alpha = delA / del;
- betta = delB / del;
-
- if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
- {
- *result = 0;
- return;
- }
-
- delX = (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
- (ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2));
-
- delY = (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
- (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2));
-
- cross->x = delX / del;
- cross->y = delY / del;
-
- *result = 1;
- return;
-}
-
-
-/*---------------------------------------------------------------------------------------*/
-
-void icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist)
-{
- double dx = point2.x - point1.x;
- double dy = point2.y - point1.y;
- *dist = sqrt( dx*dx + dy*dy );
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-
-void icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist)
-{
- double dx = point2.x - point1.x;
- double dy = point2.y - point1.y;
- double dz = point2.z - point1.z;
- *dist = sqrt( dx*dx + dy*dy + dz*dz );
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-
-/* Find line from epipole which cross image rect */
-/* Find points of cross 0 or 1 or 2. Return number of points in cross */
-void icvGetCrossRectDirect( CvSize imageSize,
- double a,double b,double c,
- CvPoint2D64d *start,CvPoint2D64d *end,
- int* result)
-{
- CvPoint2D64d frameBeg;
- CvPoint2D64d frameEnd;
- CvPoint2D64d cross[4];
- int haveCross[4];
-
- haveCross[0] = 0;
- haveCross[1] = 0;
- haveCross[2] = 0;
- haveCross[3] = 0;
-
- frameBeg.x = 0;
- frameBeg.y = 0;
- frameEnd.x = imageSize.width;
- frameEnd.y = 0;
-
- icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]);
-
- frameBeg.x = imageSize.width;
- frameBeg.y = 0;
- frameEnd.x = imageSize.width;
- frameEnd.y = imageSize.height;
- icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]);
-
- frameBeg.x = imageSize.width;
- frameBeg.y = imageSize.height;
- frameEnd.x = 0;
- frameEnd.y = imageSize.height;
- icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]);
-
- frameBeg.x = 0;
- frameBeg.y = imageSize.height;
- frameEnd.x = 0;
- frameEnd.y = 0;
- icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]);
-
- double maxDist;
-
- int maxI=0,maxJ=0;
-
-
- int i,j;
-
- maxDist = -1.0;
-
- double distance;
-
- for( i = 0; i < 3; i++ )
- {
- if( haveCross[i] == 1 )
- {
- for( j = i + 1; j < 4; j++ )
- {
- if( haveCross[j] == 1)
- {/* Compute dist */
- icvGetPieceLength(cross[i],cross[j],&distance);
- if( distance > maxDist )
- {
- maxI = i;
- maxJ = j;
- maxDist = distance;
- }
- }
- }
- }
- }
-
- if( maxDist >= 0 )
- {/* We have cross */
- *start = cross[maxI];
- *result = 1;
- if( maxDist > 0 )
- {
- *end = cross[maxJ];
- *result = 2;
- }
- }
- else
- {
- *result = 0;
- }
-
- return;
-}/* GetCrossRectDirect */
-
-/*---------------------------------------------------------------------------------------*/
-void icvProjectPointToImage( CvPoint3D64d point,
- CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,
- CvPoint2D64d* projPoint)
-{
-
- double tmpVect1[3];
- double tmpVect2[3];
-
- icvMulMatrix_64d ( rotMatr,
- 3,3,
- (double*)&point,
- 1,3,
- tmpVect1);
-
- icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3);
-
- icvMulMatrix_64d ( camMatr,
- 3,3,
- tmpVect2,
- 1,3,
- tmpVect1);
-
- projPoint->x = tmpVect1[0] / tmpVect1[2];
- projPoint->y = tmpVect1[1] / tmpVect1[2];
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-/* Get quads for transform images */
-void icvGetQuadsTransform(
- CvSize imageSize,
- CvMatr64d camMatr1,
- CvMatr64d rotMatr1,
- CvVect64d transVect1,
- CvMatr64d camMatr2,
- CvMatr64d rotMatr2,
- CvVect64d transVect2,
- CvSize* warpSize,
- double quad1[4][2],
- double quad2[4][2],
- CvMatr64d fundMatr,
- CvPoint3D64d* epipole1,
- CvPoint3D64d* epipole2
- )
-{
- /* First compute fundamental matrix and epipoles */
- int res;
-
-
- /* Compute epipoles and fundamental matrix using new functions */
- {
- double convRotMatr[9];
- double convTransVect[3];
-
- icvCreateConvertMatrVect( rotMatr1,
- transVect1,
- rotMatr2,
- transVect2,
- convRotMatr,
- convTransVect);
- float convRotMatr_32f[9];
- float convTransVect_32f[3];
-
- icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9);
- icvCvt_64d_32f(convTransVect,convTransVect_32f,3);
-
- /* We know R and t */
- /* Compute essential matrix */
- float essMatr[9];
- float fundMatr_32f[9];
-
- float camMatr1_32f[9];
- float camMatr2_32f[9];
-
- icvCvt_64d_32f(camMatr1,camMatr1_32f,9);
- icvCvt_64d_32f(camMatr2,camMatr2_32f,9);
-
- cvComputeEssentialMatrix( convRotMatr_32f,
- convTransVect_32f,
- essMatr);
-
- cvConvertEssential2Fundamental( essMatr,
- fundMatr_32f,
- camMatr1_32f,
- camMatr2_32f);
-
- CvPoint3D32f epipole1_32f;
- CvPoint3D32f epipole2_32f;
-
- cvComputeEpipolesFromFundMatrix( fundMatr_32f,
- &epipole1_32f,
- &epipole2_32f);
- /* copy to 64d epipoles */
- epipole1->x = epipole1_32f.x;
- epipole1->y = epipole1_32f.y;
- epipole1->z = epipole1_32f.z;
-
- epipole2->x = epipole2_32f.x;
- epipole2->y = epipole2_32f.y;
- epipole2->z = epipole2_32f.z;
-
- /* Convert fundamental matrix */
- icvCvt_32f_64d(fundMatr_32f,fundMatr,9);
- }
-
- double coeff11[3];
- double coeff12[3];
- double coeff21[3];
- double coeff22[3];
-
- icvGetCommonArea( imageSize,
- *epipole1,*epipole2,
- fundMatr,
- coeff11,coeff12,
- coeff21,coeff22,
- &res);
-
- CvPoint2D64d point11, point12,point21, point22;
- double width1,width2;
- double height1,height2;
- double tmpHeight1,tmpHeight2;
-
- CvPoint2D64d epipole1_2d;
- CvPoint2D64d epipole2_2d;
-
- /* ----- Image 1 ----- */
- if( fabs(epipole1->z) < 1e-8 )
- {
- return;
- }
- epipole1_2d.x = epipole1->x / epipole1->z;
- epipole1_2d.y = epipole1->y / epipole1->z;
-
- icvGetCutPiece( coeff11,coeff12,
- epipole1_2d,
- imageSize,
- &point11,&point12,
- &point21,&point22,
- &res);
-
- /* Compute distance */
- icvGetPieceLength(point11,point21,&width1);
- icvGetPieceLength(point11,point12,&tmpHeight1);
- icvGetPieceLength(point21,point22,&tmpHeight2);
- height1 = MAX(tmpHeight1,tmpHeight2);
-
- quad1[0][0] = point11.x;
- quad1[0][1] = point11.y;
-
- quad1[1][0] = point21.x;
- quad1[1][1] = point21.y;
-
- quad1[2][0] = point22.x;
- quad1[2][1] = point22.y;
-
- quad1[3][0] = point12.x;
- quad1[3][1] = point12.y;
-
- /* ----- Image 2 ----- */
- if( fabs(epipole2->z) < 1e-8 )
- {
- return;
- }
- epipole2_2d.x = epipole2->x / epipole2->z;
- epipole2_2d.y = epipole2->y / epipole2->z;
-
- icvGetCutPiece( coeff21,coeff22,
- epipole2_2d,
- imageSize,
- &point11,&point12,
- &point21,&point22,
- &res);
-
- /* Compute distance */
- icvGetPieceLength(point11,point21,&width2);
- icvGetPieceLength(point11,point12,&tmpHeight1);
- icvGetPieceLength(point21,point22,&tmpHeight2);
- height2 = MAX(tmpHeight1,tmpHeight2);
-
- quad2[0][0] = point11.x;
- quad2[0][1] = point11.y;
-
- quad2[1][0] = point21.x;
- quad2[1][1] = point21.y;
-
- quad2[2][0] = point22.x;
- quad2[2][1] = point22.y;
-
- quad2[3][0] = point12.x;
- quad2[3][1] = point12.y;
-
-
- /*=======================================================*/
- /* This is a new additional way to compute quads. */
- /* We must correct quads */
- {
- double convRotMatr[9];
- double convTransVect[3];
-
- double newQuad1[4][2];
- double newQuad2[4][2];
-
-
- icvCreateConvertMatrVect( rotMatr1,
- transVect1,
- rotMatr2,
- transVect2,
- convRotMatr,
- convTransVect);
-
- /* -------------Compute for first image-------------- */
- CvPoint2D32f pointb1;
- CvPoint2D32f pointe1;
-
- CvPoint2D32f pointb2;
- CvPoint2D32f pointe2;
-
- pointb1.x = (float)quad1[0][0];
- pointb1.y = (float)quad1[0][1];
-
- pointe1.x = (float)quad1[3][0];
- pointe1.y = (float)quad1[3][1];
-
- icvComputeeInfiniteProject1(convRotMatr,
- camMatr1,
- camMatr2,
- pointb1,
- &pointb2);
-
- icvComputeeInfiniteProject1(convRotMatr,
- camMatr1,
- camMatr2,
- pointe1,
- &pointe2);
-
- /* JUST TEST FOR POINT */
-
- /* Compute distances */
- double dxOld,dyOld;
- double dxNew,dyNew;
- double distOld,distNew;
-
- dxOld = quad2[1][0] - quad2[0][0];
- dyOld = quad2[1][1] - quad2[0][1];
- distOld = dxOld*dxOld + dyOld*dyOld;
-
- dxNew = quad2[1][0] - pointb2.x;
- dyNew = quad2[1][1] - pointb2.y;
- distNew = dxNew*dxNew + dyNew*dyNew;
-
- if( distNew > distOld )
- {/* Get new points for second quad */
- newQuad2[0][0] = pointb2.x;
- newQuad2[0][1] = pointb2.y;
- newQuad2[3][0] = pointe2.x;
- newQuad2[3][1] = pointe2.y;
- newQuad1[0][0] = quad1[0][0];
- newQuad1[0][1] = quad1[0][1];
- newQuad1[3][0] = quad1[3][0];
- newQuad1[3][1] = quad1[3][1];
- }
- else
- {/* Get new points for first quad */
-
- pointb2.x = (float)quad2[0][0];
- pointb2.y = (float)quad2[0][1];
-
- pointe2.x = (float)quad2[3][0];
- pointe2.y = (float)quad2[3][1];
-
- icvComputeeInfiniteProject2(convRotMatr,
- camMatr1,
- camMatr2,
- &pointb1,
- pointb2);
-
- icvComputeeInfiniteProject2(convRotMatr,
- camMatr1,
- camMatr2,
- &pointe1,
- pointe2);
-
-
- /* JUST TEST FOR POINT */
-
- newQuad2[0][0] = quad2[0][0];
- newQuad2[0][1] = quad2[0][1];
- newQuad2[3][0] = quad2[3][0];
- newQuad2[3][1] = quad2[3][1];
-
- newQuad1[0][0] = pointb1.x;
- newQuad1[0][1] = pointb1.y;
- newQuad1[3][0] = pointe1.x;
- newQuad1[3][1] = pointe1.y;
- }
-
- /* -------------Compute for second image-------------- */
- pointb1.x = (float)quad1[1][0];
- pointb1.y = (float)quad1[1][1];
-
- pointe1.x = (float)quad1[2][0];
- pointe1.y = (float)quad1[2][1];
-
- icvComputeeInfiniteProject1(convRotMatr,
- camMatr1,
- camMatr2,
- pointb1,
- &pointb2);
-
- icvComputeeInfiniteProject1(convRotMatr,
- camMatr1,
- camMatr2,
- pointe1,
- &pointe2);
-
- /* Compute distances */
-
- dxOld = quad2[0][0] - quad2[1][0];
- dyOld = quad2[0][1] - quad2[1][1];
- distOld = dxOld*dxOld + dyOld*dyOld;
-
- dxNew = quad2[0][0] - pointb2.x;
- dyNew = quad2[0][1] - pointb2.y;
- distNew = dxNew*dxNew + dyNew*dyNew;
-
- if( distNew > distOld )
- {/* Get new points for second quad */
- newQuad2[1][0] = pointb2.x;
- newQuad2[1][1] = pointb2.y;
- newQuad2[2][0] = pointe2.x;
- newQuad2[2][1] = pointe2.y;
- newQuad1[1][0] = quad1[1][0];
- newQuad1[1][1] = quad1[1][1];
- newQuad1[2][0] = quad1[2][0];
- newQuad1[2][1] = quad1[2][1];
- }
- else
- {/* Get new points for first quad */
-
- pointb2.x = (float)quad2[1][0];
- pointb2.y = (float)quad2[1][1];
-
- pointe2.x = (float)quad2[2][0];
- pointe2.y = (float)quad2[2][1];
-
- icvComputeeInfiniteProject2(convRotMatr,
- camMatr1,
- camMatr2,
- &pointb1,
- pointb2);
-
- icvComputeeInfiniteProject2(convRotMatr,
- camMatr1,
- camMatr2,
- &pointe1,
- pointe2);
-
- newQuad2[1][0] = quad2[1][0];
- newQuad2[1][1] = quad2[1][1];
- newQuad2[2][0] = quad2[2][0];
- newQuad2[2][1] = quad2[2][1];
-
- newQuad1[1][0] = pointb1.x;
- newQuad1[1][1] = pointb1.y;
- newQuad1[2][0] = pointe1.x;
- newQuad1[2][1] = pointe1.y;
- }
-
-
-
-/*-------------------------------------------------------------------------------*/
-
- /* Copy new quads to old quad */
- int i;
- for( i = 0; i < 4; i++ )
- {
- {
- quad1[i][0] = newQuad1[i][0];
- quad1[i][1] = newQuad1[i][1];
- quad2[i][0] = newQuad2[i][0];
- quad2[i][1] = newQuad2[i][1];
- }
- }
- }
- /*=======================================================*/
-
- double warpWidth,warpHeight;
-
- warpWidth = MAX(width1,width2);
- warpHeight = MAX(height1,height2);
-
- warpSize->width = (int)warpWidth;
- warpSize->height = (int)warpHeight;
-
- warpSize->width = cvRound(warpWidth-1);
- warpSize->height = cvRound(warpHeight-1);
-
-/* !!! by Valery Mosyagin. this lines added just for test no warp */
- warpSize->width = imageSize.width;
- warpSize->height = imageSize.height;
-
- return;
-}
-
-
-/*---------------------------------------------------------------------------------------*/
-
-void icvGetQuadsTransformNew( CvSize imageSize,
- CvMatr32f camMatr1,
- CvMatr32f camMatr2,
- CvMatr32f rotMatr1,
- CvVect32f transVect1,
- CvSize* warpSize,
- double quad1[4][2],
- double quad2[4][2],
- CvMatr32f fundMatr,
- CvPoint3D32f* epipole1,
- CvPoint3D32f* epipole2
- )
-{
- /* Convert data */
- /* Convert camera matrix */
- double camMatr1_64d[9];
- double camMatr2_64d[9];
- double rotMatr1_64d[9];
- double transVect1_64d[3];
- double rotMatr2_64d[9];
- double transVect2_64d[3];
- double fundMatr_64d[9];
- CvPoint3D64d epipole1_64d;
- CvPoint3D64d epipole2_64d;
-
- icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
- icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
- icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
- icvCvt_32f_64d(transVect1,transVect1_64d,3);
-
- /* Create vector and matrix */
-
- rotMatr2_64d[0] = 1;
- rotMatr2_64d[1] = 0;
- rotMatr2_64d[2] = 0;
- rotMatr2_64d[3] = 0;
- rotMatr2_64d[4] = 1;
- rotMatr2_64d[5] = 0;
- rotMatr2_64d[6] = 0;
- rotMatr2_64d[7] = 0;
- rotMatr2_64d[8] = 1;
-
- transVect2_64d[0] = 0;
- transVect2_64d[1] = 0;
- transVect2_64d[2] = 0;
-
- icvGetQuadsTransform( imageSize,
- camMatr1_64d,
- rotMatr1_64d,
- transVect1_64d,
- camMatr2_64d,
- rotMatr2_64d,
- transVect2_64d,
- warpSize,
- quad1,
- quad2,
- fundMatr_64d,
- &epipole1_64d,
- &epipole2_64d
- );
-
- /* Convert epipoles */
- epipole1->x = (float)(epipole1_64d.x);
- epipole1->y = (float)(epipole1_64d.y);
- epipole1->z = (float)(epipole1_64d.z);
-
- epipole2->x = (float)(epipole2_64d.x);
- epipole2->y = (float)(epipole2_64d.y);
- epipole2->z = (float)(epipole2_64d.z);
-
- /* Convert fundamental matrix */
- icvCvt_64d_32f(fundMatr_64d,fundMatr,9);
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-void icvGetQuadsTransformStruct( CvStereoCamera* stereoCamera)
-{
- /* Wrapper for icvGetQuadsTransformNew */
-
-
- double quad1[4][2];
- double quad2[4][2];
-
- icvGetQuadsTransformNew( cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),cvRound(stereoCamera->camera[0]->imgSize[1])),
- stereoCamera->camera[0]->matrix,
- stereoCamera->camera[1]->matrix,
- stereoCamera->rotMatrix,
- stereoCamera->transVector,
- &(stereoCamera->warpSize),
- quad1,
- quad2,
- stereoCamera->fundMatr,
- &(stereoCamera->epipole[0]),
- &(stereoCamera->epipole[1])
- );
-
- int i;
- for( i = 0; i < 4; i++ )
- {
- stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]);
- stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]);
- }
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
-{
- /* For given intrinsic and extrinsic parameters computes rest parameters
- ** such as fundamental matrix. warping coeffs, epipoles, ...
- */
-
-
- /* compute rotate matrix and translate vector */
- double rotMatr1[9];
- double rotMatr2[9];
-
- double transVect1[3];
- double transVect2[3];
-
- double convRotMatr[9];
- double convTransVect[3];
-
- /* fill matrices */
- icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9);
- icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9);
-
- icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3);
- icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3);
-
- icvCreateConvertMatrVect( rotMatr1,
- transVect1,
- rotMatr2,
- transVect2,
- convRotMatr,
- convTransVect);
-
- /* copy to stereo camera params */
- icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9);
- icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3);
-
-
- icvGetQuadsTransformStruct(stereoCamera);
- icvComputeRestStereoParams(stereoCamera);
-}
-
-
-
-/*---------------------------------------------------------------------------------------*/
-
-/* Get cut line for one image */
-void icvGetCutPiece( CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,
- CvPoint2D64d epipole,
- CvSize imageSize,
- CvPoint2D64d* point11,CvPoint2D64d* point12,
- CvPoint2D64d* point21,CvPoint2D64d* point22,
- int* result)
-{
- /* Compute nearest cut line to epipole */
- /* Get corners inside sector */
- /* Collect all candidate point */
-
- CvPoint2D64d candPoints[8];
- CvPoint2D64d midPoint;
- int numPoints = 0;
- int res;
- int i;
-
- double cutLine1[3];
- double cutLine2[3];
-
- /* Find middle line of sector */
- double midLine[3];
-
-
- /* Different way */
- CvPoint2D64d pointOnLine1; pointOnLine1.x = pointOnLine1.y = 0;
- CvPoint2D64d pointOnLine2; pointOnLine2.x = pointOnLine2.y = 0;
-
- CvPoint2D64d start1,end1;
-
- icvGetCrossRectDirect( imageSize,
- areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
- &start1,&end1,&res);
- if( res > 0 )
- {
- pointOnLine1 = start1;
- }
-
- icvGetCrossRectDirect( imageSize,
- areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
- &start1,&end1,&res);
- if( res > 0 )
- {
- pointOnLine2 = start1;
- }
-
- icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);
-
- icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);
-
- /* Test corner points */
- CvPoint2D64d cornerPoint;
- CvPoint2D64d tmpPoints[2];
-
- cornerPoint.x = 0;
- cornerPoint.y = 0;
- icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
- if( res == 1 )
- {/* Add point */
- candPoints[numPoints] = cornerPoint;
- numPoints++;
- }
-
- cornerPoint.x = imageSize.width;
- cornerPoint.y = 0;
- icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
- if( res == 1 )
- {/* Add point */
- candPoints[numPoints] = cornerPoint;
- numPoints++;
- }
-
- cornerPoint.x = imageSize.width;
- cornerPoint.y = imageSize.height;
- icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
- if( res == 1 )
- {/* Add point */
- candPoints[numPoints] = cornerPoint;
- numPoints++;
- }
-
- cornerPoint.x = 0;
- cornerPoint.y = imageSize.height;
- icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
- if( res == 1 )
- {/* Add point */
- candPoints[numPoints] = cornerPoint;
- numPoints++;
- }
-
- /* Find cross line 1 with image border */
- icvGetCrossRectDirect( imageSize,
- areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
- &tmpPoints[0], &tmpPoints[1],
- &res);
- for( i = 0; i < res; i++ )
- {
- candPoints[numPoints++] = tmpPoints[i];
- }
-
- /* Find cross line 2 with image border */
- icvGetCrossRectDirect( imageSize,
- areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
- &tmpPoints[0], &tmpPoints[1],
- &res);
-
- for( i = 0; i < res; i++ )
- {
- candPoints[numPoints++] = tmpPoints[i];
- }
-
- if( numPoints < 2 )
- {
- *result = 0;
- return;/* Error. Not enought points */
- }
- /* Project all points to middle line and get max and min */
-
- CvPoint2D64d projPoint;
- CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
- CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;
-
-
- double dist;
- double maxDist = 0;
- double minDist = 10000000;
-
-
- for( i = 0; i < numPoints; i++ )
- {
- icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
- icvGetPieceLength(epipole,projPoint,&dist);
- if( dist < minDist)
- {
- minDist = dist;
- minPoint = projPoint;
- }
-
- if( dist > maxDist)
- {
- maxDist = dist;
- maxPoint = projPoint;
- }
- }
-
- /* We know maximum and minimum points. Now we can compute cut lines */
-
- icvGetNormalDirect(midLine,minPoint,cutLine1);
- icvGetNormalDirect(midLine,maxPoint,cutLine2);
-
- /* Test for begin of line. */
- CvPoint2D64d tmpPoint2;
-
- /* Get cross with */
- icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
- icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);
-
- icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
- icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);
-
- if( epipole.x > imageSize.width * 0.5 )
- {/* Need to change points */
- tmpPoint2 = *point11;
- *point11 = *point21;
- *point21 = tmpPoint2;
-
- tmpPoint2 = *point12;
- *point12 = *point22;
- *point22 = tmpPoint2;
- }
-
- return;
-}
-/*---------------------------------------------------------------------------------------*/
-/* Get middle angle */
-void icvGetMiddleAnglePoint( CvPoint2D64d basePoint,
- CvPoint2D64d point1,CvPoint2D64d point2,
- CvPoint2D64d* midPoint)
-{/* !!! May be need to return error */
-
- double dist1;
- double dist2;
- icvGetPieceLength(basePoint,point1,&dist1);
- icvGetPieceLength(basePoint,point2,&dist2);
- CvPoint2D64d pointNew1;
- CvPoint2D64d pointNew2;
- double alpha = dist2/dist1;
-
- pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
- pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );
-
- pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
- pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );
-
- int res;
- icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-/* Get normal direct to direct in line */
-void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
-{
- normDirect[0] = direct[1];
- normDirect[1] = - direct[0];
- normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
-{
- return (point1.x - basePoint.x)*(point2.y - basePoint.y) -
- (point2.x - basePoint.x)*(point1.y - basePoint.y);
-}
-/*---------------------------------------------------------------------------------------*/
-/* Test for point in sector */
-/* Return 0 - point not inside sector */
-/* Return 1 - point inside sector */
-void icvTestPoint( CvPoint2D64d testPoint,
- CvVect64d line1,CvVect64d line2,
- CvPoint2D64d basePoint,
- int* result)
-{
- CvPoint2D64d point1,point2;
-
- icvProjectPointToDirect(testPoint,line1,&point1);
- icvProjectPointToDirect(testPoint,line2,&point2);
-
- double sign1 = icvGetVect(basePoint,point1,point2);
- double sign2 = icvGetVect(basePoint,point1,testPoint);
- if( sign1 * sign2 > 0 )
- {/* Correct for first line */
- sign1 = - sign1;
- sign2 = icvGetVect(basePoint,point2,testPoint);
- if( sign1 * sign2 > 0 )
- {/* Correct for both lines */
- *result = 1;
- }
- else
- {
- *result = 0;
- }
- }
- else
- {
- *result = 0;
- }
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-/* Project point to line */
-void icvProjectPointToDirect( CvPoint2D64d point,CvVect64d lineCoeff,
- CvPoint2D64d* projectPoint)
-{
- double a = lineCoeff[0];
- double b = lineCoeff[1];
-
- double det = 1.0 / ( a*a + b*b );
- double delta = a*point.y - b*point.x;
-
- projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
- projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;
-
- return;
-}
-
-/*---------------------------------------------------------------------------------------*/
-/* Get distance from point to direction */
-void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
-{
- CvPoint2D64d tmpPoint;
- icvProjectPointToDirect(point,lineCoef,&tmpPoint);
- double dx = point.x - tmpPoint.x;
- double dy = point.y - tmpPoint.y;
- *dist = sqrt(dx*dx+dy*dy);
- return;
-}
-/*---------------------------------------------------------------------------------------*/
-
-CV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
- int desired_depth, int desired_num_channels )
-{
- CvSize src_size ;
- src_size.width = src->width;
- src_size.height = src->height;
-
- CvSize dst_size = src_size;
-
- if( dst )
- {
- dst_size.width = dst->width;
- dst_size.height = dst->height;
- }
-
- if( !dst || dst->depth != desired_depth ||
- dst->nChannels != desired_num_channels ||
- dst_size.width != src_size.width ||
- dst_size.height != dst_size.height )
- {
- cvReleaseImage( &dst );
- dst = cvCreateImage( src_size, desired_depth, desired_num_channels );
- CvRect rect = cvRect(0,0,src_size.width,src_size.height);
- cvSetImageROI( dst, rect );
-
- }
-
- return dst;
-}
-
-int
-icvCvt_32f_64d( float *src, double *dst, int size )
-{
- int t;
-
- if( !src || !dst )
- return CV_NULLPTR_ERR;
- if( size <= 0 )
- return CV_BADRANGE_ERR;
-
- for( t = 0; t < size; t++ )
- {
- dst[t] = (double) (src[t]);
- }
-
- return CV_OK;
-}
-
-/*======================================================================================*/
-/* Type conversion double -> float */
-int
-icvCvt_64d_32f( double *src, float *dst, int size )
-{
- int t;
-
- if( !src || !dst )
- return CV_NULLPTR_ERR;
- if( size <= 0 )
- return CV_BADRANGE_ERR;
-
- for( t = 0; t < size; t++ )
- {
- dst[t] = (float) (src[t]);
- }
-
- return CV_OK;
-}
-
-/*----------------------------------------------------------------------------------*/
-
-
-/* Find line which cross frame by line(a,b,c) */
-void FindLineForEpiline( CvSize imageSize,
- float a,float b,float c,
- CvPoint2D32f *start,CvPoint2D32f *end,
- int* result)
-{
- result = result;
- CvPoint2D32f frameBeg;
-
- CvPoint2D32f frameEnd;
- CvPoint2D32f cross[4];
- int haveCross[4];
- float dist;
-
- haveCross[0] = 0;
- haveCross[1] = 0;
- haveCross[2] = 0;
- haveCross[3] = 0;
-
- frameBeg.x = 0;
- frameBeg.y = 0;
- frameEnd.x = (float)(imageSize.width);
- frameEnd.y = 0;
- haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
-
- frameBeg.x = (float)(imageSize.width);
- frameBeg.y = 0;
- frameEnd.x = (float)(imageSize.width);
- frameEnd.y = (float)(imageSize.height);
- haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
-
- frameBeg.x = (float)(imageSize.width);
- frameBeg.y = (float)(imageSize.height);
- frameEnd.x = 0;
- frameEnd.y = (float)(imageSize.height);
- haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
-
- frameBeg.x = 0;
- frameBeg.y = (float)(imageSize.height);
- frameEnd.x = 0;
- frameEnd.y = 0;
- haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
-
- int n;
- float minDist = (float)(INT_MAX);
- float maxDist = (float)(INT_MIN);
-
- int maxN = -1;
- int minN = -1;
-
- double midPointX = imageSize.width / 2.0;
- double midPointY = imageSize.height / 2.0;
-
- for( n = 0; n < 4; n++ )
- {
- if( haveCross[n] > 0 )
- {
- dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
- (midPointY - cross[n].y)*(midPointY - cross[n].y));
-
- if( dist < minDist )
- {
- minDist = dist;
- minN = n;
- }
-
- if( dist > maxDist )
- {
- maxDist = dist;
- maxN = n;
- }
- }
- }
-
- if( minN >= 0 && maxN >= 0 && (minN != maxN) )
- {
- *start = cross[minN];
- *end = cross[maxN];
- }
- else
- {
- start->x = 0;
- start->y = 0;
- end->x = 0;
- end->y = 0;
- }
-
- return;
-
-}
-
-
-/*----------------------------------------------------------------------------------*/
-
-int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
-{
- float width = (float)(imageSize.width);
- float height = (float)(imageSize.height);
-
- /* Get crosslines with image corners */
-
- /* Find four lines */
-
- CvPoint2D32f pa,pb,pc,pd;
-
- pa.x = 0;
- pa.y = 0;
-
- pb.x = width;
- pb.y = 0;
-
- pd.x = width;
- pd.y = height;
-
- pc.x = 0;
- pc.y = height;
-
- /* We can compute points for angle */
- /* Test for place section */
-
- float x,y;
- x = epipole.x;
- y = epipole.y;
-
- if( x < 0 )
- {/* 1,4,7 */
- if( y < 0)
- {/* 1 */
- point1 = pb;
- point2 = pc;
- }
- else if( y > height )
- {/* 7 */
- point1 = pa;
- point2 = pd;
- }
- else
- {/* 4 */
- point1 = pa;
- point2 = pc;
- }
- }
- else if ( x > width )
- {/* 3,6,9 */
- if( y < 0 )
- {/* 3 */
- point1 = pa;
- point2 = pd;
- }
- else if ( y > height )
- {/* 9 */
- point1 = pc;
- point2 = pb;
- }
- else
- {/* 6 */
- point1 = pb;
- point2 = pd;
- }
- }
- else
- {/* 2,5,8 */
- if( y < 0 )
- {/* 2 */
- point1 = pa;
- point2 = pb;
- }
- else if( y > height )
- {/* 8 */
- point1 = pc;
- point2 = pd;
- }
- else
- {/* 5 - point in the image */
- return 2;
- }
-
-
- }
-
-
- return 0;
-}
-
-/*--------------------------------------------------------------------------------------*/
-void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])
-{/* Computes perspective coeffs for transformation from src to dst quad */
-
-
- CV_FUNCNAME( "icvComputePerspectiveCoeffs" );
-
- __BEGIN__;
-
- double A[64];
- double b[8];
- double c[8];
- CvPoint2D32f pt[4];
- int i;
-
- pt[0] = srcQuad[0];
- pt[1] = srcQuad[1];
- pt[2] = srcQuad[2];
- pt[3] = srcQuad[3];
-
- for( i = 0; i < 4; i++ )
- {
-#if 0
- double x = dstQuad[i].x;
- double y = dstQuad[i].y;
- double X = pt[i].x;
- double Y = pt[i].y;
-#else
- double x = pt[i].x;
- double y = pt[i].y;
- double X = dstQuad[i].x;
- double Y = dstQuad[i].y;
-#endif
- double* a = A + i*16;
-
- a[0] = x;
- a[1] = y;
- a[2] = 1;
- a[3] = 0;
- a[4] = 0;
- a[5] = 0;
- a[6] = -X*x;
- a[7] = -X*y;
-
- a += 8;
-
- a[0] = 0;
- a[1] = 0;
- a[2] = 0;
- a[3] = x;
- a[4] = y;
- a[5] = 1;
- a[6] = -Y*x;
- a[7] = -Y*y;
-
- b[i*2] = X;
- b[i*2 + 1] = Y;
- }
-
- {
- double invA[64];
- CvMat matA = cvMat( 8, 8, CV_64F, A );
- CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
- CvMat matB = cvMat( 8, 1, CV_64F, b );
- CvMat matX = cvMat( 8, 1, CV_64F, c );
-
- CV_CALL( cvPseudoInverse( &matA, &matInvA ));
- CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
- }
-
- coeffs[0][0] = c[0];
- coeffs[0][1] = c[1];
- coeffs[0][2] = c[2];
- coeffs[1][0] = c[3];
- coeffs[1][1] = c[4];
- coeffs[1][2] = c[5];
- coeffs[2][0] = c[6];
- coeffs[2][1] = c[7];
- coeffs[2][2] = 1.0;
-
- __END__;
-
- return;
-}
-
-/*--------------------------------------------------------------------------------------*/
-
-CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY )
-{
- CV_FUNCNAME( "cvComputePerspectiveMap" );
-
- __BEGIN__;
-
- CvSize size;
- CvMat stubx, *mapx = (CvMat*)rectMapX;
- CvMat stuby, *mapy = (CvMat*)rectMapY;
- int i, j;
-
- CV_CALL( mapx = cvGetMat( mapx, &stubx ));
- CV_CALL( mapy = cvGetMat( mapy, &stuby ));
-
- if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- size = cvGetMatSize(mapx);
- assert( fabs(c[2][2] - 1.) < FLT_EPSILON );
-
- for( i = 0; i < size.height; i++ )
- {
- float* mx = (float*)(mapx->data.ptr + mapx->step*i);
- float* my = (float*)(mapy->data.ptr + mapy->step*i);
-
- for( j = 0; j < size.width; j++ )
- {
- double w = 1./(c[2][0]*j + c[2][1]*i + 1.);
- double x = (c[0][0]*j + c[0][1]*i + c[0][2])*w;
- double y = (c[1][0]*j + c[1][1]*i + c[1][2])*w;
-
- mx[j] = (float)x;
- my[j] = (float)y;
- }
- }
-
- __END__;
-}
-
-/*--------------------------------------------------------------------------------------*/
-
-CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
- CvArr* rectMap )
-{
- /* Computes Perspective Transform coeffs and map if need
- for given image size and given result quad */
- CV_FUNCNAME( "cvInitPerspectiveTransform" );
-
- __BEGIN__;
-
- double A[64];
- double b[8];
- double c[8];
- CvPoint2D32f pt[4];
- CvMat mapstub, *map = (CvMat*)rectMap;
- int i, j;
-
- if( map )
- {
- CV_CALL( map = cvGetMat( map, &mapstub ));
-
- if( CV_MAT_TYPE( map->type ) != CV_32FC2 )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- if( map->width != size.width || map->height != size.height )
- CV_ERROR( CV_StsUnmatchedSizes, "" );
- }
-
- pt[0] = cvPoint2D32f( 0, 0 );
- pt[1] = cvPoint2D32f( size.width, 0 );
- pt[2] = cvPoint2D32f( size.width, size.height );
- pt[3] = cvPoint2D32f( 0, size.height );
-
- for( i = 0; i < 4; i++ )
- {
-#if 0
- double x = quad[i].x;
- double y = quad[i].y;
- double X = pt[i].x;
- double Y = pt[i].y;
-#else
- double x = pt[i].x;
- double y = pt[i].y;
- double X = quad[i].x;
- double Y = quad[i].y;
-#endif
- double* a = A + i*16;
-
- a[0] = x;
- a[1] = y;
- a[2] = 1;
- a[3] = 0;
- a[4] = 0;
- a[5] = 0;
- a[6] = -X*x;
- a[7] = -X*y;
-
- a += 8;
-
- a[0] = 0;
- a[1] = 0;
- a[2] = 0;
- a[3] = x;
- a[4] = y;
- a[5] = 1;
- a[6] = -Y*x;
- a[7] = -Y*y;
-
- b[i*2] = X;
- b[i*2 + 1] = Y;
- }
-
- {
- double invA[64];
- CvMat matA = cvMat( 8, 8, CV_64F, A );
- CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
- CvMat matB = cvMat( 8, 1, CV_64F, b );
- CvMat matX = cvMat( 8, 1, CV_64F, c );
-
- CV_CALL( cvPseudoInverse( &matA, &matInvA ));
- CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
- }
-
- matrix[0][0] = c[0];
- matrix[0][1] = c[1];
- matrix[0][2] = c[2];
- matrix[1][0] = c[3];
- matrix[1][1] = c[4];
- matrix[1][2] = c[5];
- matrix[2][0] = c[6];
- matrix[2][1] = c[7];
- matrix[2][2] = 1.0;
-
- if( map )
- {
- for( i = 0; i < size.height; i++ )
- {
- CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i);
- for( j = 0; j < size.width; j++ )
- {
- double w = 1./(c[6]*j + c[7]*i + 1.);
- double x = (c[0]*j + c[1]*i + c[2])*w;
- double y = (c[3]*j + c[4]*i + c[5])*w;
-
- maprow[j].x = (float)x;
- maprow[j].y = (float)y;
- }
- }
- }
-
- __END__;
-
- return;
-}
-
-
-/*-----------------------------------------------------------------------*/
-/* Compute projected infinite point for second image if first image point is known */
-void icvComputeeInfiniteProject1( CvMatr64d rotMatr,
- CvMatr64d camMatr1,
- CvMatr64d camMatr2,
- CvPoint2D32f point1,
- CvPoint2D32f* point2)
-{
- double invMatr1[9];
- icvInvertMatrix_64d(camMatr1,3,invMatr1);
- double P1[3];
- double p1[3];
- p1[0] = (double)(point1.x);
- p1[1] = (double)(point1.y);
- p1[2] = 1;
-
- icvMulMatrix_64d( invMatr1,
- 3,3,
- p1,
- 1,3,
- P1);
-
- double invR[9];
- icvTransposeMatrix_64d( rotMatr, 3, 3, invR );
-
- /* Change system 1 to system 2 */
- double P2[3];
- icvMulMatrix_64d( invR,
- 3,3,
- P1,
- 1,3,
- P2);
-
- /* Now we can project this point to image 2 */
- double projP[3];
-
- icvMulMatrix_64d( camMatr2,
- 3,3,
- P2,
- 1,3,
- projP);
-
- point2->x = (float)(projP[0] / projP[2]);
- point2->y = (float)(projP[1] / projP[2]);
-
- return;
-}
-
-/*-----------------------------------------------------------------------*/
-/* Compute projected infinite point for second image if first image point is known */
-void icvComputeeInfiniteProject2( CvMatr64d rotMatr,
- CvMatr64d camMatr1,
- CvMatr64d camMatr2,
- CvPoint2D32f* point1,
- CvPoint2D32f point2)
-{
- double invMatr2[9];
- icvInvertMatrix_64d(camMatr2,3,invMatr2);
- double P2[3];
- double p2[3];
- p2[0] = (double)(point2.x);
- p2[1] = (double)(point2.y);
- p2[2] = 1;
-
- icvMulMatrix_64d( invMatr2,
- 3,3,
- p2,
- 1,3,
- P2);
-
- /* Change system 1 to system 2 */
-
- double P1[3];
- icvMulMatrix_64d( rotMatr,
- 3,3,
- P2,
- 1,3,
- P1);
-
- /* Now we can project this point to image 2 */
- double projP[3];
-
- icvMulMatrix_64d( camMatr1,
- 3,3,
- P1,
- 1,3,
- projP);
-
- point1->x = (float)(projP[0] / projP[2]);
- point1->y = (float)(projP[1] / projP[2]);
-
- return;
-}
-
-/* Select best R and t for given cameras, points, ... */
-/* For both cameras */
-int icvSelectBestRt( int numImages,
- int* numPoints,
- CvPoint2D32f* imagePoints1,
- CvPoint2D32f* imagePoints2,
- CvPoint3D32f* objectPoints,
-
- CvMatr32f cameraMatrix1,
- CvVect32f distortion1,
- CvMatr32f rotMatrs1,
- CvVect32f transVects1,
-
- CvMatr32f cameraMatrix2,
- CvVect32f distortion2,
- CvMatr32f rotMatrs2,
- CvVect32f transVects2,
-
- CvMatr32f bestRotMatr,
- CvVect32f bestTransVect
- )
-{
-
- /* Need to convert input data 32 -> 64 */
- CvPoint3D64d* objectPoints_64d;
-
- double* rotMatrs1_64d;
- double* rotMatrs2_64d;
-
- double* transVects1_64d;
- double* transVects2_64d;
-
- double cameraMatrix1_64d[9];
- double cameraMatrix2_64d[9];
-
- double distortion1_64d[4];
- double distortion2_64d[4];
-
- /* allocate memory for 64d data */
- int totalNum = 0;
-
- int i;
- for( i = 0; i < numImages; i++ )
- {
- totalNum += numPoints[i];
- }
-
- objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));
-
- rotMatrs1_64d = (double*)calloc(numImages,sizeof(double)*9);
- rotMatrs2_64d = (double*)calloc(numImages,sizeof(double)*9);
-
- transVects1_64d = (double*)calloc(numImages,sizeof(double)*3);
- transVects2_64d = (double*)calloc(numImages,sizeof(double)*3);
-
- /* Convert input data to 64d */
-
- icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d, totalNum*3);
-
- icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d, numImages*9);
- icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d, numImages*9);
-
- icvCvt_32f_64d(transVects1, transVects1_64d, numImages*3);
- icvCvt_32f_64d(transVects2, transVects2_64d, numImages*3);
-
- /* Convert to arrays */
- icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
- icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
-
- icvCvt_32f_64d(distortion1, distortion1_64d, 4);
- icvCvt_32f_64d(distortion2, distortion2_64d, 4);
-
-
- /* for each R and t compute error for image pair */
- float* errors;
-
- errors = (float*)calloc(numImages*numImages,sizeof(float));
- if( errors == 0 )
- {
- return CV_OUTOFMEM_ERR;
- }
-
- int currImagePair;
- int currRt;
- for( currRt = 0; currRt < numImages; currRt++ )
- {
- int begPoint = 0;
- for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
- {
- /* For current R,t R,t compute relative position of cameras */
-
- double convRotMatr[9];
- double convTransVect[3];
-
- icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
- transVects1_64d + currRt*3,
- rotMatrs2_64d + currRt*9,
- transVects2_64d + currRt*3,
- convRotMatr,
- convTransVect);
-
- /* Project points using relative position of cameras */
-
- double convRotMatr2[9];
- double convTransVect2[3];
-
- convRotMatr2[0] = 1;
- convRotMatr2[1] = 0;
- convRotMatr2[2] = 0;
-
- convRotMatr2[3] = 0;
- convRotMatr2[4] = 1;
- convRotMatr2[5] = 0;
-
- convRotMatr2[6] = 0;
- convRotMatr2[7] = 0;
- convRotMatr2[8] = 1;
-
- convTransVect2[0] = 0;
- convTransVect2[1] = 0;
- convTransVect2[2] = 0;
-
- /* Compute error for given pair and Rt */
- /* We must project points to image and compute error */
-
- CvPoint2D64d* projImagePoints1;
- CvPoint2D64d* projImagePoints2;
-
- CvPoint3D64d* points1;
- CvPoint3D64d* points2;
-
- int numberPnt;
- numberPnt = numPoints[currImagePair];
- projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
- projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
-
- points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
- points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
-
- /* Transform object points to first camera position */
- int i;
- for( i = 0; i < numberPnt; i++ )
- {
- /* Create second camera point */
- CvPoint3D64d tmpPoint;
- tmpPoint.x = (double)(objectPoints[i].x);
- tmpPoint.y = (double)(objectPoints[i].y);
- tmpPoint.z = (double)(objectPoints[i].z);
-
- icvConvertPointSystem( tmpPoint,
- points2+i,
- rotMatrs2_64d + currImagePair*9,
- transVects2_64d + currImagePair*3);
-
- /* Create first camera point using R, t */
- icvConvertPointSystem( points2[i],
- points1+i,
- convRotMatr,
- convTransVect);
-
- CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
- icvConvertPointSystem( tmpPoint,
- &tmpPoint2,
- rotMatrs1_64d + currImagePair*9,
- transVects1_64d + currImagePair*3);
- double err;
- double dx,dy,dz;
- dx = tmpPoint2.x - points1[i].x;
- dy = tmpPoint2.y - points1[i].y;
- dz = tmpPoint2.z - points1[i].z;
- err = sqrt(dx*dx + dy*dy + dz*dz);
-
-
- }
-
-#if 0
- cvProjectPointsSimple( numPoints[currImagePair],
- objectPoints_64d + begPoint,
- rotMatrs1_64d + currRt*9,
- transVects1_64d + currRt*3,
- cameraMatrix1_64d,
- distortion1_64d,
- projImagePoints1);
-
- cvProjectPointsSimple( numPoints[currImagePair],
- objectPoints_64d + begPoint,
- rotMatrs2_64d + currRt*9,
- transVects2_64d + currRt*3,
- cameraMatrix2_64d,
- distortion2_64d,
- projImagePoints2);
-#endif
-
- /* Project with no translate and no rotation */
-
-#if 0
- {
- double nodist[4] = {0,0,0,0};
- cvProjectPointsSimple( numPoints[currImagePair],
- points1,
- convRotMatr2,
- convTransVect2,
- cameraMatrix1_64d,
- nodist,
- projImagePoints1);
-
- cvProjectPointsSimple( numPoints[currImagePair],
- points2,
- convRotMatr2,
- convTransVect2,
- cameraMatrix2_64d,
- nodist,
- projImagePoints2);
-
- }
-#endif
-
- cvProjectPointsSimple( numPoints[currImagePair],
- points1,
- convRotMatr2,
- convTransVect2,
- cameraMatrix1_64d,
- distortion1_64d,
- projImagePoints1);
-
- cvProjectPointsSimple( numPoints[currImagePair],
- points2,
- convRotMatr2,
- convTransVect2,
- cameraMatrix2_64d,
- distortion2_64d,
- projImagePoints2);
-
- /* points are projected. Compute error */
-
- int currPoint;
- double err1 = 0;
- double err2 = 0;
- double err;
- for( currPoint = 0; currPoint < numberPnt; currPoint++ )
- {
- double len1,len2;
- double dx1,dy1;
- dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
- dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
- len1 = sqrt(dx1*dx1 + dy1*dy1);
- err1 += len1;
-
- double dx2,dy2;
- dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
- dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
- len2 = sqrt(dx2*dx2 + dy2*dy2);
- err2 += len2;
- }
-
- err1 /= (float)(numberPnt);
- err2 /= (float)(numberPnt);
-
- err = (err1+err2) * 0.5;
- begPoint += numberPnt;
-
- /* Set this error to */
- errors[numImages*currImagePair+currRt] = (float)err;
-
- free(points1);
- free(points2);
- free(projImagePoints1);
- free(projImagePoints2);
- }
- }
-
- /* Just select R and t with minimal average error */
-
- int bestnumRt = 0;
- float minError = 0;/* Just for no warnings. Uses 'first' flag. */
- int first = 1;
- for( currRt = 0; currRt < numImages; currRt++ )
- {
- float avErr = 0;
- for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
- {
- avErr += errors[numImages*currImagePair+currRt];
- }
- avErr /= (float)(numImages);
-
- if( first )
- {
- bestnumRt = 0;
- minError = avErr;
- first = 0;
- }
- else
- {
- if( avErr < minError )
- {
- bestnumRt = currRt;
- minError = avErr;
- }
- }
-
- }
-
- double bestRotMatr_64d[9];
- double bestTransVect_64d[3];
-
- icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
- transVects1_64d + bestnumRt * 3,
- rotMatrs2_64d + bestnumRt * 9,
- transVects2_64d + bestnumRt * 3,
- bestRotMatr_64d,
- bestTransVect_64d);
-
- icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
- icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);
-
-
- free(errors);
-
- return CV_OK;
-}
-
-
-/* ----------------- Stereo calibration functions --------------------- */
-
-float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
-{
- float ax = point2.x - point1.x;
- float ay = point2.y - point1.y;
-
- float bx = point.x - point1.x;
- float by = point.y - point1.y;
-
- return (ax*by - ay*bx);
-}
-
-/* Convert function for stereo warping */
-int icvConvertWarpCoordinates(double coeffs[3][3],
- CvPoint2D32f* cameraPoint,
- CvPoint2D32f* warpPoint,
- int direction)
-{
- double x,y;
- double det;
- if( direction == CV_WARP_TO_CAMERA )
- {/* convert from camera image to warped image coordinates */
- x = warpPoint->x;
- y = warpPoint->y;
-
- det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
- if( fabs(det) > 1e-8 )
- {
- cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
- cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
- return CV_OK;
- }
- }
- else if( direction == CV_CAMERA_TO_WARP )
- {/* convert from warped image to camera image coordinates */
- x = cameraPoint->x;
- y = cameraPoint->y;
-
- det = (coeffs[2][0]*x-coeffs[0][0])*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[2][0]*y-coeffs[1][0]);
-
- if( fabs(det) > 1e-8 )
- {
- warpPoint->x = (float)(((coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[1][2]-coeffs[2][2]*y))/det);
- warpPoint->y = (float)(((coeffs[2][0]*x-coeffs[0][0])*(coeffs[1][2]-coeffs[2][2]*y)-(coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][0]*y-coeffs[1][0]))/det);
- return CV_OK;
- }
- }
-
- return CV_BADFACTOR_ERR;
-}
-
-/* Compute stereo params using some camera params */
-/* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
-int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
-{
-
-
- icvGetQuadsTransformStruct(stereoparams);
-
- cvInitPerspectiveTransform( stereoparams->warpSize,
- stereoparams->quad[0],
- stereoparams->coeffs[0],
- 0);
-
- cvInitPerspectiveTransform( stereoparams->warpSize,
- stereoparams->quad[1],
- stereoparams->coeffs[1],
- 0);
-
- /* Create border for warped images */
- CvPoint2D32f corns[4];
- corns[0].x = 0;
- corns[0].y = 0;
-
- corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
- corns[1].y = 0;
-
- corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
- corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
-
- corns[3].x = 0;
- corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
-
- int i;
- for( i = 0; i < 4; i++ )
- {
- /* For first camera */
- icvConvertWarpCoordinates( stereoparams->coeffs[0],
- corns+i,
- stereoparams->border[0]+i,
- CV_CAMERA_TO_WARP);
-
- /* For second camera */
- icvConvertWarpCoordinates( stereoparams->coeffs[1],
- corns+i,
- stereoparams->border[1]+i,
- CV_CAMERA_TO_WARP);
- }
-
- /* Test compute */
- {
- CvPoint2D32f warpPoints[4];
- warpPoints[0] = cvPoint2D32f(0,0);
- warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0);
- warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1);
- warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1);
-
- CvPoint2D32f camPoints1[4];
- CvPoint2D32f camPoints2[4];
-
- for( int i = 0; i < 4; i++ )
- {
- icvConvertWarpCoordinates(stereoparams->coeffs[0],
- camPoints1+i,
- warpPoints+i,
- CV_WARP_TO_CAMERA);
-
- icvConvertWarpCoordinates(stereoparams->coeffs[1],
- camPoints2+i,
- warpPoints+i,
- CV_WARP_TO_CAMERA);
- }
- }
-
-
- /* Allocate memory for scanlines coeffs */
-
- stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));
-
- /* Compute coeffs for epilines */
-
- icvComputeCoeffForStereo( stereoparams);
-
- /* all coeffs are known */
- return CV_OK;
-}
-
-/*-------------------------------------------------------------------------------------------*/
-
-int icvStereoCalibration( int numImages,
- int* nums,
- CvSize imageSize,
- CvPoint2D32f* imagePoints1,
- CvPoint2D32f* imagePoints2,
- CvPoint3D32f* objectPoints,
- CvStereoCamera* stereoparams
- )
-{
- /* Firstly we must calibrate both cameras */
- /* Alocate memory for data */
- /* Allocate for translate vectors */
- float* transVects1;
- float* transVects2;
- float* rotMatrs1;
- float* rotMatrs2;
-
- transVects1 = (float*)calloc(numImages,sizeof(float)*3);
- transVects2 = (float*)calloc(numImages,sizeof(float)*3);
-
- rotMatrs1 = (float*)calloc(numImages,sizeof(float)*9);
- rotMatrs2 = (float*)calloc(numImages,sizeof(float)*9);
-
- /* Calibrate first camera */
- cvCalibrateCamera( numImages,
- nums,
- imageSize,
- imagePoints1,
- objectPoints,
- stereoparams->camera[0]->distortion,
- stereoparams->camera[0]->matrix,
- transVects1,
- rotMatrs1,
- 1);
-
- /* Calibrate second camera */
- cvCalibrateCamera( numImages,
- nums,
- imageSize,
- imagePoints2,
- objectPoints,
- stereoparams->camera[1]->distortion,
- stereoparams->camera[1]->matrix,
- transVects2,
- rotMatrs2,
- 1);
-
- /* Cameras are calibrated */
-
- stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
- stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
-
- stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
- stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
-
- icvSelectBestRt( numImages,
- nums,
- imagePoints1,
- imagePoints2,
- objectPoints,
- stereoparams->camera[0]->matrix,
- stereoparams->camera[0]->distortion,
- rotMatrs1,
- transVects1,
- stereoparams->camera[1]->matrix,
- stereoparams->camera[1]->distortion,
- rotMatrs2,
- transVects2,
- stereoparams->rotMatrix,
- stereoparams->transVector
- );
-
- /* Free memory */
- free(transVects1);
- free(transVects2);
- free(rotMatrs1);
- free(rotMatrs2);
-
- icvComputeRestStereoParams(stereoparams);
-
- return CV_NO_ERR;
-}
-
-/* Find line from epipole */
-void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
-{
- CvPoint2D32f frameBeg;
- CvPoint2D32f frameEnd;
- CvPoint2D32f cross[4];
- int haveCross[4];
- float dist;
-
- haveCross[0] = 0;
- haveCross[1] = 0;
- haveCross[2] = 0;
- haveCross[3] = 0;
-
- frameBeg.x = 0;
- frameBeg.y = 0;
- frameEnd.x = (float)(imageSize.width);
- frameEnd.y = 0;
- haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);
-
- frameBeg.x = (float)(imageSize.width);
- frameBeg.y = 0;
- frameEnd.x = (float)(imageSize.width);
- frameEnd.y = (float)(imageSize.height);
- haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);
-
- frameBeg.x = (float)(imageSize.width);
- frameBeg.y = (float)(imageSize.height);
- frameEnd.x = 0;
- frameEnd.y = (float)(imageSize.height);
- haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);
-
- frameBeg.x = 0;
- frameBeg.y = (float)(imageSize.height);
- frameEnd.x = 0;
- frameEnd.y = 0;
- haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);
-
- int n;
- float minDist = (float)(INT_MAX);
- float maxDist = (float)(INT_MIN);
-
- int maxN = -1;
- int minN = -1;
-
- for( n = 0; n < 4; n++ )
- {
- if( haveCross[n] > 0 )
- {
- dist = (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
- (epipole.y - cross[n].y)*(epipole.y - cross[n].y);
-
- if( dist < minDist )
- {
- minDist = dist;
- minN = n;
- }
-
- if( dist > maxDist )
- {
- maxDist = dist;
- maxN = n;
- }
- }
- }
-
- if( minN >= 0 && maxN >= 0 && (minN != maxN) )
- {
- *start = cross[minN];
- *end = cross[maxN];
- }
- else
- {
- start->x = 0;
- start->y = 0;
- end->x = 0;
- end->y = 0;
- }
-
- return;
-}
-
-
-/* Find line which cross frame by line(a,b,c) */
-void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
-{
- CvPoint2D32f frameBeg;
- CvPoint2D32f frameEnd;
- CvPoint2D32f cross[4];
- int haveCross[4];
- float dist;
-
- haveCross[0] = 0;
- haveCross[1] = 0;
- haveCross[2] = 0;
- haveCross[3] = 0;
-
- frameBeg.x = 0;
- frameBeg.y = 0;
- frameEnd.x = (float)(imageSize.width);
- frameEnd.y = 0;
- haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
-
- frameBeg.x = (float)(imageSize.width);
- frameBeg.y = 0;
- frameEnd.x = (float)(imageSize.width);
- frameEnd.y = (float)(imageSize.height);
- haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
-
- frameBeg.x = (float)(imageSize.width);
- frameBeg.y = (float)(imageSize.height);
- frameEnd.x = 0;
- frameEnd.y = (float)(imageSize.height);
- haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
-
- frameBeg.x = 0;
- frameBeg.y = (float)(imageSize.height);
- frameEnd.x = 0;
- frameEnd.y = 0;
- haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
-
- int n;
- float minDist = (float)(INT_MAX);
- float maxDist = (float)(INT_MIN);
-
- int maxN = -1;
- int minN = -1;
-
- double midPointX = imageSize.width / 2.0;
- double midPointY = imageSize.height / 2.0;
-
- for( n = 0; n < 4; n++ )
- {
- if( haveCross[n] > 0 )
- {
- dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
- (midPointY - cross[n].y)*(midPointY - cross[n].y));
-
- if( dist < minDist )
- {
- minDist = dist;
- minN = n;
- }
-
- if( dist > maxDist )
- {
- maxDist = dist;
- maxN = n;
- }
- }
- }
-
- if( minN >= 0 && maxN >= 0 && (minN != maxN) )
- {
- *start = cross[minN];
- *end = cross[maxN];
- }
- else
- {
- start->x = 0;
- start->y = 0;
- end->x = 0;
- end->y = 0;
- }
-
- return;
-
-}
-
-/* Cross lines */
-int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
-{
- double ex1,ey1,ex2,ey2;
- double px1,py1,px2,py2;
- double del;
- double delA,delB,delX,delY;
- double alpha,betta;
-
- ex1 = p1_start.x;
- ey1 = p1_start.y;
- ex2 = p1_end.x;
- ey2 = p1_end.y;
-
- px1 = p2_start.x;
- py1 = p2_start.y;
- px2 = p2_end.x;
- py2 = p2_end.y;
-
- del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
- if( del == 0)
- {
- return -1;
- }
-
- delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
- delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
-
- alpha = delA / del;
- betta = -delB / del;
-
- if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
- {
- return -1;
- }
-
- delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
- (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
-
- delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
- (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
-
- cross->x = (float)( delX / del);
- cross->y = (float)(-delY / del);
- return 1;
-}
-
-
-int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
-{
- double ex1,ey1,ex2,ey2;
- double px1,py1,px2,py2;
- double del;
- double delA,delB,delX,delY;
- double alpha,betta;
-
- ex1 = p1_start.x;
- ey1 = p1_start.y;
- ex2 = p1_end.x;
- ey2 = p1_end.y;
-
- px1 = v2_start.x;
- py1 = v2_start.y;
- px2 = v2_end.x;
- py2 = v2_end.y;
-
- del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
- if( del == 0)
- {
- return -1;
- }
-
- delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
- delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
-
- alpha = delA / del;
- betta = -delB / del;
-
- if( alpha < 0 || alpha > 1.0 )
- {
- return -1;
- }
-
- delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
- (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
-
- delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
- (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
-
- cross->x = (float)( delX / del);
- cross->y = (float)(-delY / del);
- return 1;
-}
-
-
-int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
-{
- double del;
- double delX,delY,delA;
-
- double px1,px2,py1,py2;
- double X,Y,alpha;
-
- px1 = p1.x;
- py1 = p1.y;
-
- px2 = p2.x;
- py2 = p2.y;
-
- del = a * (px2 - px1) + b * (py2-py1);
- if( del == 0 )
- {
- return -1;
- }
-
- delA = - c - a*px1 - b*py1;
- alpha = delA / del;
-
- if( alpha < 0 || alpha > 1.0 )
- {
- return -1;/* no cross */
- }
-
- delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
- delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);
-
- X = delX / del;
- Y = delY / del;
-
- cross->x = (float)X;
- cross->y = (float)Y;
-
- return 1;
-}
-
-int cvComputeEpipoles( CvMatr32f camMatr1, CvMatr32f camMatr2,
- CvMatr32f rotMatr1, CvMatr32f rotMatr2,
- CvVect32f transVect1,CvVect32f transVect2,
- CvVect32f epipole1,
- CvVect32f epipole2)
-{
-
- /* Copy matrix */
-
- CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1);
- CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2);
- CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1);
- CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2);
- CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1);
- CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2);
- CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1);
- CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2);
-
-
- CvMat cmatrP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1);
- CvMat cmatrP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2);
- CvMat cvectp1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1);
- CvMat cvectp2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2);
- CvMat ctmpF1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1);
- CvMat ctmpM1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1);
- CvMat ctmpM2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2);
- CvMat cinvP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1);
- CvMat cinvP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2);
- CvMat ctmpMatr = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr);
- CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1);
- CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2);
- CvMat cmatrF1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1);
- CvMat ctmpF = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF);
- CvMat ctmpE1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1);
- CvMat ctmpE2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2);
-
- /* Compute first */
- cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
- cvmInvert( &cmatrP1,&cinvP1 );
- cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );
-
- /* Compute second */
- cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
- cvmInvert( &cmatrP2,&cinvP2 );
- cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );
-
- cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
- cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
- cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);
-
- cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
- cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
- cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);
-
-
- /* Need scale */
-
- cvmScale(&ctmpE1,&cepipole1,1.0);
- cvmScale(&ctmpE2,&cepipole2,1.0);
-
- cvmFree(&cmatrP1);
- cvmFree(&cmatrP1);
- cvmFree(&cvectp1);
- cvmFree(&cvectp2);
- cvmFree(&ctmpF1);
- cvmFree(&ctmpM1);
- cvmFree(&ctmpM2);
- cvmFree(&cinvP1);
- cvmFree(&cinvP2);
- cvmFree(&ctmpMatr);
- cvmFree(&ctmpVect1);
- cvmFree(&ctmpVect2);
- cvmFree(&cmatrF1);
- cvmFree(&ctmpF);
- cvmFree(&ctmpE1);
- cvmFree(&ctmpE2);
-
- return CV_NO_ERR;
-}/* cvComputeEpipoles */
-
-
-/* Compute epipoles for fundamental matrix */
-int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
- CvPoint3D32f* epipole1,
- CvPoint3D32f* epipole2)
-{
- /* Decompose fundamental matrix using SVD ( A = U W V') */
- CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
-
- CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
- CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
- CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);
-
- /* From svd we need just last vector of U and V or last row from U' and V' */
- /* We get transposed matrixes U and V */
- cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T);
-
- /* Get last row from U' and compute epipole1 */
- epipole1->x = matrU->data.fl[6];
- epipole1->y = matrU->data.fl[7];
- epipole1->z = matrU->data.fl[8];
-
- /* Get last row from V' and compute epipole2 */
- epipole2->x = matrV->data.fl[6];
- epipole2->y = matrV->data.fl[7];
- epipole2->z = matrV->data.fl[8];
-
- cvReleaseMat(&matrW);
- cvReleaseMat(&matrU);
- cvReleaseMat(&matrV);
- return CV_OK;
-}
-
-int cvConvertEssential2Fundamental( CvMatr32f essMatr,
- CvMatr32f fundMatr,
- CvMatr32f cameraMatr1,
- CvMatr32f cameraMatr2)
-{/* Fund = inv(CM1') * Ess * inv(CM2) */
-
- CvMat essMatrC = cvMat(3,3,CV_MAT32F,essMatr);
- CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
- CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1);
- CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2);
-
- CvMat* invCM2 = cvCreateMat(3,3,CV_MAT32F);
- CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
- CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);
-
- cvTranspose(&cameraMatr1C,tmpMatr);
- cvInvert(tmpMatr,invCM1T);
- cvmMul(invCM1T,&essMatrC,tmpMatr);
- cvInvert(&cameraMatr2C,invCM2);
- cvmMul(tmpMatr,invCM2,&fundMatrC);
-
- /* Scale fundamental matrix */
- double scale;
- scale = 1.0/fundMatrC.data.fl[8];
- cvConvertScale(&fundMatrC,&fundMatrC,scale);
-
- cvReleaseMat(&invCM2);
- cvReleaseMat(&tmpMatr);
- cvReleaseMat(&invCM1T);
-
- return CV_OK;
-}
-
-/* Compute essential matrix */
-
-int cvComputeEssentialMatrix( CvMatr32f rotMatr,
- CvMatr32f transVect,
- CvMatr32f essMatr)
-{
- float transMatr[9];
-
- /* Make antisymmetric matrix from transpose vector */
- transMatr[0] = 0;
- transMatr[1] = - transVect[2];
- transMatr[2] = transVect[1];
-
- transMatr[3] = transVect[2];
- transMatr[4] = 0;
- transMatr[5] = - transVect[0];
-
- transMatr[6] = - transVect[1];
- transMatr[7] = transVect[0];
- transMatr[8] = 0;
-
- icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);
-
- return CV_OK;
-}
-
-
projects / opencv / blobdiff