1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
55 int cvComputeEssentialMatrix( CvMatr32f rotMatr,
59 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
61 CvMatr32f cameraMatr1,
62 CvMatr32f cameraMatr2);
64 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
65 CvPoint3D32f* epipole1,
66 CvPoint3D32f* epipole2);
68 void icvTestPoint( CvPoint2D64d testPoint,
69 CvVect64d line1,CvVect64d line2,
70 CvPoint2D64d basePoint,
75 int icvGetSymPoint3D( CvPoint3D64d pointCorner,
78 CvPoint3D64d *pointSym2)
82 icvGetPieceLength3D(pointCorner,point1,&len1);
87 icvGetPieceLength3D(pointCorner,point2,&len2);
90 pointSym2->x = pointCorner.x + alpha*(point1.x - pointCorner.x);
91 pointSym2->y = pointCorner.y + alpha*(point1.y - pointCorner.y);
92 pointSym2->z = pointCorner.z + alpha*(point1.z - pointCorner.z);
96 /* author Valery Mosyagin */
98 /* Compute 3D point for scanline and alpha betta */
99 int icvCompute3DPoint( double alpha,double betta,
100 CvStereoLineCoeff* coeffs,
110 double alphabetta = alpha*betta;
112 partAll = alpha - betta;
113 if( fabs(partAll) > 0.00001 ) /* alpha must be > betta */
116 partX = coeffs->Xcoef + coeffs->XcoefA *alpha +
117 coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta;
119 partY = coeffs->Ycoef + coeffs->YcoefA *alpha +
120 coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta;
122 partZ = coeffs->Zcoef + coeffs->ZcoefA *alpha +
123 coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta;
125 invPartAll = 1.0 / partAll;
127 point->x = partX * invPartAll;
128 point->y = partY * invPartAll;
129 point->z = partZ * invPartAll;
134 return CV_BADFACTOR_ERR;
138 /*--------------------------------------------------------------------------------------*/
140 /* Compute rotate matrix and trans vector for change system */
141 int icvCreateConvertMatrVect( CvMatr64d rotMatr1,
142 CvMatr64d transVect1,
144 CvMatr64d transVect2,
145 CvMatr64d convRotMatr,
146 CvMatr64d convTransVect)
148 double invRotMatr2[9];
152 icvInvertMatrix_64d(rotMatr2,3,invRotMatr2);
155 icvMulMatrix_64d( rotMatr1,
161 icvMulMatrix_64d( convRotMatr,
167 icvSubVector_64d(transVect1,tmpVect,convTransVect,3);
173 /*--------------------------------------------------------------------------------------*/
175 /* Compute point coordinates in other system */
176 int icvConvertPointSystem(CvPoint3D64d M2,
184 icvMulMatrix_64d( rotMatr,
190 icvAddVector_64d(tmpVect,transVect,(double*)M1,3);
194 /*--------------------------------------------------------------------------------------*/
195 int icvComputeCoeffForStereoV3( double quad1[4][2],
200 CvMatr64d transVect1,
203 CvMatr64d transVect2,
204 CvStereoLineCoeff* startCoeffs,
208 /* In this function we must define position of cameras */
217 for( currLine = 0; currLine < numScanlines; currLine++ )
220 double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */
222 point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
223 point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];
225 point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
226 point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];
228 point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
229 point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];
231 point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
232 point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];
234 /* We can compute coeffs for this line */
235 icvComCoeffForLine( point1,
245 &startCoeffs[currLine],
250 /*--------------------------------------------------------------------------------------*/
251 int icvComputeCoeffForStereoNew( double quad1[4][2],
256 CvMatr32f transVect1,
258 CvStereoLineCoeff* startCoeffs,
263 double camMatr1_64d[9];
264 double camMatr2_64d[9];
266 double rotMatr1_64d[9];
267 double transVect1_64d[3];
269 double rotMatr2_64d[9];
270 double transVect2_64d[3];
272 icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
273 icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
275 icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
276 icvCvt_32f_64d(transVect1,transVect1_64d,3);
288 transVect2_64d[0] = 0;
289 transVect2_64d[1] = 0;
290 transVect2_64d[2] = 0;
292 int status = icvComputeCoeffForStereoV3( quad1,
308 /*--------------------------------------------------------------------------------------*/
309 int icvComputeCoeffForStereo( CvStereoCamera* stereoCamera)
315 for( i = 0; i < 4; i++ )
317 quad1[i][0] = stereoCamera->quad[0][i].x;
318 quad1[i][1] = stereoCamera->quad[0][i].y;
320 quad2[i][0] = stereoCamera->quad[1][i].x;
321 quad2[i][1] = stereoCamera->quad[1][i].y;
324 icvComputeCoeffForStereoNew( quad1,
326 stereoCamera->warpSize.height,
327 stereoCamera->camera[0]->matrix,
328 stereoCamera->rotMatrix,
329 stereoCamera->transVector,
330 stereoCamera->camera[1]->matrix,
331 stereoCamera->lineCoeffs,
332 &(stereoCamera->needSwapCameras));
338 /*--------------------------------------------------------------------------------------*/
339 int icvComCoeffForLine( CvPoint2D64d point1,
345 CvMatr64d transVect1,
348 CvMatr64d transVect2,
349 CvStereoLineCoeff* coeffs,
352 /* Get direction for all points */
353 /* Direction for camera 1 */
365 icvGetDirectionForPoint( point1,
367 (CvPoint3D64d*)direct1);
369 icvGetDirectionForPoint( point2,
371 (CvPoint3D64d*)direct2);
373 /* Direction for camera 2 */
375 icvGetDirectionForPoint( point3,
377 (CvPoint3D64d*)directS3);
379 icvGetDirectionForPoint( point4,
381 (CvPoint3D64d*)directS4);
383 /* Create convertion for camera 2: two direction and camera point */
385 double convRotMatr[9];
386 double convTransVect[3];
388 icvCreateConvertMatrVect( rotMatr1,
396 zeroVect[0] = zeroVect[1] = zeroVect[2] = 0.0;
397 camPoint1[0] = camPoint1[1] = camPoint1[2] = 0.0;
399 icvConvertPointSystem(*((CvPoint3D64d*)directS3),(CvPoint3D64d*)direct3,convRotMatr,convTransVect);
400 icvConvertPointSystem(*((CvPoint3D64d*)directS4),(CvPoint3D64d*)direct4,convRotMatr,convTransVect);
401 icvConvertPointSystem(*((CvPoint3D64d*)zeroVect),(CvPoint3D64d*)camPoint2,convRotMatr,convTransVect);
408 /* Compute point B: xB,yB,zB */
409 icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct2),
410 *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct3),
411 (CvPoint3D64d*)pointB);
413 if( pointB[2] < 0 )/* If negative use other lines for cross */
416 icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct1),
417 *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct4),
418 (CvPoint3D64d*)pointB);
421 CvPoint3D64d pointNewA;
422 CvPoint3D64d pointNewC;
424 pointNewA.x = pointNewA.y = pointNewA.z = 0;
425 pointNewC.x = pointNewC.y = pointNewC.z = 0;
429 icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1),
430 *((CvPoint3D64d*)direct1),
431 *((CvPoint3D64d*)pointB),
434 icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2),
435 *((CvPoint3D64d*)direct4),
436 *((CvPoint3D64d*)pointB),
440 {/* In this case we must change cameras */
442 icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2),
443 *((CvPoint3D64d*)direct3),
444 *((CvPoint3D64d*)pointB),
447 icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1),
448 *((CvPoint3D64d*)direct2),
449 *((CvPoint3D64d*)pointB),
479 len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) );
480 len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) );
483 icvComputeStereoLineCoeffs( pointNewA,
484 *((CvPoint3D64d*)pointB),
485 *((CvPoint3D64d*)camPoint1),
493 /*--------------------------------------------------------------------------------------*/
495 int icvGetDirectionForPoint( CvPoint2D64d point,
497 CvPoint3D64d* direct)
504 icvInvertMatrix_64d(camMatr,3,invMatr);
505 /* TEST FOR ERRORS */
513 icvMulMatrix_64d( invMatr,
522 /*--------------------------------------------------------------------------------------*/
524 int icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,
525 CvPoint3D64d point21,CvPoint3D64d point22,
526 CvPoint3D64d* midPoint)
553 double a11,a12,a21,a22;
556 a11 = (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA);
557 a12 = -(xD-xC)*(xB-xA)-(yD-yC)*(yB-yA)-(zD-zC)*(zB-zA);
558 a21 = (xB-xA)*(xD-xC)+(yB-yA)*(yD-yC)+(zB-zA)*(zD-zC);
559 a22 = -(xD-xC)*(xD-xC)-(yD-yC)*(yD-yC)-(zD-zC)*(zD-zC);
560 b1 = -( (xA-xC)*(xB-xA)+(yA-yC)*(yB-yA)+(zA-zC)*(zB-zA) );
561 b2 = -( (xA-xC)*(xD-xC)+(yA-yC)*(yD-yC)+(zA-zC)*(zD-zC) );
564 double deltaA,deltaB;
567 delta = a11*a22-a12*a21;
569 if( fabs(delta) < EPS64D )
574 deltaA = b1*a22-b2*a12;
575 deltaB = a11*b2-b1*a21;
577 alpha = deltaA / delta;
578 betta = deltaB / delta;
580 xM = xA+alpha*(xB-xA);
581 yM = yA+alpha*(yB-yA);
582 zM = zA+alpha*(zB-zA);
584 xN = xC+betta*(xD-xC);
585 yN = yC+betta*(yD-yC);
586 zN = zC+betta*(zD-zC);
588 /* Compute middle point */
589 midPoint->x = (xM + xN) * 0.5;
590 midPoint->y = (yM + yN) * 0.5;
591 midPoint->z = (zM + zN) * 0.5;
596 /*--------------------------------------------------------------------------------------*/
598 int icvComputeStereoLineCoeffs( CvPoint3D64d pointA,
600 CvPoint3D64d pointCam1,
602 CvStereoLineCoeff* coeffs)
623 coeffs->Xcoef = -x1 + xA;
624 coeffs->XcoefA = xB + x1 - xA;
625 coeffs->XcoefB = -xA - gamma * x1 + gamma * xA;
626 coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;
628 coeffs->Ycoef = -y1 + yA;
629 coeffs->YcoefA = yB + y1 - yA;
630 coeffs->YcoefB = -yA - gamma * y1 + gamma * yA;
631 coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;
633 coeffs->Zcoef = -z1 + zA;
634 coeffs->ZcoefA = zB + z1 - zA;
635 coeffs->ZcoefB = -zA - gamma * z1 + gamma * zA;
636 coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
641 coeffs->Xcoef = -( -x1 + xA);
642 coeffs->XcoefB = -( xB + x1 - xA);
643 coeffs->XcoefA = -( -xA - gamma * x1 + gamma * xA);
644 coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA);
646 coeffs->Ycoef = -( -y1 + yA);
647 coeffs->YcoefB = -( yB + y1 - yA);
648 coeffs->YcoefA = -( -yA - gamma * y1 + gamma * yA);
649 coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA);
651 coeffs->Zcoef = -( -z1 + zA);
652 coeffs->ZcoefB = -( zB + z1 - zA);
653 coeffs->ZcoefA = -( -zA - gamma * z1 + gamma * zA);
654 coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA);
661 /*--------------------------------------------------------------------------------------*/
664 /*---------------------------------------------------------------------------------------*/
666 /* This function get minimum angle started at point which contains rect */
667 int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2)
669 /* Get crosslines with image corners */
671 /* Find four lines */
673 CvPoint2D64d pa,pb,pc,pd;
678 pb.x = imageSize.width-1;
681 pd.x = imageSize.width-1;
682 pd.y = imageSize.height-1;
685 pc.y = imageSize.height-1;
687 /* We can compute points for angle */
688 /* Test for place section */
690 if( startPoint.x < 0 )
692 if( startPoint.y < 0)
697 else if( startPoint.y > imageSize.height-1 )
708 else if ( startPoint.x > imageSize.width-1 )
710 if( startPoint.y < 0 )
715 else if ( startPoint.y > imageSize.height-1 )
728 if( startPoint.y < 0 )
730 if( startPoint.x < imageSize.width/2 )
741 else if( startPoint.y > imageSize.height-1 )
743 if( startPoint.x < imageSize.width/2 )
755 {/* 5 - point in the image */
762 /*---------------------------------------------------------------------------------------*/
764 void icvGetCoefForPiece( CvPoint2D64d p_start,CvPoint2D64d p_end,
765 double *a,double *b,double *c,
769 double detA,detB,detC;
771 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;
772 if( fabs(det) < EPS64D)/* Error */
778 detA = p_start.y - p_end.y;
779 detB = p_end.x - p_start.x;
780 detC = p_start.x*p_end.y - p_end.x*p_start.y;
782 double invDet = 1.0 / det;
791 /*---------------------------------------------------------------------------------------*/
793 /* Get common area of rectifying */
794 void icvGetCommonArea( CvSize imageSize,
795 CvPoint3D64d epipole1,CvPoint3D64d epipole2,
797 CvVect64d coeff11,CvVect64d coeff12,
798 CvVect64d coeff21,CvVect64d coeff22,
802 CvPoint2D64d point11;
803 CvPoint2D64d point12;
804 CvPoint2D64d point21;
805 CvPoint2D64d point22;
817 double transFundMatr[3*3];
818 /* Compute transpose of fundamental matrix */
819 icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );
821 CvPoint2D64d epipole1_2d;
822 CvPoint2D64d epipole2_2d;
824 if( fabs(epipole1.z) < 1e-8 )
825 {/* epipole1 in infinity */
829 epipole1_2d.x = epipole1.x / epipole1.z;
830 epipole1_2d.y = epipole1.y / epipole1.z;
832 if( fabs(epipole2.z) < 1e-8 )
833 {/* epipole2 in infinity */
837 epipole2_2d.x = epipole2.x / epipole2.z;
838 epipole2_2d.y = epipole2.y / epipole2.z;
840 int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12);
848 stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22);
856 /* ============= Computation for line 1 ================ */
857 /* Find correspondence line for angle points11 */
858 /* corr21 = Fund'*p1 */
860 pointW11[0] = point11.x;
861 pointW11[1] = point11.y;
864 icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
869 /* Find crossing of line with image 2 */
872 icvGetCrossRectDirect( imageSize,
873 corr21[0],corr21[1],corr21[2],
878 {/* We have not cross */
879 /* We must define new angle */
881 pointW21[0] = point21.x;
882 pointW21[1] = point21.y;
885 /* Find correspondence line for this angle points */
886 /* We know point and try to get corr line */
888 /* corr11 = Fund * p21 */
890 icvTransformVector_64d( fundMatr, /* !!! Modified */
895 /* We have cross. And it's result cross for up line. Set result coefs */
897 /* Set coefs for line 1 image 1 */
898 coeff11[0] = corr11[0];
899 coeff11[1] = corr11[1];
900 coeff11[2] = corr11[2];
902 /* Set coefs for line 1 image 2 */
903 icvGetCoefForPiece( epipole2_2d,point21,
904 &coeff21[0],&coeff21[1],&coeff21[2],
913 {/* Line 1 cross image 2 */
914 /* Set coefs for line 1 image 1 */
915 icvGetCoefForPiece( epipole1_2d,point11,
916 &coeff11[0],&coeff11[1],&coeff11[2],
924 /* Set coefs for line 1 image 2 */
925 coeff21[0] = corr21[0];
926 coeff21[1] = corr21[1];
927 coeff21[2] = corr21[2];
931 /* ============= Computation for line 2 ================ */
932 /* Find correspondence line for angle points11 */
933 /* corr22 = Fund*p2 */
935 pointW12[0] = point12.x;
936 pointW12[1] = point12.y;
939 icvTransformVector_64d( transFundMatr,
944 /* Find crossing of line with image 2 */
945 icvGetCrossRectDirect( imageSize,
946 corr22[0],corr22[1],corr22[2],
951 {/* We have not cross */
952 /* We must define new angle */
954 pointW22[0] = point22.x;
955 pointW22[1] = point22.y;
958 /* Find correspondence line for this angle points */
959 /* We know point and try to get corr line */
961 /* corr2 = Fund' * p1 */
963 icvTransformVector_64d( fundMatr,
969 /* We have cross. And it's result cross for down line. Set result coefs */
971 /* Set coefs for line 2 image 1 */
972 coeff12[0] = corr12[0];
973 coeff12[1] = corr12[1];
974 coeff12[2] = corr12[2];
976 /* Set coefs for line 1 image 2 */
977 icvGetCoefForPiece( epipole2_2d,point22,
978 &coeff22[0],&coeff22[1],&coeff22[2],
987 {/* Line 2 cross image 2 */
988 /* Set coefs for line 2 image 1 */
989 icvGetCoefForPiece( epipole1_2d,point12,
990 &coeff12[0],&coeff12[1],&coeff12[2],
998 /* Set coefs for line 1 image 2 */
999 coeff22[0] = corr22[0];
1000 coeff22[1] = corr22[1];
1001 coeff22[2] = corr22[2];
1005 /* Now we know common area */
1009 }/* GetCommonArea */
1011 /*---------------------------------------------------------------------------------------*/
1013 /* Get cross for direction1 and direction2 */
1014 /* Result = 1 - cross */
1015 /* Result = 2 - parallel and not equal */
1016 /* Result = 3 - parallel and equal */
1018 void icvGetCrossDirectDirect( CvVect64d direct1,CvVect64d direct2,
1019 CvPoint2D64d *cross,int* result)
1021 double det = direct1[0]*direct2[1] - direct2[0]*direct1[1];
1022 double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2];
1024 if( fabs(det) > EPS64D )
1026 cross->x = detx/det;
1027 cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det;
1031 {/* may be parallel */
1032 if( fabs(detx) > EPS64D )
1033 {/* parallel and not equal */
1045 /*---------------------------------------------------------------------------------------*/
1047 /* Get cross for piece p1,p2 and direction a,b,c */
1048 /* Result = 0 - no cross */
1049 /* Result = 1 - cross */
1050 /* Result = 2 - parallel and not equal */
1051 /* Result = 3 - parallel and equal */
1053 void icvGetCrossPieceDirect( CvPoint2D64d p_start,CvPoint2D64d p_end,
1054 double a,double b,double c,
1055 CvPoint2D64d *cross,int* result)
1058 if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 )
1063 det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
1065 if( fabs(det) < EPS64D )
1066 {/* lines are parallel and may be equal or line is point */
1067 if( fabs(a*p_start.x + b*p_start.y + c) < EPS64D )
1068 {/* line is point or not diff */
1079 detxc = b*(p_end.y*p_start.x - p_start.y*p_end.x) + c*(p_start.x - p_end.x);
1080 detyc = a*(p_end.x*p_start.y - p_start.x*p_end.y) + c*(p_start.y - p_end.y);
1082 cross->x = detxc / det;
1083 cross->y = detyc / det;
1093 /*--------------------------------------------------------------------------------------*/
1095 void icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end,
1096 CvPoint2D64d p2_start,CvPoint2D64d p2_end,
1097 CvPoint2D64d* cross,
1100 double ex1,ey1,ex2,ey2;
1101 double px1,py1,px2,py2;
1103 double delA,delB,delX,delY;
1116 del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2);
1117 if( fabs(del) <= EPS64D )
1118 {/* May be they are parallel !!! */
1123 delA = (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1);
1124 delB = (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1);
1129 if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
1135 delX = (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
1136 (ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2));
1138 delY = (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
1139 (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2));
1141 cross->x = delX / del;
1142 cross->y = delY / del;
1149 /*---------------------------------------------------------------------------------------*/
1151 void icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist)
1153 double dx = point2.x - point1.x;
1154 double dy = point2.y - point1.y;
1155 *dist = sqrt( dx*dx + dy*dy );
1159 /*---------------------------------------------------------------------------------------*/
1161 void icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist)
1163 double dx = point2.x - point1.x;
1164 double dy = point2.y - point1.y;
1165 double dz = point2.z - point1.z;
1166 *dist = sqrt( dx*dx + dy*dy + dz*dz );
1170 /*---------------------------------------------------------------------------------------*/
1172 /* Find line from epipole which cross image rect */
1173 /* Find points of cross 0 or 1 or 2. Return number of points in cross */
1174 void icvGetCrossRectDirect( CvSize imageSize,
1175 double a,double b,double c,
1176 CvPoint2D64d *start,CvPoint2D64d *end,
1179 CvPoint2D64d frameBeg;
1180 CvPoint2D64d frameEnd;
1181 CvPoint2D64d cross[4];
1191 frameEnd.x = imageSize.width;
1194 icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]);
1196 frameBeg.x = imageSize.width;
1198 frameEnd.x = imageSize.width;
1199 frameEnd.y = imageSize.height;
1200 icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]);
1202 frameBeg.x = imageSize.width;
1203 frameBeg.y = imageSize.height;
1205 frameEnd.y = imageSize.height;
1206 icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]);
1209 frameBeg.y = imageSize.height;
1212 icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]);
1225 for( i = 0; i < 3; i++ )
1227 if( haveCross[i] == 1 )
1229 for( j = i + 1; j < 4; j++ )
1231 if( haveCross[j] == 1)
1233 icvGetPieceLength(cross[i],cross[j],&distance);
1234 if( distance > maxDist )
1246 {/* We have cross */
1247 *start = cross[maxI];
1261 }/* GetCrossRectDirect */
1263 /*---------------------------------------------------------------------------------------*/
1264 void icvProjectPointToImage( CvPoint3D64d point,
1265 CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,
1266 CvPoint2D64d* projPoint)
1272 icvMulMatrix_64d ( rotMatr,
1278 icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3);
1280 icvMulMatrix_64d ( camMatr,
1286 projPoint->x = tmpVect1[0] / tmpVect1[2];
1287 projPoint->y = tmpVect1[1] / tmpVect1[2];
1292 /*---------------------------------------------------------------------------------------*/
1293 /* Get quads for transform images */
1294 void icvGetQuadsTransform(
1298 CvVect64d transVect1,
1301 CvVect64d transVect2,
1306 CvPoint3D64d* epipole1,
1307 CvPoint3D64d* epipole2
1310 /* First compute fundamental matrix and epipoles */
1314 /* Compute epipoles and fundamental matrix using new functions */
1316 double convRotMatr[9];
1317 double convTransVect[3];
1319 icvCreateConvertMatrVect( rotMatr1,
1325 float convRotMatr_32f[9];
1326 float convTransVect_32f[3];
1328 icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9);
1329 icvCvt_64d_32f(convTransVect,convTransVect_32f,3);
1331 /* We know R and t */
1332 /* Compute essential matrix */
1334 float fundMatr_32f[9];
1336 float camMatr1_32f[9];
1337 float camMatr2_32f[9];
1339 icvCvt_64d_32f(camMatr1,camMatr1_32f,9);
1340 icvCvt_64d_32f(camMatr2,camMatr2_32f,9);
1342 cvComputeEssentialMatrix( convRotMatr_32f,
1346 cvConvertEssential2Fundamental( essMatr,
1351 CvPoint3D32f epipole1_32f;
1352 CvPoint3D32f epipole2_32f;
1354 cvComputeEpipolesFromFundMatrix( fundMatr_32f,
1357 /* copy to 64d epipoles */
1358 epipole1->x = epipole1_32f.x;
1359 epipole1->y = epipole1_32f.y;
1360 epipole1->z = epipole1_32f.z;
1362 epipole2->x = epipole2_32f.x;
1363 epipole2->y = epipole2_32f.y;
1364 epipole2->z = epipole2_32f.z;
1366 /* Convert fundamental matrix */
1367 icvCvt_32f_64d(fundMatr_32f,fundMatr,9);
1375 icvGetCommonArea( imageSize,
1376 *epipole1,*epipole2,
1382 CvPoint2D64d point11, point12,point21, point22;
1383 double width1,width2;
1384 double height1,height2;
1385 double tmpHeight1,tmpHeight2;
1387 CvPoint2D64d epipole1_2d;
1388 CvPoint2D64d epipole2_2d;
1390 /* ----- Image 1 ----- */
1391 if( fabs(epipole1->z) < 1e-8 )
1395 epipole1_2d.x = epipole1->x / epipole1->z;
1396 epipole1_2d.y = epipole1->y / epipole1->z;
1398 icvGetCutPiece( coeff11,coeff12,
1405 /* Compute distance */
1406 icvGetPieceLength(point11,point21,&width1);
1407 icvGetPieceLength(point11,point12,&tmpHeight1);
1408 icvGetPieceLength(point21,point22,&tmpHeight2);
1409 height1 = MAX(tmpHeight1,tmpHeight2);
1411 quad1[0][0] = point11.x;
1412 quad1[0][1] = point11.y;
1414 quad1[1][0] = point21.x;
1415 quad1[1][1] = point21.y;
1417 quad1[2][0] = point22.x;
1418 quad1[2][1] = point22.y;
1420 quad1[3][0] = point12.x;
1421 quad1[3][1] = point12.y;
1423 /* ----- Image 2 ----- */
1424 if( fabs(epipole2->z) < 1e-8 )
1428 epipole2_2d.x = epipole2->x / epipole2->z;
1429 epipole2_2d.y = epipole2->y / epipole2->z;
1431 icvGetCutPiece( coeff21,coeff22,
1438 /* Compute distance */
1439 icvGetPieceLength(point11,point21,&width2);
1440 icvGetPieceLength(point11,point12,&tmpHeight1);
1441 icvGetPieceLength(point21,point22,&tmpHeight2);
1442 height2 = MAX(tmpHeight1,tmpHeight2);
1444 quad2[0][0] = point11.x;
1445 quad2[0][1] = point11.y;
1447 quad2[1][0] = point21.x;
1448 quad2[1][1] = point21.y;
1450 quad2[2][0] = point22.x;
1451 quad2[2][1] = point22.y;
1453 quad2[3][0] = point12.x;
1454 quad2[3][1] = point12.y;
1457 /*=======================================================*/
1458 /* This is a new additional way to compute quads. */
1459 /* We must correct quads */
1461 double convRotMatr[9];
1462 double convTransVect[3];
1464 double newQuad1[4][2];
1465 double newQuad2[4][2];
1468 icvCreateConvertMatrVect( rotMatr1,
1475 /* -------------Compute for first image-------------- */
1476 CvPoint2D32f pointb1;
1477 CvPoint2D32f pointe1;
1479 CvPoint2D32f pointb2;
1480 CvPoint2D32f pointe2;
1482 pointb1.x = (float)quad1[0][0];
1483 pointb1.y = (float)quad1[0][1];
1485 pointe1.x = (float)quad1[3][0];
1486 pointe1.y = (float)quad1[3][1];
1488 icvComputeeInfiniteProject1(convRotMatr,
1494 icvComputeeInfiniteProject1(convRotMatr,
1500 /* JUST TEST FOR POINT */
1502 /* Compute distances */
1505 double distOld,distNew;
1507 dxOld = quad2[1][0] - quad2[0][0];
1508 dyOld = quad2[1][1] - quad2[0][1];
1509 distOld = dxOld*dxOld + dyOld*dyOld;
1511 dxNew = quad2[1][0] - pointb2.x;
1512 dyNew = quad2[1][1] - pointb2.y;
1513 distNew = dxNew*dxNew + dyNew*dyNew;
1515 if( distNew > distOld )
1516 {/* Get new points for second quad */
1517 newQuad2[0][0] = pointb2.x;
1518 newQuad2[0][1] = pointb2.y;
1519 newQuad2[3][0] = pointe2.x;
1520 newQuad2[3][1] = pointe2.y;
1521 newQuad1[0][0] = quad1[0][0];
1522 newQuad1[0][1] = quad1[0][1];
1523 newQuad1[3][0] = quad1[3][0];
1524 newQuad1[3][1] = quad1[3][1];
1527 {/* Get new points for first quad */
1529 pointb2.x = (float)quad2[0][0];
1530 pointb2.y = (float)quad2[0][1];
1532 pointe2.x = (float)quad2[3][0];
1533 pointe2.y = (float)quad2[3][1];
1535 icvComputeeInfiniteProject2(convRotMatr,
1541 icvComputeeInfiniteProject2(convRotMatr,
1548 /* JUST TEST FOR POINT */
1550 newQuad2[0][0] = quad2[0][0];
1551 newQuad2[0][1] = quad2[0][1];
1552 newQuad2[3][0] = quad2[3][0];
1553 newQuad2[3][1] = quad2[3][1];
1555 newQuad1[0][0] = pointb1.x;
1556 newQuad1[0][1] = pointb1.y;
1557 newQuad1[3][0] = pointe1.x;
1558 newQuad1[3][1] = pointe1.y;
1561 /* -------------Compute for second image-------------- */
1562 pointb1.x = (float)quad1[1][0];
1563 pointb1.y = (float)quad1[1][1];
1565 pointe1.x = (float)quad1[2][0];
1566 pointe1.y = (float)quad1[2][1];
1568 icvComputeeInfiniteProject1(convRotMatr,
1574 icvComputeeInfiniteProject1(convRotMatr,
1580 /* Compute distances */
1582 dxOld = quad2[0][0] - quad2[1][0];
1583 dyOld = quad2[0][1] - quad2[1][1];
1584 distOld = dxOld*dxOld + dyOld*dyOld;
1586 dxNew = quad2[0][0] - pointb2.x;
1587 dyNew = quad2[0][1] - pointb2.y;
1588 distNew = dxNew*dxNew + dyNew*dyNew;
1590 if( distNew > distOld )
1591 {/* Get new points for second quad */
1592 newQuad2[1][0] = pointb2.x;
1593 newQuad2[1][1] = pointb2.y;
1594 newQuad2[2][0] = pointe2.x;
1595 newQuad2[2][1] = pointe2.y;
1596 newQuad1[1][0] = quad1[1][0];
1597 newQuad1[1][1] = quad1[1][1];
1598 newQuad1[2][0] = quad1[2][0];
1599 newQuad1[2][1] = quad1[2][1];
1602 {/* Get new points for first quad */
1604 pointb2.x = (float)quad2[1][0];
1605 pointb2.y = (float)quad2[1][1];
1607 pointe2.x = (float)quad2[2][0];
1608 pointe2.y = (float)quad2[2][1];
1610 icvComputeeInfiniteProject2(convRotMatr,
1616 icvComputeeInfiniteProject2(convRotMatr,
1622 newQuad2[1][0] = quad2[1][0];
1623 newQuad2[1][1] = quad2[1][1];
1624 newQuad2[2][0] = quad2[2][0];
1625 newQuad2[2][1] = quad2[2][1];
1627 newQuad1[1][0] = pointb1.x;
1628 newQuad1[1][1] = pointb1.y;
1629 newQuad1[2][0] = pointe1.x;
1630 newQuad1[2][1] = pointe1.y;
1635 /*-------------------------------------------------------------------------------*/
1637 /* Copy new quads to old quad */
1639 for( i = 0; i < 4; i++ )
1642 quad1[i][0] = newQuad1[i][0];
1643 quad1[i][1] = newQuad1[i][1];
1644 quad2[i][0] = newQuad2[i][0];
1645 quad2[i][1] = newQuad2[i][1];
1649 /*=======================================================*/
1651 double warpWidth,warpHeight;
1653 warpWidth = MAX(width1,width2);
1654 warpHeight = MAX(height1,height2);
1656 warpSize->width = (int)warpWidth;
1657 warpSize->height = (int)warpHeight;
1659 warpSize->width = cvRound(warpWidth-1);
1660 warpSize->height = cvRound(warpHeight-1);
1662 /* !!! by Valery Mosyagin. this lines added just for test no warp */
1663 warpSize->width = imageSize.width;
1664 warpSize->height = imageSize.height;
1670 /*---------------------------------------------------------------------------------------*/
1672 void icvGetQuadsTransformNew( CvSize imageSize,
1676 CvVect32f transVect1,
1681 CvPoint3D32f* epipole1,
1682 CvPoint3D32f* epipole2
1686 /* Convert camera matrix */
1687 double camMatr1_64d[9];
1688 double camMatr2_64d[9];
1689 double rotMatr1_64d[9];
1690 double transVect1_64d[3];
1691 double rotMatr2_64d[9];
1692 double transVect2_64d[3];
1693 double fundMatr_64d[9];
1694 CvPoint3D64d epipole1_64d;
1695 CvPoint3D64d epipole2_64d;
1697 icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
1698 icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
1699 icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
1700 icvCvt_32f_64d(transVect1,transVect1_64d,3);
1702 /* Create vector and matrix */
1704 rotMatr2_64d[0] = 1;
1705 rotMatr2_64d[1] = 0;
1706 rotMatr2_64d[2] = 0;
1707 rotMatr2_64d[3] = 0;
1708 rotMatr2_64d[4] = 1;
1709 rotMatr2_64d[5] = 0;
1710 rotMatr2_64d[6] = 0;
1711 rotMatr2_64d[7] = 0;
1712 rotMatr2_64d[8] = 1;
1714 transVect2_64d[0] = 0;
1715 transVect2_64d[1] = 0;
1716 transVect2_64d[2] = 0;
1718 icvGetQuadsTransform( imageSize,
1733 /* Convert epipoles */
1734 epipole1->x = (float)(epipole1_64d.x);
1735 epipole1->y = (float)(epipole1_64d.y);
1736 epipole1->z = (float)(epipole1_64d.z);
1738 epipole2->x = (float)(epipole2_64d.x);
1739 epipole2->y = (float)(epipole2_64d.y);
1740 epipole2->z = (float)(epipole2_64d.z);
1742 /* Convert fundamental matrix */
1743 icvCvt_64d_32f(fundMatr_64d,fundMatr,9);
1748 /*---------------------------------------------------------------------------------------*/
1749 void icvGetQuadsTransformStruct( CvStereoCamera* stereoCamera)
1751 /* Wrapper for icvGetQuadsTransformNew */
1757 icvGetQuadsTransformNew( cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),cvRound(stereoCamera->camera[0]->imgSize[1])),
1758 stereoCamera->camera[0]->matrix,
1759 stereoCamera->camera[1]->matrix,
1760 stereoCamera->rotMatrix,
1761 stereoCamera->transVector,
1762 &(stereoCamera->warpSize),
1765 stereoCamera->fundMatr,
1766 &(stereoCamera->epipole[0]),
1767 &(stereoCamera->epipole[1])
1771 for( i = 0; i < 4; i++ )
1773 stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]);
1774 stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]);
1780 /*---------------------------------------------------------------------------------------*/
1781 void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
1783 /* For given intrinsic and extrinsic parameters computes rest parameters
1784 ** such as fundamental matrix. warping coeffs, epipoles, ...
1788 /* compute rotate matrix and translate vector */
1792 double transVect1[3];
1793 double transVect2[3];
1795 double convRotMatr[9];
1796 double convTransVect[3];
1799 icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9);
1800 icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9);
1802 icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3);
1803 icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3);
1805 icvCreateConvertMatrVect( rotMatr1,
1812 /* copy to stereo camera params */
1813 icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9);
1814 icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3);
1817 icvGetQuadsTransformStruct(stereoCamera);
1818 icvComputeRestStereoParams(stereoCamera);
1823 /*---------------------------------------------------------------------------------------*/
1825 /* Get cut line for one image */
1826 void icvGetCutPiece( CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,
1827 CvPoint2D64d epipole,
1829 CvPoint2D64d* point11,CvPoint2D64d* point12,
1830 CvPoint2D64d* point21,CvPoint2D64d* point22,
1833 /* Compute nearest cut line to epipole */
1834 /* Get corners inside sector */
1835 /* Collect all candidate point */
1837 CvPoint2D64d candPoints[8];
1838 CvPoint2D64d midPoint;
1846 /* Find middle line of sector */
1851 CvPoint2D64d pointOnLine1; pointOnLine1.x = pointOnLine1.y = 0;
1852 CvPoint2D64d pointOnLine2; pointOnLine2.x = pointOnLine2.y = 0;
1854 CvPoint2D64d start1,end1;
1856 icvGetCrossRectDirect( imageSize,
1857 areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
1858 &start1,&end1,&res);
1861 pointOnLine1 = start1;
1864 icvGetCrossRectDirect( imageSize,
1865 areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
1866 &start1,&end1,&res);
1869 pointOnLine2 = start1;
1872 icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);
1874 icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);
1876 /* Test corner points */
1877 CvPoint2D64d cornerPoint;
1878 CvPoint2D64d tmpPoints[2];
1882 icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1885 candPoints[numPoints] = cornerPoint;
1889 cornerPoint.x = imageSize.width;
1891 icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1894 candPoints[numPoints] = cornerPoint;
1898 cornerPoint.x = imageSize.width;
1899 cornerPoint.y = imageSize.height;
1900 icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1903 candPoints[numPoints] = cornerPoint;
1908 cornerPoint.y = imageSize.height;
1909 icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1912 candPoints[numPoints] = cornerPoint;
1916 /* Find cross line 1 with image border */
1917 icvGetCrossRectDirect( imageSize,
1918 areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
1919 &tmpPoints[0], &tmpPoints[1],
1921 for( i = 0; i < res; i++ )
1923 candPoints[numPoints++] = tmpPoints[i];
1926 /* Find cross line 2 with image border */
1927 icvGetCrossRectDirect( imageSize,
1928 areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
1929 &tmpPoints[0], &tmpPoints[1],
1932 for( i = 0; i < res; i++ )
1934 candPoints[numPoints++] = tmpPoints[i];
1940 return;/* Error. Not enought points */
1942 /* Project all points to middle line and get max and min */
1944 CvPoint2D64d projPoint;
1945 CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
1946 CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;
1951 double minDist = 10000000;
1954 for( i = 0; i < numPoints; i++ )
1956 icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
1957 icvGetPieceLength(epipole,projPoint,&dist);
1961 minPoint = projPoint;
1967 maxPoint = projPoint;
1971 /* We know maximum and minimum points. Now we can compute cut lines */
1973 icvGetNormalDirect(midLine,minPoint,cutLine1);
1974 icvGetNormalDirect(midLine,maxPoint,cutLine2);
1976 /* Test for begin of line. */
1977 CvPoint2D64d tmpPoint2;
1979 /* Get cross with */
1980 icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
1981 icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);
1983 icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
1984 icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);
1986 if( epipole.x > imageSize.width * 0.5 )
1987 {/* Need to change points */
1988 tmpPoint2 = *point11;
1989 *point11 = *point21;
1990 *point21 = tmpPoint2;
1992 tmpPoint2 = *point12;
1993 *point12 = *point22;
1994 *point22 = tmpPoint2;
1999 /*---------------------------------------------------------------------------------------*/
2000 /* Get middle angle */
2001 void icvGetMiddleAnglePoint( CvPoint2D64d basePoint,
2002 CvPoint2D64d point1,CvPoint2D64d point2,
2003 CvPoint2D64d* midPoint)
2004 {/* !!! May be need to return error */
2008 icvGetPieceLength(basePoint,point1,&dist1);
2009 icvGetPieceLength(basePoint,point2,&dist2);
2010 CvPoint2D64d pointNew1;
2011 CvPoint2D64d pointNew2;
2012 double alpha = dist2/dist1;
2014 pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
2015 pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );
2017 pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
2018 pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );
2021 icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);
2026 /*---------------------------------------------------------------------------------------*/
2027 /* Get normal direct to direct in line */
2028 void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
2030 normDirect[0] = direct[1];
2031 normDirect[1] = - direct[0];
2032 normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);
2036 /*---------------------------------------------------------------------------------------*/
2037 CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
2039 return (point1.x - basePoint.x)*(point2.y - basePoint.y) -
2040 (point2.x - basePoint.x)*(point1.y - basePoint.y);
2042 /*---------------------------------------------------------------------------------------*/
2043 /* Test for point in sector */
2044 /* Return 0 - point not inside sector */
2045 /* Return 1 - point inside sector */
2046 void icvTestPoint( CvPoint2D64d testPoint,
2047 CvVect64d line1,CvVect64d line2,
2048 CvPoint2D64d basePoint,
2051 CvPoint2D64d point1,point2;
2053 icvProjectPointToDirect(testPoint,line1,&point1);
2054 icvProjectPointToDirect(testPoint,line2,&point2);
2056 double sign1 = icvGetVect(basePoint,point1,point2);
2057 double sign2 = icvGetVect(basePoint,point1,testPoint);
2058 if( sign1 * sign2 > 0 )
2059 {/* Correct for first line */
2061 sign2 = icvGetVect(basePoint,point2,testPoint);
2062 if( sign1 * sign2 > 0 )
2063 {/* Correct for both lines */
2079 /*---------------------------------------------------------------------------------------*/
2080 /* Project point to line */
2081 void icvProjectPointToDirect( CvPoint2D64d point,CvVect64d lineCoeff,
2082 CvPoint2D64d* projectPoint)
2084 double a = lineCoeff[0];
2085 double b = lineCoeff[1];
2087 double det = 1.0 / ( a*a + b*b );
2088 double delta = a*point.y - b*point.x;
2090 projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
2091 projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;
2096 /*---------------------------------------------------------------------------------------*/
2097 /* Get distance from point to direction */
2098 void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
2100 CvPoint2D64d tmpPoint;
2101 icvProjectPointToDirect(point,lineCoef,&tmpPoint);
2102 double dx = point.x - tmpPoint.x;
2103 double dy = point.y - tmpPoint.y;
2104 *dist = sqrt(dx*dx+dy*dy);
2107 /*---------------------------------------------------------------------------------------*/
2109 CV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
2110 int desired_depth, int desired_num_channels )
2113 src_size.width = src->width;
2114 src_size.height = src->height;
2116 CvSize dst_size = src_size;
2120 dst_size.width = dst->width;
2121 dst_size.height = dst->height;
2124 if( !dst || dst->depth != desired_depth ||
2125 dst->nChannels != desired_num_channels ||
2126 dst_size.width != src_size.width ||
2127 dst_size.height != dst_size.height )
2129 cvReleaseImage( &dst );
2130 dst = cvCreateImage( src_size, desired_depth, desired_num_channels );
2131 CvRect rect = cvRect(0,0,src_size.width,src_size.height);
2132 cvSetImageROI( dst, rect );
2140 icvCvt_32f_64d( float *src, double *dst, int size )
2145 return CV_NULLPTR_ERR;
2147 return CV_BADRANGE_ERR;
2149 for( t = 0; t < size; t++ )
2151 dst[t] = (double) (src[t]);
2157 /*======================================================================================*/
2158 /* Type conversion double -> float */
2160 icvCvt_64d_32f( double *src, float *dst, int size )
2165 return CV_NULLPTR_ERR;
2167 return CV_BADRANGE_ERR;
2169 for( t = 0; t < size; t++ )
2171 dst[t] = (float) (src[t]);
2177 /*----------------------------------------------------------------------------------*/
2180 /* Find line which cross frame by line(a,b,c) */
2181 void FindLineForEpiline( CvSize imageSize,
2182 float a,float b,float c,
2183 CvPoint2D32f *start,CvPoint2D32f *end,
2187 CvPoint2D32f frameBeg;
2189 CvPoint2D32f frameEnd;
2190 CvPoint2D32f cross[4];
2201 frameEnd.x = (float)(imageSize.width);
2203 haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
2205 frameBeg.x = (float)(imageSize.width);
2207 frameEnd.x = (float)(imageSize.width);
2208 frameEnd.y = (float)(imageSize.height);
2209 haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
2211 frameBeg.x = (float)(imageSize.width);
2212 frameBeg.y = (float)(imageSize.height);
2214 frameEnd.y = (float)(imageSize.height);
2215 haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
2218 frameBeg.y = (float)(imageSize.height);
2221 haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
2224 float minDist = (float)(INT_MAX);
2225 float maxDist = (float)(INT_MIN);
2230 double midPointX = imageSize.width / 2.0;
2231 double midPointY = imageSize.height / 2.0;
2233 for( n = 0; n < 4; n++ )
2235 if( haveCross[n] > 0 )
2237 dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
2238 (midPointY - cross[n].y)*(midPointY - cross[n].y));
2240 if( dist < minDist )
2246 if( dist > maxDist )
2254 if( minN >= 0 && maxN >= 0 && (minN != maxN) )
2256 *start = cross[minN];
2272 /*----------------------------------------------------------------------------------*/
2274 int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
2276 float width = (float)(imageSize.width);
2277 float height = (float)(imageSize.height);
2279 /* Get crosslines with image corners */
2281 /* Find four lines */
2283 CvPoint2D32f pa,pb,pc,pd;
2297 /* We can compute points for angle */
2298 /* Test for place section */
2311 else if( y > height )
2322 else if ( x > width )
2329 else if ( y > height )
2347 else if( y > height )
2353 {/* 5 - point in the image */
2364 /*--------------------------------------------------------------------------------------*/
2365 void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])
2366 {/* Computes perspective coeffs for transformation from src to dst quad */
2369 CV_FUNCNAME( "icvComputePerspectiveCoeffs" );
2384 for( i = 0; i < 4; i++ )
2387 double x = dstQuad[i].x;
2388 double y = dstQuad[i].y;
2394 double X = dstQuad[i].x;
2395 double Y = dstQuad[i].y;
2397 double* a = A + i*16;
2425 CvMat matA = cvMat( 8, 8, CV_64F, A );
2426 CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
2427 CvMat matB = cvMat( 8, 1, CV_64F, b );
2428 CvMat matX = cvMat( 8, 1, CV_64F, c );
2430 CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2431 CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2434 coeffs[0][0] = c[0];
2435 coeffs[0][1] = c[1];
2436 coeffs[0][2] = c[2];
2437 coeffs[1][0] = c[3];
2438 coeffs[1][1] = c[4];
2439 coeffs[1][2] = c[5];
2440 coeffs[2][0] = c[6];
2441 coeffs[2][1] = c[7];
2449 /*--------------------------------------------------------------------------------------*/
2451 CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY )
2453 CV_FUNCNAME( "cvComputePerspectiveMap" );
2458 CvMat stubx, *mapx = (CvMat*)rectMapX;
2459 CvMat stuby, *mapy = (CvMat*)rectMapY;
2462 CV_CALL( mapx = cvGetMat( mapx, &stubx ));
2463 CV_CALL( mapy = cvGetMat( mapy, &stuby ));
2465 if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 )
2466 CV_ERROR( CV_StsUnsupportedFormat, "" );
2468 size = cvGetMatSize(mapx);
2469 assert( fabs(c[2][2] - 1.) < FLT_EPSILON );
2471 for( i = 0; i < size.height; i++ )
2473 float* mx = (float*)(mapx->data.ptr + mapx->step*i);
2474 float* my = (float*)(mapy->data.ptr + mapy->step*i);
2476 for( j = 0; j < size.width; j++ )
2478 double w = 1./(c[2][0]*j + c[2][1]*i + 1.);
2479 double x = (c[0][0]*j + c[0][1]*i + c[0][2])*w;
2480 double y = (c[1][0]*j + c[1][1]*i + c[1][2])*w;
2490 /*--------------------------------------------------------------------------------------*/
2492 CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
2495 /* Computes Perspective Transform coeffs and map if need
2496 for given image size and given result quad */
2497 CV_FUNCNAME( "cvInitPerspectiveTransform" );
2505 CvMat mapstub, *map = (CvMat*)rectMap;
2510 CV_CALL( map = cvGetMat( map, &mapstub ));
2512 if( CV_MAT_TYPE( map->type ) != CV_32FC2 )
2513 CV_ERROR( CV_StsUnsupportedFormat, "" );
2515 if( map->width != size.width || map->height != size.height )
2516 CV_ERROR( CV_StsUnmatchedSizes, "" );
2519 pt[0] = cvPoint2D32f( 0, 0 );
2520 pt[1] = cvPoint2D32f( size.width, 0 );
2521 pt[2] = cvPoint2D32f( size.width, size.height );
2522 pt[3] = cvPoint2D32f( 0, size.height );
2524 for( i = 0; i < 4; i++ )
2527 double x = quad[i].x;
2528 double y = quad[i].y;
2534 double X = quad[i].x;
2535 double Y = quad[i].y;
2537 double* a = A + i*16;
2565 CvMat matA = cvMat( 8, 8, CV_64F, A );
2566 CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
2567 CvMat matB = cvMat( 8, 1, CV_64F, b );
2568 CvMat matX = cvMat( 8, 1, CV_64F, c );
2570 CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2571 CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2574 matrix[0][0] = c[0];
2575 matrix[0][1] = c[1];
2576 matrix[0][2] = c[2];
2577 matrix[1][0] = c[3];
2578 matrix[1][1] = c[4];
2579 matrix[1][2] = c[5];
2580 matrix[2][0] = c[6];
2581 matrix[2][1] = c[7];
2586 for( i = 0; i < size.height; i++ )
2588 CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i);
2589 for( j = 0; j < size.width; j++ )
2591 double w = 1./(c[6]*j + c[7]*i + 1.);
2592 double x = (c[0]*j + c[1]*i + c[2])*w;
2593 double y = (c[3]*j + c[4]*i + c[5])*w;
2595 maprow[j].x = (float)x;
2596 maprow[j].y = (float)y;
2607 /*-----------------------------------------------------------------------*/
2608 /* Compute projected infinite point for second image if first image point is known */
2609 void icvComputeeInfiniteProject1( CvMatr64d rotMatr,
2612 CvPoint2D32f point1,
2613 CvPoint2D32f* point2)
2616 icvInvertMatrix_64d(camMatr1,3,invMatr1);
2619 p1[0] = (double)(point1.x);
2620 p1[1] = (double)(point1.y);
2623 icvMulMatrix_64d( invMatr1,
2630 icvTransposeMatrix_64d( rotMatr, 3, 3, invR );
2632 /* Change system 1 to system 2 */
2634 icvMulMatrix_64d( invR,
2640 /* Now we can project this point to image 2 */
2643 icvMulMatrix_64d( camMatr2,
2649 point2->x = (float)(projP[0] / projP[2]);
2650 point2->y = (float)(projP[1] / projP[2]);
2655 /*-----------------------------------------------------------------------*/
2656 /* Compute projected infinite point for second image if first image point is known */
2657 void icvComputeeInfiniteProject2( CvMatr64d rotMatr,
2660 CvPoint2D32f* point1,
2661 CvPoint2D32f point2)
2664 icvInvertMatrix_64d(camMatr2,3,invMatr2);
2667 p2[0] = (double)(point2.x);
2668 p2[1] = (double)(point2.y);
2671 icvMulMatrix_64d( invMatr2,
2677 /* Change system 1 to system 2 */
2680 icvMulMatrix_64d( rotMatr,
2686 /* Now we can project this point to image 2 */
2689 icvMulMatrix_64d( camMatr1,
2695 point1->x = (float)(projP[0] / projP[2]);
2696 point1->y = (float)(projP[1] / projP[2]);
2701 /* Select best R and t for given cameras, points, ... */
2702 /* For both cameras */
2703 int icvSelectBestRt( int numImages,
2705 CvPoint2D32f* imagePoints1,
2706 CvPoint2D32f* imagePoints2,
2707 CvPoint3D32f* objectPoints,
2709 CvMatr32f cameraMatrix1,
2710 CvVect32f distortion1,
2711 CvMatr32f rotMatrs1,
2712 CvVect32f transVects1,
2714 CvMatr32f cameraMatrix2,
2715 CvVect32f distortion2,
2716 CvMatr32f rotMatrs2,
2717 CvVect32f transVects2,
2719 CvMatr32f bestRotMatr,
2720 CvVect32f bestTransVect
2724 /* Need to convert input data 32 -> 64 */
2725 CvPoint3D64d* objectPoints_64d;
2727 double* rotMatrs1_64d;
2728 double* rotMatrs2_64d;
2730 double* transVects1_64d;
2731 double* transVects2_64d;
2733 double cameraMatrix1_64d[9];
2734 double cameraMatrix2_64d[9];
2736 double distortion1_64d[4];
2737 double distortion2_64d[4];
2739 /* allocate memory for 64d data */
2743 for( i = 0; i < numImages; i++ )
2745 totalNum += numPoints[i];
2748 objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));
2750 rotMatrs1_64d = (double*)calloc(numImages,sizeof(double)*9);
2751 rotMatrs2_64d = (double*)calloc(numImages,sizeof(double)*9);
2753 transVects1_64d = (double*)calloc(numImages,sizeof(double)*3);
2754 transVects2_64d = (double*)calloc(numImages,sizeof(double)*3);
2756 /* Convert input data to 64d */
2758 icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d, totalNum*3);
2760 icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d, numImages*9);
2761 icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d, numImages*9);
2763 icvCvt_32f_64d(transVects1, transVects1_64d, numImages*3);
2764 icvCvt_32f_64d(transVects2, transVects2_64d, numImages*3);
2766 /* Convert to arrays */
2767 icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
2768 icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
2770 icvCvt_32f_64d(distortion1, distortion1_64d, 4);
2771 icvCvt_32f_64d(distortion2, distortion2_64d, 4);
2774 /* for each R and t compute error for image pair */
2777 errors = (float*)calloc(numImages*numImages,sizeof(float));
2780 return CV_OUTOFMEM_ERR;
2785 for( currRt = 0; currRt < numImages; currRt++ )
2788 for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2790 /* For current R,t R,t compute relative position of cameras */
2792 double convRotMatr[9];
2793 double convTransVect[3];
2795 icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
2796 transVects1_64d + currRt*3,
2797 rotMatrs2_64d + currRt*9,
2798 transVects2_64d + currRt*3,
2802 /* Project points using relative position of cameras */
2804 double convRotMatr2[9];
2805 double convTransVect2[3];
2807 convRotMatr2[0] = 1;
2808 convRotMatr2[1] = 0;
2809 convRotMatr2[2] = 0;
2811 convRotMatr2[3] = 0;
2812 convRotMatr2[4] = 1;
2813 convRotMatr2[5] = 0;
2815 convRotMatr2[6] = 0;
2816 convRotMatr2[7] = 0;
2817 convRotMatr2[8] = 1;
2819 convTransVect2[0] = 0;
2820 convTransVect2[1] = 0;
2821 convTransVect2[2] = 0;
2823 /* Compute error for given pair and Rt */
2824 /* We must project points to image and compute error */
2826 CvPoint2D64d* projImagePoints1;
2827 CvPoint2D64d* projImagePoints2;
2829 CvPoint3D64d* points1;
2830 CvPoint3D64d* points2;
2833 numberPnt = numPoints[currImagePair];
2834 projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2835 projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2837 points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2838 points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2840 /* Transform object points to first camera position */
2842 for( i = 0; i < numberPnt; i++ )
2844 /* Create second camera point */
2845 CvPoint3D64d tmpPoint;
2846 tmpPoint.x = (double)(objectPoints[i].x);
2847 tmpPoint.y = (double)(objectPoints[i].y);
2848 tmpPoint.z = (double)(objectPoints[i].z);
2850 icvConvertPointSystem( tmpPoint,
2852 rotMatrs2_64d + currImagePair*9,
2853 transVects2_64d + currImagePair*3);
2855 /* Create first camera point using R, t */
2856 icvConvertPointSystem( points2[i],
2861 CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
2862 icvConvertPointSystem( tmpPoint,
2864 rotMatrs1_64d + currImagePair*9,
2865 transVects1_64d + currImagePair*3);
2868 dx = tmpPoint2.x - points1[i].x;
2869 dy = tmpPoint2.y - points1[i].y;
2870 dz = tmpPoint2.z - points1[i].z;
2871 err = sqrt(dx*dx + dy*dy + dz*dz);
2877 cvProjectPointsSimple( numPoints[currImagePair],
2878 objectPoints_64d + begPoint,
2879 rotMatrs1_64d + currRt*9,
2880 transVects1_64d + currRt*3,
2885 cvProjectPointsSimple( numPoints[currImagePair],
2886 objectPoints_64d + begPoint,
2887 rotMatrs2_64d + currRt*9,
2888 transVects2_64d + currRt*3,
2894 /* Project with no translate and no rotation */
2898 double nodist[4] = {0,0,0,0};
2899 cvProjectPointsSimple( numPoints[currImagePair],
2907 cvProjectPointsSimple( numPoints[currImagePair],
2918 cvProjectPointsSimple( numPoints[currImagePair],
2926 cvProjectPointsSimple( numPoints[currImagePair],
2934 /* points are projected. Compute error */
2940 for( currPoint = 0; currPoint < numberPnt; currPoint++ )
2944 dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
2945 dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
2946 len1 = sqrt(dx1*dx1 + dy1*dy1);
2950 dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
2951 dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
2952 len2 = sqrt(dx2*dx2 + dy2*dy2);
2956 err1 /= (float)(numberPnt);
2957 err2 /= (float)(numberPnt);
2959 err = (err1+err2) * 0.5;
2960 begPoint += numberPnt;
2962 /* Set this error to */
2963 errors[numImages*currImagePair+currRt] = (float)err;
2967 free(projImagePoints1);
2968 free(projImagePoints2);
2972 /* Just select R and t with minimal average error */
2975 float minError = 0;/* Just for no warnings. Uses 'first' flag. */
2977 for( currRt = 0; currRt < numImages; currRt++ )
2980 for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2982 avErr += errors[numImages*currImagePair+currRt];
2984 avErr /= (float)(numImages);
2994 if( avErr < minError )
3003 double bestRotMatr_64d[9];
3004 double bestTransVect_64d[3];
3006 icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
3007 transVects1_64d + bestnumRt * 3,
3008 rotMatrs2_64d + bestnumRt * 9,
3009 transVects2_64d + bestnumRt * 3,
3013 icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
3014 icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);
3023 /* ----------------- Stereo calibration functions --------------------- */
3025 float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
3027 float ax = point2.x - point1.x;
3028 float ay = point2.y - point1.y;
3030 float bx = point.x - point1.x;
3031 float by = point.y - point1.y;
3033 return (ax*by - ay*bx);
3036 /* Convert function for stereo warping */
3037 int icvConvertWarpCoordinates(double coeffs[3][3],
3038 CvPoint2D32f* cameraPoint,
3039 CvPoint2D32f* warpPoint,
3044 if( direction == CV_WARP_TO_CAMERA )
3045 {/* convert from camera image to warped image coordinates */
3049 det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
3050 if( fabs(det) > 1e-8 )
3052 cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
3053 cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
3057 else if( direction == CV_CAMERA_TO_WARP )
3058 {/* convert from warped image to camera image coordinates */
3062 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]);
3064 if( fabs(det) > 1e-8 )
3066 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);
3067 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);
3072 return CV_BADFACTOR_ERR;
3075 /* Compute stereo params using some camera params */
3076 /* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
3077 int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
3081 icvGetQuadsTransformStruct(stereoparams);
3083 cvInitPerspectiveTransform( stereoparams->warpSize,
3084 stereoparams->quad[0],
3085 stereoparams->coeffs[0],
3088 cvInitPerspectiveTransform( stereoparams->warpSize,
3089 stereoparams->quad[1],
3090 stereoparams->coeffs[1],
3093 /* Create border for warped images */
3094 CvPoint2D32f corns[4];
3098 corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3101 corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3102 corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3105 corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3108 for( i = 0; i < 4; i++ )
3110 /* For first camera */
3111 icvConvertWarpCoordinates( stereoparams->coeffs[0],
3113 stereoparams->border[0]+i,
3116 /* For second camera */
3117 icvConvertWarpCoordinates( stereoparams->coeffs[1],
3119 stereoparams->border[1]+i,
3125 CvPoint2D32f warpPoints[4];
3126 warpPoints[0] = cvPoint2D32f(0,0);
3127 warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0);
3128 warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1);
3129 warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1);
3131 CvPoint2D32f camPoints1[4];
3132 CvPoint2D32f camPoints2[4];
3134 for( int i = 0; i < 4; i++ )
3136 icvConvertWarpCoordinates(stereoparams->coeffs[0],
3141 icvConvertWarpCoordinates(stereoparams->coeffs[1],
3149 /* Allocate memory for scanlines coeffs */
3151 stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));
3153 /* Compute coeffs for epilines */
3155 icvComputeCoeffForStereo( stereoparams);
3157 /* all coeffs are known */
3161 /*-------------------------------------------------------------------------------------------*/
3163 int icvStereoCalibration( int numImages,
3166 CvPoint2D32f* imagePoints1,
3167 CvPoint2D32f* imagePoints2,
3168 CvPoint3D32f* objectPoints,
3169 CvStereoCamera* stereoparams
3172 /* Firstly we must calibrate both cameras */
3173 /* Alocate memory for data */
3174 /* Allocate for translate vectors */
3180 transVects1 = (float*)calloc(numImages,sizeof(float)*3);
3181 transVects2 = (float*)calloc(numImages,sizeof(float)*3);
3183 rotMatrs1 = (float*)calloc(numImages,sizeof(float)*9);
3184 rotMatrs2 = (float*)calloc(numImages,sizeof(float)*9);
3186 /* Calibrate first camera */
3187 cvCalibrateCamera( numImages,
3192 stereoparams->camera[0]->distortion,
3193 stereoparams->camera[0]->matrix,
3198 /* Calibrate second camera */
3199 cvCalibrateCamera( numImages,
3204 stereoparams->camera[1]->distortion,
3205 stereoparams->camera[1]->matrix,
3210 /* Cameras are calibrated */
3212 stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
3213 stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
3215 stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
3216 stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
3218 icvSelectBestRt( numImages,
3223 stereoparams->camera[0]->matrix,
3224 stereoparams->camera[0]->distortion,
3227 stereoparams->camera[1]->matrix,
3228 stereoparams->camera[1]->distortion,
3231 stereoparams->rotMatrix,
3232 stereoparams->transVector
3241 icvComputeRestStereoParams(stereoparams);
3246 /* Find line from epipole */
3247 void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
3249 CvPoint2D32f frameBeg;
3250 CvPoint2D32f frameEnd;
3251 CvPoint2D32f cross[4];
3262 frameEnd.x = (float)(imageSize.width);
3264 haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);
3266 frameBeg.x = (float)(imageSize.width);
3268 frameEnd.x = (float)(imageSize.width);
3269 frameEnd.y = (float)(imageSize.height);
3270 haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);
3272 frameBeg.x = (float)(imageSize.width);
3273 frameBeg.y = (float)(imageSize.height);
3275 frameEnd.y = (float)(imageSize.height);
3276 haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);
3279 frameBeg.y = (float)(imageSize.height);
3282 haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);
3285 float minDist = (float)(INT_MAX);
3286 float maxDist = (float)(INT_MIN);
3291 for( n = 0; n < 4; n++ )
3293 if( haveCross[n] > 0 )
3295 dist = (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
3296 (epipole.y - cross[n].y)*(epipole.y - cross[n].y);
3298 if( dist < minDist )
3304 if( dist > maxDist )
3312 if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3314 *start = cross[minN];
3329 /* Find line which cross frame by line(a,b,c) */
3330 void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
3332 CvPoint2D32f frameBeg;
3333 CvPoint2D32f frameEnd;
3334 CvPoint2D32f cross[4];
3345 frameEnd.x = (float)(imageSize.width);
3347 haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
3349 frameBeg.x = (float)(imageSize.width);
3351 frameEnd.x = (float)(imageSize.width);
3352 frameEnd.y = (float)(imageSize.height);
3353 haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
3355 frameBeg.x = (float)(imageSize.width);
3356 frameBeg.y = (float)(imageSize.height);
3358 frameEnd.y = (float)(imageSize.height);
3359 haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
3362 frameBeg.y = (float)(imageSize.height);
3365 haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
3368 float minDist = (float)(INT_MAX);
3369 float maxDist = (float)(INT_MIN);
3374 double midPointX = imageSize.width / 2.0;
3375 double midPointY = imageSize.height / 2.0;
3377 for( n = 0; n < 4; n++ )
3379 if( haveCross[n] > 0 )
3381 dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
3382 (midPointY - cross[n].y)*(midPointY - cross[n].y));
3384 if( dist < minDist )
3390 if( dist > maxDist )
3398 if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3400 *start = cross[minN];
3416 int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
3418 double ex1,ey1,ex2,ey2;
3419 double px1,py1,px2,py2;
3421 double delA,delB,delX,delY;
3434 del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3440 delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3441 delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3444 betta = -delB / del;
3446 if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
3451 delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3452 (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3454 delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3455 (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3457 cross->x = (float)( delX / del);
3458 cross->y = (float)(-delY / del);
3463 int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
3465 double ex1,ey1,ex2,ey2;
3466 double px1,py1,px2,py2;
3468 double delA,delB,delX,delY;
3481 del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3487 delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3488 delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3491 betta = -delB / del;
3493 if( alpha < 0 || alpha > 1.0 )
3498 delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3499 (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3501 delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3502 (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3504 cross->x = (float)( delX / del);
3505 cross->y = (float)(-delY / del);
3510 int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
3513 double delX,delY,delA;
3515 double px1,px2,py1,py2;
3524 del = a * (px2 - px1) + b * (py2-py1);
3530 delA = - c - a*px1 - b*py1;
3533 if( alpha < 0 || alpha > 1.0 )
3535 return -1;/* no cross */
3538 delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
3539 delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);
3544 cross->x = (float)X;
3545 cross->y = (float)Y;
3550 int cvComputeEpipoles( CvMatr32f camMatr1, CvMatr32f camMatr2,
3551 CvMatr32f rotMatr1, CvMatr32f rotMatr2,
3552 CvVect32f transVect1,CvVect32f transVect2,
3559 CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1);
3560 CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2);
3561 CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1);
3562 CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2);
3563 CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1);
3564 CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2);
3565 CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1);
3566 CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2);
3569 CvMat cmatrP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1);
3570 CvMat cmatrP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2);
3571 CvMat cvectp1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1);
3572 CvMat cvectp2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2);
3573 CvMat ctmpF1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1);
3574 CvMat ctmpM1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1);
3575 CvMat ctmpM2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2);
3576 CvMat cinvP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1);
3577 CvMat cinvP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2);
3578 CvMat ctmpMatr = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr);
3579 CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1);
3580 CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2);
3581 CvMat cmatrF1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1);
3582 CvMat ctmpF = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF);
3583 CvMat ctmpE1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1);
3584 CvMat ctmpE2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2);
3587 cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
3588 cvmInvert( &cmatrP1,&cinvP1 );
3589 cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );
3591 /* Compute second */
3592 cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
3593 cvmInvert( &cmatrP2,&cinvP2 );
3594 cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );
3596 cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
3597 cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
3598 cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);
3600 cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
3601 cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
3602 cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);
3607 cvmScale(&ctmpE1,&cepipole1,1.0);
3608 cvmScale(&ctmpE2,&cepipole2,1.0);
3620 cvmFree(&ctmpVect1);
3621 cvmFree(&ctmpVect2);
3628 }/* cvComputeEpipoles */
3631 /* Compute epipoles for fundamental matrix */
3632 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
3633 CvPoint3D32f* epipole1,
3634 CvPoint3D32f* epipole2)
3636 /* Decompose fundamental matrix using SVD ( A = U W V') */
3637 CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
3639 CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
3640 CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
3641 CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);
3643 /* From svd we need just last vector of U and V or last row from U' and V' */
3644 /* We get transposed matrixes U and V */
3645 cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T);
3647 /* Get last row from U' and compute epipole1 */
3648 epipole1->x = matrU->data.fl[6];
3649 epipole1->y = matrU->data.fl[7];
3650 epipole1->z = matrU->data.fl[8];
3652 /* Get last row from V' and compute epipole2 */
3653 epipole2->x = matrV->data.fl[6];
3654 epipole2->y = matrV->data.fl[7];
3655 epipole2->z = matrV->data.fl[8];
3657 cvReleaseMat(&matrW);
3658 cvReleaseMat(&matrU);
3659 cvReleaseMat(&matrV);
3663 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
3665 CvMatr32f cameraMatr1,
3666 CvMatr32f cameraMatr2)
3667 {/* Fund = inv(CM1') * Ess * inv(CM2) */
3669 CvMat essMatrC = cvMat(3,3,CV_MAT32F,essMatr);
3670 CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
3671 CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1);
3672 CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2);
3674 CvMat* invCM2 = cvCreateMat(3,3,CV_MAT32F);
3675 CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
3676 CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);
3678 cvTranspose(&cameraMatr1C,tmpMatr);
3679 cvInvert(tmpMatr,invCM1T);
3680 cvmMul(invCM1T,&essMatrC,tmpMatr);
3681 cvInvert(&cameraMatr2C,invCM2);
3682 cvmMul(tmpMatr,invCM2,&fundMatrC);
3684 /* Scale fundamental matrix */
3686 scale = 1.0/fundMatrC.data.fl[8];
3687 cvConvertScale(&fundMatrC,&fundMatrC,scale);
3689 cvReleaseMat(&invCM2);
3690 cvReleaseMat(&tmpMatr);
3691 cvReleaseMat(&invCM1T);
3696 /* Compute essential matrix */
3698 int cvComputeEssentialMatrix( CvMatr32f rotMatr,
3699 CvMatr32f transVect,
3704 /* Make antisymmetric matrix from transpose vector */
3706 transMatr[1] = - transVect[2];
3707 transMatr[2] = transVect[1];
3709 transMatr[3] = transVect[2];
3711 transMatr[5] = - transVect[0];
3713 transMatr[6] = - transVect[1];
3714 transMatr[7] = transVect[0];
3717 icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);