/*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 "precomp.hpp" #include #include /* 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; } /*--------------------------------------------------------------------------------------*/ static 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; } /*--------------------------------------------------------------------------------------*/ static 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 */ CvPoint3D64f direct1; CvPoint3D64f direct2; CvPoint3D64f camPoint1; CvPoint3D64f directS3; CvPoint3D64f directS4; CvPoint3D64f direct3; CvPoint3D64f direct4; CvPoint3D64f camPoint2; icvGetDirectionForPoint( point1, camMatr1, &direct1); icvGetDirectionForPoint( point2, camMatr1, &direct2); /* Direction for camera 2 */ icvGetDirectionForPoint( point3, camMatr2, &directS3); icvGetDirectionForPoint( point4, camMatr2, &directS4); /* Create convertion for camera 2: two direction and camera point */ double convRotMatr[9]; double convTransVect[3]; icvCreateConvertMatrVect( rotMatr1, transVect1, rotMatr2, transVect2, convRotMatr, convTransVect); CvPoint3D64f zeroVect; zeroVect.x = zeroVect.y = zeroVect.z = 0.0; camPoint1.x = camPoint1.y = camPoint1.z = 0.0; icvConvertPointSystem(directS3,&direct3,convRotMatr,convTransVect); icvConvertPointSystem(directS4,&direct4,convRotMatr,convTransVect); icvConvertPointSystem(zeroVect,&camPoint2,convRotMatr,convTransVect); CvPoint3D64f pointB; int postype = 0; /* Changed order */ /* Compute point B: xB,yB,zB */ icvGetCrossLines(camPoint1,direct2, camPoint2,direct3, &pointB); if( pointB.z < 0 )/* If negative use other lines for cross */ { postype = 1; icvGetCrossLines(camPoint1,direct1, camPoint2,direct4, &pointB); } CvPoint3D64d pointNewA; CvPoint3D64d pointNewC; pointNewA.x = pointNewA.y = pointNewA.z = 0; pointNewC.x = pointNewC.y = pointNewC.z = 0; if( postype == 0 ) { icvGetSymPoint3D( camPoint1, direct1, pointB, &pointNewA); icvGetSymPoint3D( camPoint2, direct4, pointB, &pointNewC); } else {/* In this case we must change cameras */ *needSwapCamera = 1; icvGetSymPoint3D( camPoint2, direct3, pointB, &pointNewA); icvGetSymPoint3D( camPoint1, direct2, pointB, &pointNewC); } double gamma; double xA,yA,zA; double xB,yB,zB; double xC,yC,zC; xA = pointNewA.x; yA = pointNewA.y; zA = pointNewA.z; xB = pointB.x; yB = pointB.y; zB = pointB.z; 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, pointB, 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 */ static 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; } /*---------------------------------------------------------------------------------------*/ static 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]={0,0,0}; /* 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; } static 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 */ static 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; } /*----------------------------------------------------------------------------------*/ #if 0 /* Find line which cross frame by line(a,b,c) */ static void FindLineForEpiline( CvSize imageSize, float a,float b,float c, CvPoint2D32f *start,CvPoint2D32f *end, int*) { 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; } /*----------------------------------------------------------------------------------*/ static 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; } /*--------------------------------------------------------------------------------------*/ static 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; } #endif /*--------------------------------------------------------------------------------------*/ 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 */ static 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; for(int 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 */ for(int 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); for(int 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; } #if 0 /* Find line from epipole */ static 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) */ static 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 */ static 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; } #endif int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross) { double ex1 = p1_start.x; double ey1 = p1_start.y; double ex2 = p1_end.x; double ey2 = p1_end.y; double px1 = v2_start.x; double py1 = v2_start.y; double px2 = v2_end.x; double py2 = v2_end.y; double del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1); if( del == 0) { return -1; } double delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2); //double delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2); double alpha = delA / del; //double betta = -delB / del; if( alpha < 0 || alpha > 1.0 ) { return -1; } double delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+ (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2)); double 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; } #if 0 static 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 */ #endif /* 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; }