1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
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.
8 //
9 //
10 //                        Intel License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 //   * Redistribution's of source code must retain the above copyright notice,
20 //     this list of conditions and the following disclaimer.
21 //
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.
25 //
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.
28 //
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.
39 //
40 //M*/
41 
42 
43 #include "_cvaux.h"
44 #include "cvtypes.h"
45 #include <float.h>
46 #include <limits.h>
47 #include "cv.h"
48 
49 /* Valery Mosyagin */
50 
51 #undef quad
52 
53 #define EPS64D 1e-9
54 
55 int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
56                                     CvMatr32f transVect,
57                                     CvMatr32f essMatr);
58 
59 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
60                                          CvMatr32f fundMatr,
61                                          CvMatr32f cameraMatr1,
62                                          CvMatr32f cameraMatr2);
63 
64 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
65                                          CvPoint3D32f* epipole1,
66                                          CvPoint3D32f* epipole2);
67 
68 void icvTestPoint( CvPoint2D64d testPoint,
69                 CvVect64d line1,CvVect64d line2,
70                 CvPoint2D64d basePoint,
71                 int* result);
72 
73 
74 
icvGetSymPoint3D(CvPoint3D64d pointCorner,CvPoint3D64d point1,CvPoint3D64d point2,CvPoint3D64d * pointSym2)75 int icvGetSymPoint3D(  CvPoint3D64d pointCorner,
76                             CvPoint3D64d point1,
77                             CvPoint3D64d point2,
78                             CvPoint3D64d *pointSym2)
79 {
80     double len1,len2;
81     double alpha;
82     icvGetPieceLength3D(pointCorner,point1,&len1);
83     if( len1 < EPS64D )
84     {
85         return CV_BADARG_ERR;
86     }
87     icvGetPieceLength3D(pointCorner,point2,&len2);
88     alpha = len2 / len1;
89 
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);
93     return CV_NO_ERR;
94 }
95 
96 /*  author Valery Mosyagin */
97 
98 /* Compute 3D point for scanline and alpha betta */
icvCompute3DPoint(double alpha,double betta,CvStereoLineCoeff * coeffs,CvPoint3D64d * point)99 int icvCompute3DPoint( double alpha,double betta,
100                             CvStereoLineCoeff* coeffs,
101                             CvPoint3D64d* point)
102 {
103 
104     double partX;
105     double partY;
106     double partZ;
107     double partAll;
108     double invPartAll;
109 
110     double alphabetta = alpha*betta;
111 
112     partAll = alpha - betta;
113     if( fabs(partAll) > 0.00001  ) /* alpha must be > betta */
114     {
115 
116         partX   = coeffs->Xcoef        + coeffs->XcoefA *alpha +
117                   coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta;
118 
119         partY   = coeffs->Ycoef        + coeffs->YcoefA *alpha +
120                   coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta;
121 
122         partZ   = coeffs->Zcoef        + coeffs->ZcoefA *alpha +
123                   coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta;
124 
125         invPartAll = 1.0 / partAll;
126 
127         point->x = partX * invPartAll;
128         point->y = partY * invPartAll;
129         point->z = partZ * invPartAll;
130         return CV_NO_ERR;
131     }
132     else
133     {
134         return CV_BADFACTOR_ERR;
135     }
136 }
137 
138 /*--------------------------------------------------------------------------------------*/
139 
140 /* Compute rotate matrix and trans vector for change system */
icvCreateConvertMatrVect(CvMatr64d rotMatr1,CvMatr64d transVect1,CvMatr64d rotMatr2,CvMatr64d transVect2,CvMatr64d convRotMatr,CvMatr64d convTransVect)141 int icvCreateConvertMatrVect( CvMatr64d     rotMatr1,
142                                 CvMatr64d     transVect1,
143                                 CvMatr64d     rotMatr2,
144                                 CvMatr64d     transVect2,
145                                 CvMatr64d     convRotMatr,
146                                 CvMatr64d     convTransVect)
147 {
148     double invRotMatr2[9];
149     double tmpVect[3];
150 
151 
152     icvInvertMatrix_64d(rotMatr2,3,invRotMatr2);
153     /* Test for error */
154 
155     icvMulMatrix_64d(   rotMatr1,
156                         3,3,
157                         invRotMatr2,
158                         3,3,
159                         convRotMatr);
160 
161     icvMulMatrix_64d(   convRotMatr,
162                         3,3,
163                         transVect2,
164                         1,3,
165                         tmpVect);
166 
167     icvSubVector_64d(transVect1,tmpVect,convTransVect,3);
168 
169 
170     return CV_NO_ERR;
171 }
172 
173 /*--------------------------------------------------------------------------------------*/
174 
175 /* Compute point coordinates in other system */
icvConvertPointSystem(CvPoint3D64d M2,CvPoint3D64d * M1,CvMatr64d rotMatr,CvMatr64d transVect)176 int icvConvertPointSystem(CvPoint3D64d  M2,
177                             CvPoint3D64d* M1,
178                             CvMatr64d     rotMatr,
179                             CvMatr64d     transVect
180                             )
181 {
182     double tmpVect[3];
183 
184     icvMulMatrix_64d(   rotMatr,
185                         3,3,
186                         (double*)&M2,
187                         1,3,
188                         tmpVect);
189 
190     icvAddVector_64d(tmpVect,transVect,(double*)M1,3);
191 
192     return CV_NO_ERR;
193 }
194 /*--------------------------------------------------------------------------------------*/
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)195 int icvComputeCoeffForStereoV3( double quad1[4][2],
196                                 double quad2[4][2],
197                                 int    numScanlines,
198                                 CvMatr64d    camMatr1,
199                                 CvMatr64d    rotMatr1,
200                                 CvMatr64d    transVect1,
201                                 CvMatr64d    camMatr2,
202                                 CvMatr64d    rotMatr2,
203                                 CvMatr64d    transVect2,
204                                 CvStereoLineCoeff*    startCoeffs,
205                                 int* needSwapCamera)
206 {
207     /* For each pair */
208     /* In this function we must define position of cameras */
209 
210     CvPoint2D64d point1;
211     CvPoint2D64d point2;
212     CvPoint2D64d point3;
213     CvPoint2D64d point4;
214 
215     int currLine;
216     *needSwapCamera = 0;
217     for( currLine = 0; currLine < numScanlines; currLine++ )
218     {
219         /* Compute points */
220         double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */
221 
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];
224 
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];
227 
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];
230 
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];
233 
234         /* We can compute coeffs for this line */
235         icvComCoeffForLine(    point1,
236                             point2,
237                             point3,
238                             point4,
239                             camMatr1,
240                             rotMatr1,
241                             transVect1,
242                             camMatr2,
243                             rotMatr2,
244                             transVect2,
245                             &startCoeffs[currLine],
246                             needSwapCamera);
247     }
248     return CV_NO_ERR;
249 }
250 /*--------------------------------------------------------------------------------------*/
icvComputeCoeffForStereoNew(double quad1[4][2],double quad2[4][2],int numScanlines,CvMatr32f camMatr1,CvMatr32f rotMatr1,CvMatr32f transVect1,CvMatr32f camMatr2,CvStereoLineCoeff * startCoeffs,int * needSwapCamera)251 int icvComputeCoeffForStereoNew(   double quad1[4][2],
252                                         double quad2[4][2],
253                                         int    numScanlines,
254                                         CvMatr32f    camMatr1,
255                                         CvMatr32f    rotMatr1,
256                                         CvMatr32f    transVect1,
257                                         CvMatr32f    camMatr2,
258                                         CvStereoLineCoeff*    startCoeffs,
259                                         int* needSwapCamera)
260 {
261     /* Convert data */
262 
263     double camMatr1_64d[9];
264     double camMatr2_64d[9];
265 
266     double rotMatr1_64d[9];
267     double transVect1_64d[3];
268 
269     double rotMatr2_64d[9];
270     double transVect2_64d[3];
271 
272     icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
273     icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
274 
275     icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
276     icvCvt_32f_64d(transVect1,transVect1_64d,3);
277 
278     rotMatr2_64d[0] = 1;
279     rotMatr2_64d[1] = 0;
280     rotMatr2_64d[2] = 0;
281     rotMatr2_64d[3] = 0;
282     rotMatr2_64d[4] = 1;
283     rotMatr2_64d[5] = 0;
284     rotMatr2_64d[6] = 0;
285     rotMatr2_64d[7] = 0;
286     rotMatr2_64d[8] = 1;
287 
288     transVect2_64d[0] = 0;
289     transVect2_64d[1] = 0;
290     transVect2_64d[2] = 0;
291 
292     int status = icvComputeCoeffForStereoV3( quad1,
293                                                 quad2,
294                                                 numScanlines,
295                                                 camMatr1_64d,
296                                                 rotMatr1_64d,
297                                                 transVect1_64d,
298                                                 camMatr2_64d,
299                                                 rotMatr2_64d,
300                                                 transVect2_64d,
301                                                 startCoeffs,
302                                                 needSwapCamera);
303 
304 
305     return status;
306 
307 }
308 /*--------------------------------------------------------------------------------------*/
icvComputeCoeffForStereo(CvStereoCamera * stereoCamera)309 int icvComputeCoeffForStereo(  CvStereoCamera* stereoCamera)
310 {
311     double quad1[4][2];
312     double quad2[4][2];
313 
314     int i;
315     for( i = 0; i < 4; i++ )
316     {
317         quad1[i][0] = stereoCamera->quad[0][i].x;
318         quad1[i][1] = stereoCamera->quad[0][i].y;
319 
320         quad2[i][0] = stereoCamera->quad[1][i].x;
321         quad2[i][1] = stereoCamera->quad[1][i].y;
322     }
323 
324     icvComputeCoeffForStereoNew(        quad1,
325                                         quad2,
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));
333     return CV_OK;
334 }
335 
336 
337 
338 /*--------------------------------------------------------------------------------------*/
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)339 int icvComCoeffForLine(   CvPoint2D64d point1,
340                             CvPoint2D64d point2,
341                             CvPoint2D64d point3,
342                             CvPoint2D64d point4,
343                             CvMatr64d    camMatr1,
344                             CvMatr64d    rotMatr1,
345                             CvMatr64d    transVect1,
346                             CvMatr64d    camMatr2,
347                             CvMatr64d    rotMatr2,
348                             CvMatr64d    transVect2,
349                             CvStereoLineCoeff* coeffs,
350                             int* needSwapCamera)
351 {
352     /* Get direction for all points */
353     /* Direction for camera 1 */
354 
355     double direct1[3];
356     double direct2[3];
357     double camPoint1[3];
358 
359     double directS3[3];
360     double directS4[3];
361     double direct3[3];
362     double direct4[3];
363     double camPoint2[3];
364 
365     icvGetDirectionForPoint(   point1,
366                             camMatr1,
367                             (CvPoint3D64d*)direct1);
368 
369     icvGetDirectionForPoint(   point2,
370                             camMatr1,
371                             (CvPoint3D64d*)direct2);
372 
373     /* Direction for camera 2 */
374 
375     icvGetDirectionForPoint(   point3,
376                             camMatr2,
377                             (CvPoint3D64d*)directS3);
378 
379     icvGetDirectionForPoint(   point4,
380                             camMatr2,
381                             (CvPoint3D64d*)directS4);
382 
383     /* Create convertion for camera 2: two direction and camera point */
384 
385     double convRotMatr[9];
386     double convTransVect[3];
387 
388     icvCreateConvertMatrVect(  rotMatr1,
389                             transVect1,
390                             rotMatr2,
391                             transVect2,
392                             convRotMatr,
393                             convTransVect);
394 
395     double zeroVect[3];
396     zeroVect[0] = zeroVect[1] = zeroVect[2] = 0.0;
397     camPoint1[0] = camPoint1[1] = camPoint1[2] = 0.0;
398 
399     icvConvertPointSystem(*((CvPoint3D64d*)directS3),(CvPoint3D64d*)direct3,convRotMatr,convTransVect);
400     icvConvertPointSystem(*((CvPoint3D64d*)directS4),(CvPoint3D64d*)direct4,convRotMatr,convTransVect);
401     icvConvertPointSystem(*((CvPoint3D64d*)zeroVect),(CvPoint3D64d*)camPoint2,convRotMatr,convTransVect);
402 
403     double pointB[3];
404 
405     int postype = 0;
406 
407     /* Changed order */
408     /* Compute point B: xB,yB,zB */
409     icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct2),
410                   *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct3),
411                   (CvPoint3D64d*)pointB);
412 
413     if( pointB[2] < 0 )/* If negative use other lines for cross */
414     {
415         postype = 1;
416         icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct1),
417                       *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct4),
418                       (CvPoint3D64d*)pointB);
419     }
420 
421     CvPoint3D64d pointNewA;
422     CvPoint3D64d pointNewC;
423 
424     pointNewA.x = pointNewA.y = pointNewA.z = 0;
425     pointNewC.x = pointNewC.y = pointNewC.z = 0;
426 
427     if( postype == 0 )
428     {
429         icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint1),
430                             *((CvPoint3D64d*)direct1),
431                             *((CvPoint3D64d*)pointB),
432                             &pointNewA);
433 
434         icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint2),
435                             *((CvPoint3D64d*)direct4),
436                             *((CvPoint3D64d*)pointB),
437                             &pointNewC);
438     }
439     else
440     {/* In this case we must change cameras */
441         *needSwapCamera = 1;
442         icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint2),
443                             *((CvPoint3D64d*)direct3),
444                             *((CvPoint3D64d*)pointB),
445                             &pointNewA);
446 
447         icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint1),
448                             *((CvPoint3D64d*)direct2),
449                             *((CvPoint3D64d*)pointB),
450                             &pointNewC);
451     }
452 
453 
454     double gamma;
455 
456     double x1,y1,z1;
457 
458     x1 = camPoint1[0];
459     y1 = camPoint1[1];
460     z1 = camPoint1[2];
461 
462     double xA,yA,zA;
463     double xB,yB,zB;
464     double xC,yC,zC;
465 
466     xA = pointNewA.x;
467     yA = pointNewA.y;
468     zA = pointNewA.z;
469 
470     xB = pointB[0];
471     yB = pointB[1];
472     zB = pointB[2];
473 
474     xC = pointNewC.x;
475     yC = pointNewC.y;
476     zC = pointNewC.z;
477 
478     double len1,len2;
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) );
481     gamma = len2 / len1;
482 
483     icvComputeStereoLineCoeffs( pointNewA,
484                                 *((CvPoint3D64d*)pointB),
485                                 *((CvPoint3D64d*)camPoint1),
486                                 gamma,
487                                 coeffs);
488 
489     return CV_NO_ERR;
490 }
491 
492 
493 /*--------------------------------------------------------------------------------------*/
494 
icvGetDirectionForPoint(CvPoint2D64d point,CvMatr64d camMatr,CvPoint3D64d * direct)495 int icvGetDirectionForPoint(  CvPoint2D64d point,
496                                 CvMatr64d camMatr,
497                                 CvPoint3D64d* direct)
498 {
499     /*  */
500     double invMatr[9];
501 
502     /* Invert matrix */
503 
504     icvInvertMatrix_64d(camMatr,3,invMatr);
505     /* TEST FOR ERRORS */
506 
507     double vect[3];
508     vect[0] = point.x;
509     vect[1] = point.y;
510     vect[2] = 1;
511 
512     /* Mul matr */
513     icvMulMatrix_64d(   invMatr,
514                         3,3,
515                         vect,
516                         1,3,
517                         (double*)direct);
518 
519     return CV_NO_ERR;
520 }
521 
522 /*--------------------------------------------------------------------------------------*/
523 
icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,CvPoint3D64d point21,CvPoint3D64d point22,CvPoint3D64d * midPoint)524 int icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,
525                        CvPoint3D64d point21,CvPoint3D64d point22,
526                        CvPoint3D64d* midPoint)
527 {
528     double xM,yM,zM;
529     double xN,yN,zN;
530 
531     double xA,yA,zA;
532     double xB,yB,zB;
533 
534     double xC,yC,zC;
535     double xD,yD,zD;
536 
537     xA = point11.x;
538     yA = point11.y;
539     zA = point11.z;
540 
541     xB = point12.x;
542     yB = point12.y;
543     zB = point12.z;
544 
545     xC = point21.x;
546     yC = point21.y;
547     zC = point21.z;
548 
549     xD = point22.x;
550     yD = point22.y;
551     zD = point22.z;
552 
553     double a11,a12,a21,a22;
554     double b1,b2;
555 
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) );
562 
563     double delta;
564     double deltaA,deltaB;
565     double alpha,betta;
566 
567     delta  = a11*a22-a12*a21;
568 
569     if( fabs(delta) < EPS64D )
570     {
571         /*return ERROR;*/
572     }
573 
574     deltaA = b1*a22-b2*a12;
575     deltaB = a11*b2-b1*a21;
576 
577     alpha = deltaA / delta;
578     betta = deltaB / delta;
579 
580     xM = xA+alpha*(xB-xA);
581     yM = yA+alpha*(yB-yA);
582     zM = zA+alpha*(zB-zA);
583 
584     xN = xC+betta*(xD-xC);
585     yN = yC+betta*(yD-yC);
586     zN = zC+betta*(zD-zC);
587 
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;
592 
593     return CV_NO_ERR;
594 }
595 
596 /*--------------------------------------------------------------------------------------*/
597 
icvComputeStereoLineCoeffs(CvPoint3D64d pointA,CvPoint3D64d pointB,CvPoint3D64d pointCam1,double gamma,CvStereoLineCoeff * coeffs)598 int icvComputeStereoLineCoeffs(   CvPoint3D64d pointA,
599                                     CvPoint3D64d pointB,
600                                     CvPoint3D64d pointCam1,
601                                     double gamma,
602                                     CvStereoLineCoeff*    coeffs)
603 {
604     double x1,y1,z1;
605 
606     x1 = pointCam1.x;
607     y1 = pointCam1.y;
608     z1 = pointCam1.z;
609 
610     double xA,yA,zA;
611     double xB,yB,zB;
612 
613     xA = pointA.x;
614     yA = pointA.y;
615     zA = pointA.z;
616 
617     xB = pointB.x;
618     yB = pointB.y;
619     zB = pointB.z;
620 
621     if( gamma > 0 )
622     {
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;
627 
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;
632 
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;
637     }
638     else
639     {
640         gamma = - gamma;
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);
645 
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);
650 
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);
655     }
656 
657 
658 
659     return CV_NO_ERR;
660 }
661 /*--------------------------------------------------------------------------------------*/
662 
663 
664 /*---------------------------------------------------------------------------------------*/
665 
666 /* This function get minimum angle started at point which contains rect */
icvGetAngleLine(CvPoint2D64d startPoint,CvSize imageSize,CvPoint2D64d * point1,CvPoint2D64d * point2)667 int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2)
668 {
669     /* Get crosslines with image corners */
670 
671     /* Find four lines */
672 
673     CvPoint2D64d pa,pb,pc,pd;
674 
675     pa.x = 0;
676     pa.y = 0;
677 
678     pb.x = imageSize.width-1;
679     pb.y = 0;
680 
681     pd.x = imageSize.width-1;
682     pd.y = imageSize.height-1;
683 
684     pc.x = 0;
685     pc.y = imageSize.height-1;
686 
687     /* We can compute points for angle */
688     /* Test for place section */
689 
690     if( startPoint.x < 0 )
691     {/* 1,4,7 */
692         if( startPoint.y < 0)
693         {/* 1 */
694             *point1 = pb;
695             *point2 = pc;
696         }
697         else if( startPoint.y > imageSize.height-1 )
698         {/* 7 */
699             *point1 = pa;
700             *point2 = pd;
701         }
702         else
703         {/* 4 */
704             *point1 = pa;
705             *point2 = pc;
706         }
707     }
708     else if ( startPoint.x > imageSize.width-1 )
709     {/* 3,6,9 */
710         if( startPoint.y < 0 )
711         {/* 3 */
712             *point1 = pa;
713             *point2 = pd;
714         }
715         else if ( startPoint.y > imageSize.height-1 )
716         {/* 9 */
717             *point1 = pb;
718             *point2 = pc;
719         }
720         else
721         {/* 6 */
722             *point1 = pb;
723             *point2 = pd;
724         }
725     }
726     else
727     {/* 2,5,8 */
728         if( startPoint.y < 0 )
729         {/* 2 */
730             if( startPoint.x < imageSize.width/2 )
731             {
732                 *point1 = pb;
733                 *point2 = pa;
734             }
735             else
736             {
737                 *point1 = pa;
738                 *point2 = pb;
739             }
740         }
741         else if( startPoint.y > imageSize.height-1 )
742         {/* 8 */
743             if( startPoint.x < imageSize.width/2 )
744             {
745                 *point1 = pc;
746                 *point2 = pd;
747             }
748             else
749             {
750                 *point1 = pd;
751                 *point2 = pc;
752             }
753         }
754         else
755         {/* 5 - point in the image */
756             return 2;
757         }
758     }
759     return 0;
760 }/* GetAngleLine */
761 
762 /*---------------------------------------------------------------------------------------*/
763 
icvGetCoefForPiece(CvPoint2D64d p_start,CvPoint2D64d p_end,double * a,double * b,double * c,int * result)764 void icvGetCoefForPiece(   CvPoint2D64d p_start,CvPoint2D64d p_end,
765                         double *a,double *b,double *c,
766                         int* result)
767 {
768     double det;
769     double detA,detB,detC;
770 
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 */
773     {
774         *result = 0;
775         return;
776     }
777 
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;
781 
782     double invDet = 1.0 / det;
783     *a = detA * invDet;
784     *b = detB * invDet;
785     *c = detC * invDet;
786 
787     *result = 1;
788     return;
789 }
790 
791 /*---------------------------------------------------------------------------------------*/
792 
793 /* Get common area of rectifying */
icvGetCommonArea(CvSize imageSize,CvPoint3D64d epipole1,CvPoint3D64d epipole2,CvMatr64d fundMatr,CvVect64d coeff11,CvVect64d coeff12,CvVect64d coeff21,CvVect64d coeff22,int * result)794 void icvGetCommonArea( CvSize imageSize,
795                     CvPoint3D64d epipole1,CvPoint3D64d epipole2,
796                     CvMatr64d fundMatr,
797                     CvVect64d coeff11,CvVect64d coeff12,
798                     CvVect64d coeff21,CvVect64d coeff22,
799                     int* result)
800 {
801     int res = 0;
802     CvPoint2D64d point11;
803     CvPoint2D64d point12;
804     CvPoint2D64d point21;
805     CvPoint2D64d point22;
806 
807     double corr11[3];
808     double corr12[3];
809     double corr21[3];
810     double corr22[3];
811 
812     double pointW11[3];
813     double pointW12[3];
814     double pointW21[3];
815     double pointW22[3];
816 
817     double transFundMatr[3*3];
818     /* Compute transpose of fundamental matrix */
819     icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );
820 
821     CvPoint2D64d epipole1_2d;
822     CvPoint2D64d epipole2_2d;
823 
824     if( fabs(epipole1.z) < 1e-8 )
825     {/* epipole1 in infinity */
826         *result = 0;
827         return;
828     }
829     epipole1_2d.x = epipole1.x / epipole1.z;
830     epipole1_2d.y = epipole1.y / epipole1.z;
831 
832     if( fabs(epipole2.z) < 1e-8 )
833     {/* epipole2 in infinity */
834         *result = 0;
835         return;
836     }
837     epipole2_2d.x = epipole2.x / epipole2.z;
838     epipole2_2d.y = epipole2.y / epipole2.z;
839 
840     int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12);
841     if( stat == 2 )
842     {
843         /* No angle */
844         *result = 0;
845         return;
846     }
847 
848     stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22);
849     if( stat == 2 )
850     {
851         /* No angle */
852         *result = 0;
853         return;
854     }
855 
856     /* ============= Computation for line 1 ================ */
857     /* Find correspondence line for angle points11 */
858     /* corr21 = Fund'*p1 */
859 
860     pointW11[0] = point11.x;
861     pointW11[1] = point11.y;
862     pointW11[2] = 1.0;
863 
864     icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
865                             pointW11,
866                             corr21,
867                             3,3);
868 
869     /* Find crossing of line with image 2 */
870     CvPoint2D64d start;
871     CvPoint2D64d end;
872     icvGetCrossRectDirect( imageSize,
873                         corr21[0],corr21[1],corr21[2],
874                         &start,&end,
875                         &res);
876 
877     if( res == 0 )
878     {/* We have not cross */
879         /* We must define new angle */
880 
881         pointW21[0] = point21.x;
882         pointW21[1] = point21.y;
883         pointW21[2] = 1.0;
884 
885         /* Find correspondence line for this angle points */
886         /* We know point and try to get corr line */
887         /* For point21 */
888         /* corr11 = Fund * p21 */
889 
890         icvTransformVector_64d( fundMatr, /* !!! Modified */
891                                 pointW21,
892                                 corr11,
893                                 3,3);
894 
895         /* We have cross. And it's result cross for up line. Set result coefs */
896 
897         /* Set coefs for line 1 image 1 */
898         coeff11[0] = corr11[0];
899         coeff11[1] = corr11[1];
900         coeff11[2] = corr11[2];
901 
902         /* Set coefs for line 1 image 2 */
903         icvGetCoefForPiece(    epipole2_2d,point21,
904                             &coeff21[0],&coeff21[1],&coeff21[2],
905                             &res);
906         if( res == 0 )
907         {
908             *result = 0;
909             return;/* Error */
910         }
911     }
912     else
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],
917                             &res);
918         if( res == 0 )
919         {
920             *result = 0;
921             return;/* Error */
922         }
923 
924         /* Set coefs for line 1 image 2 */
925         coeff21[0] = corr21[0];
926         coeff21[1] = corr21[1];
927         coeff21[2] = corr21[2];
928 
929     }
930 
931     /* ============= Computation for line 2 ================ */
932     /* Find correspondence line for angle points11 */
933     /* corr22 = Fund*p2 */
934 
935     pointW12[0] = point12.x;
936     pointW12[1] = point12.y;
937     pointW12[2] = 1.0;
938 
939     icvTransformVector_64d( transFundMatr,
940                             pointW12,
941                             corr22,
942                             3,3);
943 
944     /* Find crossing of line with image 2 */
945     icvGetCrossRectDirect( imageSize,
946                         corr22[0],corr22[1],corr22[2],
947                         &start,&end,
948                         &res);
949 
950     if( res == 0 )
951     {/* We have not cross */
952         /* We must define new angle */
953 
954         pointW22[0] = point22.x;
955         pointW22[1] = point22.y;
956         pointW22[2] = 1.0;
957 
958         /* Find correspondence line for this angle points */
959         /* We know point and try to get corr line */
960         /* For point21 */
961         /* corr2 = Fund' * p1 */
962 
963         icvTransformVector_64d( fundMatr,
964                                 pointW22,
965                                 corr12,
966                                 3,3);
967 
968 
969         /* We have cross. And it's result cross for down line. Set result coefs */
970 
971         /* Set coefs for line 2 image 1 */
972         coeff12[0] = corr12[0];
973         coeff12[1] = corr12[1];
974         coeff12[2] = corr12[2];
975 
976         /* Set coefs for line 1 image 2 */
977         icvGetCoefForPiece(    epipole2_2d,point22,
978                             &coeff22[0],&coeff22[1],&coeff22[2],
979                             &res);
980         if( res == 0 )
981         {
982             *result = 0;
983             return;/* Error */
984         }
985     }
986     else
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],
991                             &res);
992         if( res == 0 )
993         {
994             *result = 0;
995             return;/* Error */
996         }
997 
998         /* Set coefs for line 1 image 2 */
999         coeff22[0] = corr22[0];
1000         coeff22[1] = corr22[1];
1001         coeff22[2] = corr22[2];
1002 
1003     }
1004 
1005     /* Now we know common area */
1006 
1007     return;
1008 
1009 }/* GetCommonArea */
1010 
1011 /*---------------------------------------------------------------------------------------*/
1012 
1013 /* Get cross for direction1 and direction2 */
1014 /*  Result = 1 - cross */
1015 /*  Result = 2 - parallel and not equal */
1016 /*  Result = 3 - parallel and equal */
1017 
icvGetCrossDirectDirect(CvVect64d direct1,CvVect64d direct2,CvPoint2D64d * cross,int * result)1018 void icvGetCrossDirectDirect(  CvVect64d direct1,CvVect64d direct2,
1019                             CvPoint2D64d *cross,int* result)
1020 {
1021     double det  = direct1[0]*direct2[1] - direct2[0]*direct1[1];
1022     double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2];
1023 
1024     if( fabs(det) > EPS64D )
1025     {/* Have cross */
1026         cross->x = detx/det;
1027         cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det;
1028         *result = 1;
1029     }
1030     else
1031     {/* may be parallel */
1032         if( fabs(detx) > EPS64D )
1033         {/* parallel and not equal */
1034             *result = 2;
1035         }
1036         else
1037         {/* equals */
1038             *result = 3;
1039         }
1040     }
1041 
1042     return;
1043 }
1044 
1045 /*---------------------------------------------------------------------------------------*/
1046 
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 */
1052 
icvGetCrossPieceDirect(CvPoint2D64d p_start,CvPoint2D64d p_end,double a,double b,double c,CvPoint2D64d * cross,int * result)1053 void icvGetCrossPieceDirect(   CvPoint2D64d p_start,CvPoint2D64d p_end,
1054                             double a,double b,double c,
1055                             CvPoint2D64d *cross,int* result)
1056 {
1057 
1058     if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 )
1059     {/* Have cross */
1060         double det;
1061         double detxc,detyc;
1062 
1063         det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
1064 
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 */
1069                 *result = 3;
1070                 return;
1071             }
1072             else
1073             {
1074                 *result = 2;
1075             }
1076             return;
1077         }
1078 
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);
1081 
1082         cross->x = detxc / det;
1083         cross->y = detyc / det;
1084         *result = 1;
1085 
1086     }
1087     else
1088     {
1089         *result = 0;
1090     }
1091     return;
1092 }
1093 /*--------------------------------------------------------------------------------------*/
1094 
icvGetCrossPiecePiece(CvPoint2D64d p1_start,CvPoint2D64d p1_end,CvPoint2D64d p2_start,CvPoint2D64d p2_end,CvPoint2D64d * cross,int * result)1095 void icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end,
1096                             CvPoint2D64d p2_start,CvPoint2D64d p2_end,
1097                             CvPoint2D64d* cross,
1098                             int* result)
1099 {
1100     double ex1,ey1,ex2,ey2;
1101     double px1,py1,px2,py2;
1102     double del;
1103     double delA,delB,delX,delY;
1104     double alpha,betta;
1105 
1106     ex1 = p1_start.x;
1107     ey1 = p1_start.y;
1108     ex2 = p1_end.x;
1109     ey2 = p1_end.y;
1110 
1111     px1 = p2_start.x;
1112     py1 = p2_start.y;
1113     px2 = p2_end.x;
1114     py2 = p2_end.y;
1115 
1116     del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2);
1117     if( fabs(del) <= EPS64D )
1118     {/* May be they are parallel !!! */
1119         *result = 0;
1120         return;
1121     }
1122 
1123     delA =  (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1);
1124     delB =  (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1);
1125 
1126     alpha = delA / del;
1127     betta = delB / del;
1128 
1129     if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
1130     {
1131         *result = 0;
1132         return;
1133     }
1134 
1135     delX =  (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
1136             (ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2));
1137 
1138     delY =  (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
1139             (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2));
1140 
1141     cross->x = delX / del;
1142     cross->y = delY / del;
1143 
1144     *result = 1;
1145     return;
1146 }
1147 
1148 
1149 /*---------------------------------------------------------------------------------------*/
1150 
icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double * dist)1151 void icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist)
1152 {
1153     double dx = point2.x - point1.x;
1154     double dy = point2.y - point1.y;
1155     *dist = sqrt( dx*dx + dy*dy );
1156     return;
1157 }
1158 
1159 /*---------------------------------------------------------------------------------------*/
1160 
icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double * dist)1161 void icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist)
1162 {
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 );
1167     return;
1168 }
1169 
1170 /*---------------------------------------------------------------------------------------*/
1171 
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 */
icvGetCrossRectDirect(CvSize imageSize,double a,double b,double c,CvPoint2D64d * start,CvPoint2D64d * end,int * result)1174 void icvGetCrossRectDirect(    CvSize imageSize,
1175                             double a,double b,double c,
1176                             CvPoint2D64d *start,CvPoint2D64d *end,
1177                             int* result)
1178 {
1179     CvPoint2D64d frameBeg;
1180     CvPoint2D64d frameEnd;
1181     CvPoint2D64d cross[4];
1182     int     haveCross[4];
1183 
1184     haveCross[0] = 0;
1185     haveCross[1] = 0;
1186     haveCross[2] = 0;
1187     haveCross[3] = 0;
1188 
1189     frameBeg.x = 0;
1190     frameBeg.y = 0;
1191     frameEnd.x = imageSize.width;
1192     frameEnd.y = 0;
1193 
1194     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]);
1195 
1196     frameBeg.x = imageSize.width;
1197     frameBeg.y = 0;
1198     frameEnd.x = imageSize.width;
1199     frameEnd.y = imageSize.height;
1200     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]);
1201 
1202     frameBeg.x = imageSize.width;
1203     frameBeg.y = imageSize.height;
1204     frameEnd.x = 0;
1205     frameEnd.y = imageSize.height;
1206     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]);
1207 
1208     frameBeg.x = 0;
1209     frameBeg.y = imageSize.height;
1210     frameEnd.x = 0;
1211     frameEnd.y = 0;
1212     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]);
1213 
1214     double maxDist;
1215 
1216     int maxI=0,maxJ=0;
1217 
1218 
1219     int i,j;
1220 
1221     maxDist = -1.0;
1222 
1223     double distance;
1224 
1225     for( i = 0; i < 3; i++ )
1226     {
1227         if( haveCross[i] == 1 )
1228         {
1229             for( j = i + 1; j < 4; j++ )
1230             {
1231                 if( haveCross[j] == 1)
1232                 {/* Compute dist */
1233                     icvGetPieceLength(cross[i],cross[j],&distance);
1234                     if( distance > maxDist )
1235                     {
1236                         maxI = i;
1237                         maxJ = j;
1238                         maxDist = distance;
1239                     }
1240                 }
1241             }
1242         }
1243     }
1244 
1245     if( maxDist >= 0 )
1246     {/* We have cross */
1247         *start = cross[maxI];
1248         *result = 1;
1249         if( maxDist > 0 )
1250         {
1251             *end   = cross[maxJ];
1252             *result = 2;
1253         }
1254     }
1255     else
1256     {
1257         *result = 0;
1258     }
1259 
1260     return;
1261 }/* GetCrossRectDirect */
1262 
1263 /*---------------------------------------------------------------------------------------*/
icvProjectPointToImage(CvPoint3D64d point,CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,CvPoint2D64d * projPoint)1264 void icvProjectPointToImage(   CvPoint3D64d point,
1265                             CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,
1266                             CvPoint2D64d* projPoint)
1267 {
1268 
1269     double tmpVect1[3];
1270     double tmpVect2[3];
1271 
1272     icvMulMatrix_64d (  rotMatr,
1273                         3,3,
1274                         (double*)&point,
1275                         1,3,
1276                         tmpVect1);
1277 
1278     icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3);
1279 
1280     icvMulMatrix_64d (  camMatr,
1281                         3,3,
1282                         tmpVect2,
1283                         1,3,
1284                         tmpVect1);
1285 
1286     projPoint->x = tmpVect1[0] / tmpVect1[2];
1287     projPoint->y = tmpVect1[1] / tmpVect1[2];
1288 
1289     return;
1290 }
1291 
1292 /*---------------------------------------------------------------------------------------*/
1293 /* Get quads for transform images */
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)1294 void icvGetQuadsTransform(
1295                           CvSize        imageSize,
1296                         CvMatr64d     camMatr1,
1297                         CvMatr64d     rotMatr1,
1298                         CvVect64d     transVect1,
1299                         CvMatr64d     camMatr2,
1300                         CvMatr64d     rotMatr2,
1301                         CvVect64d     transVect2,
1302                         CvSize*       warpSize,
1303                         double quad1[4][2],
1304                         double quad2[4][2],
1305                         CvMatr64d     fundMatr,
1306                         CvPoint3D64d* epipole1,
1307                         CvPoint3D64d* epipole2
1308                         )
1309 {
1310     /* First compute fundamental matrix and epipoles */
1311     int res;
1312 
1313 
1314     /* Compute epipoles and fundamental matrix using new functions */
1315     {
1316         double convRotMatr[9];
1317         double convTransVect[3];
1318 
1319         icvCreateConvertMatrVect( rotMatr1,
1320                                   transVect1,
1321                                   rotMatr2,
1322                                   transVect2,
1323                                   convRotMatr,
1324                                   convTransVect);
1325         float convRotMatr_32f[9];
1326         float convTransVect_32f[3];
1327 
1328         icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9);
1329         icvCvt_64d_32f(convTransVect,convTransVect_32f,3);
1330 
1331         /* We know R and t */
1332         /* Compute essential matrix */
1333         float essMatr[9];
1334         float fundMatr_32f[9];
1335 
1336         float camMatr1_32f[9];
1337         float camMatr2_32f[9];
1338 
1339         icvCvt_64d_32f(camMatr1,camMatr1_32f,9);
1340         icvCvt_64d_32f(camMatr2,camMatr2_32f,9);
1341 
1342         cvComputeEssentialMatrix(   convRotMatr_32f,
1343                                     convTransVect_32f,
1344                                     essMatr);
1345 
1346         cvConvertEssential2Fundamental( essMatr,
1347                                         fundMatr_32f,
1348                                         camMatr1_32f,
1349                                         camMatr2_32f);
1350 
1351         CvPoint3D32f epipole1_32f;
1352         CvPoint3D32f epipole2_32f;
1353 
1354         cvComputeEpipolesFromFundMatrix( fundMatr_32f,
1355                                          &epipole1_32f,
1356                                          &epipole2_32f);
1357         /* copy to 64d epipoles */
1358         epipole1->x = epipole1_32f.x;
1359         epipole1->y = epipole1_32f.y;
1360         epipole1->z = epipole1_32f.z;
1361 
1362         epipole2->x = epipole2_32f.x;
1363         epipole2->y = epipole2_32f.y;
1364         epipole2->z = epipole2_32f.z;
1365 
1366         /* Convert fundamental matrix */
1367         icvCvt_32f_64d(fundMatr_32f,fundMatr,9);
1368     }
1369 
1370     double coeff11[3];
1371     double coeff12[3];
1372     double coeff21[3];
1373     double coeff22[3];
1374 
1375     icvGetCommonArea(   imageSize,
1376                         *epipole1,*epipole2,
1377                         fundMatr,
1378                         coeff11,coeff12,
1379                         coeff21,coeff22,
1380                         &res);
1381 
1382     CvPoint2D64d point11, point12,point21, point22;
1383     double width1,width2;
1384     double height1,height2;
1385     double tmpHeight1,tmpHeight2;
1386 
1387     CvPoint2D64d epipole1_2d;
1388     CvPoint2D64d epipole2_2d;
1389 
1390     /* ----- Image 1 ----- */
1391     if( fabs(epipole1->z) < 1e-8 )
1392     {
1393         return;
1394     }
1395     epipole1_2d.x = epipole1->x / epipole1->z;
1396     epipole1_2d.y = epipole1->y / epipole1->z;
1397 
1398     icvGetCutPiece( coeff11,coeff12,
1399                 epipole1_2d,
1400                 imageSize,
1401                 &point11,&point12,
1402                 &point21,&point22,
1403                 &res);
1404 
1405     /* Compute distance */
1406     icvGetPieceLength(point11,point21,&width1);
1407     icvGetPieceLength(point11,point12,&tmpHeight1);
1408     icvGetPieceLength(point21,point22,&tmpHeight2);
1409     height1 = MAX(tmpHeight1,tmpHeight2);
1410 
1411     quad1[0][0] = point11.x;
1412     quad1[0][1] = point11.y;
1413 
1414     quad1[1][0] = point21.x;
1415     quad1[1][1] = point21.y;
1416 
1417     quad1[2][0] = point22.x;
1418     quad1[2][1] = point22.y;
1419 
1420     quad1[3][0] = point12.x;
1421     quad1[3][1] = point12.y;
1422 
1423     /* ----- Image 2 ----- */
1424     if( fabs(epipole2->z) < 1e-8 )
1425     {
1426         return;
1427     }
1428     epipole2_2d.x = epipole2->x / epipole2->z;
1429     epipole2_2d.y = epipole2->y / epipole2->z;
1430 
1431     icvGetCutPiece( coeff21,coeff22,
1432                 epipole2_2d,
1433                 imageSize,
1434                 &point11,&point12,
1435                 &point21,&point22,
1436                 &res);
1437 
1438     /* Compute distance */
1439     icvGetPieceLength(point11,point21,&width2);
1440     icvGetPieceLength(point11,point12,&tmpHeight1);
1441     icvGetPieceLength(point21,point22,&tmpHeight2);
1442     height2 = MAX(tmpHeight1,tmpHeight2);
1443 
1444     quad2[0][0] = point11.x;
1445     quad2[0][1] = point11.y;
1446 
1447     quad2[1][0] = point21.x;
1448     quad2[1][1] = point21.y;
1449 
1450     quad2[2][0] = point22.x;
1451     quad2[2][1] = point22.y;
1452 
1453     quad2[3][0] = point12.x;
1454     quad2[3][1] = point12.y;
1455 
1456 
1457     /*=======================================================*/
1458     /* This is a new additional way to compute quads. */
1459     /* We must correct quads */
1460     {
1461         double convRotMatr[9];
1462         double convTransVect[3];
1463 
1464         double newQuad1[4][2];
1465         double newQuad2[4][2];
1466 
1467 
1468         icvCreateConvertMatrVect( rotMatr1,
1469                                   transVect1,
1470                                   rotMatr2,
1471                                   transVect2,
1472                                   convRotMatr,
1473                                   convTransVect);
1474 
1475         /* -------------Compute for first image-------------- */
1476         CvPoint2D32f pointb1;
1477         CvPoint2D32f pointe1;
1478 
1479         CvPoint2D32f pointb2;
1480         CvPoint2D32f pointe2;
1481 
1482         pointb1.x = (float)quad1[0][0];
1483         pointb1.y = (float)quad1[0][1];
1484 
1485         pointe1.x = (float)quad1[3][0];
1486         pointe1.y = (float)quad1[3][1];
1487 
1488         icvComputeeInfiniteProject1(convRotMatr,
1489                                     camMatr1,
1490                                     camMatr2,
1491                                     pointb1,
1492                                     &pointb2);
1493 
1494         icvComputeeInfiniteProject1(convRotMatr,
1495                                     camMatr1,
1496                                     camMatr2,
1497                                     pointe1,
1498                                     &pointe2);
1499 
1500         /*  JUST TEST FOR POINT */
1501 
1502         /* Compute distances */
1503         double dxOld,dyOld;
1504         double dxNew,dyNew;
1505         double distOld,distNew;
1506 
1507         dxOld = quad2[1][0] - quad2[0][0];
1508         dyOld = quad2[1][1] - quad2[0][1];
1509         distOld = dxOld*dxOld + dyOld*dyOld;
1510 
1511         dxNew = quad2[1][0] - pointb2.x;
1512         dyNew = quad2[1][1] - pointb2.y;
1513         distNew = dxNew*dxNew + dyNew*dyNew;
1514 
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];
1525         }
1526         else
1527         {/* Get new points for first quad */
1528 
1529             pointb2.x = (float)quad2[0][0];
1530             pointb2.y = (float)quad2[0][1];
1531 
1532             pointe2.x = (float)quad2[3][0];
1533             pointe2.y = (float)quad2[3][1];
1534 
1535             icvComputeeInfiniteProject2(convRotMatr,
1536                                         camMatr1,
1537                                         camMatr2,
1538                                         &pointb1,
1539                                         pointb2);
1540 
1541             icvComputeeInfiniteProject2(convRotMatr,
1542                                         camMatr1,
1543                                         camMatr2,
1544                                         &pointe1,
1545                                         pointe2);
1546 
1547 
1548             /*  JUST TEST FOR POINT */
1549 
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];
1554 
1555             newQuad1[0][0] = pointb1.x;
1556             newQuad1[0][1] = pointb1.y;
1557             newQuad1[3][0] = pointe1.x;
1558             newQuad1[3][1] = pointe1.y;
1559         }
1560 
1561         /* -------------Compute for second image-------------- */
1562         pointb1.x = (float)quad1[1][0];
1563         pointb1.y = (float)quad1[1][1];
1564 
1565         pointe1.x = (float)quad1[2][0];
1566         pointe1.y = (float)quad1[2][1];
1567 
1568         icvComputeeInfiniteProject1(convRotMatr,
1569                                     camMatr1,
1570                                     camMatr2,
1571                                     pointb1,
1572                                     &pointb2);
1573 
1574         icvComputeeInfiniteProject1(convRotMatr,
1575                                     camMatr1,
1576                                     camMatr2,
1577                                     pointe1,
1578                                     &pointe2);
1579 
1580         /* Compute distances */
1581 
1582         dxOld = quad2[0][0] - quad2[1][0];
1583         dyOld = quad2[0][1] - quad2[1][1];
1584         distOld = dxOld*dxOld + dyOld*dyOld;
1585 
1586         dxNew = quad2[0][0] - pointb2.x;
1587         dyNew = quad2[0][1] - pointb2.y;
1588         distNew = dxNew*dxNew + dyNew*dyNew;
1589 
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];
1600         }
1601         else
1602         {/* Get new points for first quad */
1603 
1604             pointb2.x = (float)quad2[1][0];
1605             pointb2.y = (float)quad2[1][1];
1606 
1607             pointe2.x = (float)quad2[2][0];
1608             pointe2.y = (float)quad2[2][1];
1609 
1610             icvComputeeInfiniteProject2(convRotMatr,
1611                                         camMatr1,
1612                                         camMatr2,
1613                                         &pointb1,
1614                                         pointb2);
1615 
1616             icvComputeeInfiniteProject2(convRotMatr,
1617                                         camMatr1,
1618                                         camMatr2,
1619                                         &pointe1,
1620                                         pointe2);
1621 
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];
1626 
1627             newQuad1[1][0] = pointb1.x;
1628             newQuad1[1][1] = pointb1.y;
1629             newQuad1[2][0] = pointe1.x;
1630             newQuad1[2][1] = pointe1.y;
1631         }
1632 
1633 
1634 
1635 /*-------------------------------------------------------------------------------*/
1636 
1637         /* Copy new quads to old quad */
1638         int i;
1639         for( i = 0; i < 4; i++ )
1640         {
1641             {
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];
1646             }
1647         }
1648     }
1649     /*=======================================================*/
1650 
1651     double warpWidth,warpHeight;
1652 
1653     warpWidth  = MAX(width1,width2);
1654     warpHeight = MAX(height1,height2);
1655 
1656     warpSize->width  = (int)warpWidth;
1657     warpSize->height = (int)warpHeight;
1658 
1659     warpSize->width  = cvRound(warpWidth-1);
1660     warpSize->height = cvRound(warpHeight-1);
1661 
1662 /* !!! by Valery Mosyagin. this lines added just for test no warp */
1663     warpSize->width  = imageSize.width;
1664     warpSize->height = imageSize.height;
1665 
1666     return;
1667 }
1668 
1669 
1670 /*---------------------------------------------------------------------------------------*/
1671 
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)1672 void icvGetQuadsTransformNew(  CvSize        imageSize,
1673                             CvMatr32f     camMatr1,
1674                             CvMatr32f     camMatr2,
1675                             CvMatr32f     rotMatr1,
1676                             CvVect32f     transVect1,
1677                             CvSize*       warpSize,
1678                             double        quad1[4][2],
1679                             double        quad2[4][2],
1680                             CvMatr32f     fundMatr,
1681                             CvPoint3D32f* epipole1,
1682                             CvPoint3D32f* epipole2
1683                         )
1684 {
1685     /* Convert data */
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;
1696 
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);
1701 
1702     /* Create vector and matrix */
1703 
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;
1713 
1714     transVect2_64d[0] = 0;
1715     transVect2_64d[1] = 0;
1716     transVect2_64d[2] = 0;
1717 
1718     icvGetQuadsTransform(   imageSize,
1719                             camMatr1_64d,
1720                             rotMatr1_64d,
1721                             transVect1_64d,
1722                             camMatr2_64d,
1723                             rotMatr2_64d,
1724                             transVect2_64d,
1725                             warpSize,
1726                             quad1,
1727                             quad2,
1728                             fundMatr_64d,
1729                             &epipole1_64d,
1730                             &epipole2_64d
1731                         );
1732 
1733     /* Convert epipoles */
1734     epipole1->x = (float)(epipole1_64d.x);
1735     epipole1->y = (float)(epipole1_64d.y);
1736     epipole1->z = (float)(epipole1_64d.z);
1737 
1738     epipole2->x = (float)(epipole2_64d.x);
1739     epipole2->y = (float)(epipole2_64d.y);
1740     epipole2->z = (float)(epipole2_64d.z);
1741 
1742     /* Convert fundamental matrix */
1743     icvCvt_64d_32f(fundMatr_64d,fundMatr,9);
1744 
1745     return;
1746 }
1747 
1748 /*---------------------------------------------------------------------------------------*/
icvGetQuadsTransformStruct(CvStereoCamera * stereoCamera)1749 void icvGetQuadsTransformStruct(  CvStereoCamera* stereoCamera)
1750 {
1751     /* Wrapper for icvGetQuadsTransformNew */
1752 
1753 
1754     double  quad1[4][2];
1755     double  quad2[4][2];
1756 
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),
1763                             quad1,
1764                             quad2,
1765                             stereoCamera->fundMatr,
1766                             &(stereoCamera->epipole[0]),
1767                             &(stereoCamera->epipole[1])
1768                         );
1769 
1770     int i;
1771     for( i = 0; i < 4; i++ )
1772     {
1773         stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]);
1774         stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]);
1775     }
1776 
1777     return;
1778 }
1779 
1780 /*---------------------------------------------------------------------------------------*/
icvComputeStereoParamsForCameras(CvStereoCamera * stereoCamera)1781 void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
1782 {
1783     /* For given intrinsic and extrinsic parameters computes rest parameters
1784     **   such as fundamental matrix. warping coeffs, epipoles, ...
1785     */
1786 
1787 
1788     /* compute rotate matrix and translate vector */
1789     double rotMatr1[9];
1790     double rotMatr2[9];
1791 
1792     double transVect1[3];
1793     double transVect2[3];
1794 
1795     double convRotMatr[9];
1796     double convTransVect[3];
1797 
1798     /* fill matrices */
1799     icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9);
1800     icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9);
1801 
1802     icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3);
1803     icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3);
1804 
1805     icvCreateConvertMatrVect(   rotMatr1,
1806                                 transVect1,
1807                                 rotMatr2,
1808                                 transVect2,
1809                                 convRotMatr,
1810                                 convTransVect);
1811 
1812     /* copy to stereo camera params */
1813     icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9);
1814     icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3);
1815 
1816 
1817     icvGetQuadsTransformStruct(stereoCamera);
1818     icvComputeRestStereoParams(stereoCamera);
1819 }
1820 
1821 
1822 
1823 /*---------------------------------------------------------------------------------------*/
1824 
1825 /* Get cut line for one image */
icvGetCutPiece(CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,CvPoint2D64d epipole,CvSize imageSize,CvPoint2D64d * point11,CvPoint2D64d * point12,CvPoint2D64d * point21,CvPoint2D64d * point22,int * result)1826 void icvGetCutPiece(   CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,
1827                     CvPoint2D64d epipole,
1828                     CvSize imageSize,
1829                     CvPoint2D64d* point11,CvPoint2D64d* point12,
1830                     CvPoint2D64d* point21,CvPoint2D64d* point22,
1831                     int* result)
1832 {
1833     /* Compute nearest cut line to epipole */
1834     /* Get corners inside sector */
1835     /* Collect all candidate point */
1836 
1837     CvPoint2D64d candPoints[8];
1838     CvPoint2D64d midPoint;
1839     int numPoints = 0;
1840     int res;
1841     int i;
1842 
1843     double cutLine1[3];
1844     double cutLine2[3];
1845 
1846     /* Find middle line of sector */
1847     double midLine[3];
1848 
1849 
1850     /* Different way  */
1851     CvPoint2D64d pointOnLine1;  pointOnLine1.x = pointOnLine1.y = 0;
1852     CvPoint2D64d pointOnLine2;  pointOnLine2.x = pointOnLine2.y = 0;
1853 
1854     CvPoint2D64d start1,end1;
1855 
1856     icvGetCrossRectDirect( imageSize,
1857                         areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
1858                         &start1,&end1,&res);
1859     if( res > 0 )
1860     {
1861         pointOnLine1 = start1;
1862     }
1863 
1864     icvGetCrossRectDirect( imageSize,
1865                         areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
1866                         &start1,&end1,&res);
1867     if( res > 0 )
1868     {
1869         pointOnLine2 = start1;
1870     }
1871 
1872     icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);
1873 
1874     icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);
1875 
1876     /* Test corner points */
1877     CvPoint2D64d cornerPoint;
1878     CvPoint2D64d tmpPoints[2];
1879 
1880     cornerPoint.x = 0;
1881     cornerPoint.y = 0;
1882     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1883     if( res == 1 )
1884     {/* Add point */
1885         candPoints[numPoints] = cornerPoint;
1886         numPoints++;
1887     }
1888 
1889     cornerPoint.x = imageSize.width;
1890     cornerPoint.y = 0;
1891     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1892     if( res == 1 )
1893     {/* Add point */
1894         candPoints[numPoints] = cornerPoint;
1895         numPoints++;
1896     }
1897 
1898     cornerPoint.x = imageSize.width;
1899     cornerPoint.y = imageSize.height;
1900     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1901     if( res == 1 )
1902     {/* Add point */
1903         candPoints[numPoints] = cornerPoint;
1904         numPoints++;
1905     }
1906 
1907     cornerPoint.x = 0;
1908     cornerPoint.y = imageSize.height;
1909     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1910     if( res == 1 )
1911     {/* Add point */
1912         candPoints[numPoints] = cornerPoint;
1913         numPoints++;
1914     }
1915 
1916     /* Find cross line 1 with image border */
1917     icvGetCrossRectDirect( imageSize,
1918                         areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
1919                         &tmpPoints[0], &tmpPoints[1],
1920                         &res);
1921     for( i = 0; i < res; i++ )
1922     {
1923         candPoints[numPoints++] = tmpPoints[i];
1924     }
1925 
1926     /* Find cross line 2 with image border */
1927     icvGetCrossRectDirect( imageSize,
1928                         areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
1929                         &tmpPoints[0], &tmpPoints[1],
1930                         &res);
1931 
1932     for( i = 0; i < res; i++ )
1933     {
1934         candPoints[numPoints++] = tmpPoints[i];
1935     }
1936 
1937     if( numPoints < 2 )
1938     {
1939         *result = 0;
1940         return;/* Error. Not enought points */
1941     }
1942     /* Project all points to middle line and get max and min */
1943 
1944     CvPoint2D64d projPoint;
1945     CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
1946     CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;
1947 
1948 
1949     double dist;
1950     double maxDist = 0;
1951     double minDist = 10000000;
1952 
1953 
1954     for( i = 0; i < numPoints; i++ )
1955     {
1956         icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
1957         icvGetPieceLength(epipole,projPoint,&dist);
1958         if( dist < minDist)
1959         {
1960             minDist = dist;
1961             minPoint = projPoint;
1962         }
1963 
1964         if( dist > maxDist)
1965         {
1966             maxDist = dist;
1967             maxPoint = projPoint;
1968         }
1969     }
1970 
1971     /* We know maximum and minimum points. Now we can compute cut lines */
1972 
1973     icvGetNormalDirect(midLine,minPoint,cutLine1);
1974     icvGetNormalDirect(midLine,maxPoint,cutLine2);
1975 
1976     /* Test for begin of line. */
1977     CvPoint2D64d tmpPoint2;
1978 
1979     /* Get cross with */
1980     icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
1981     icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);
1982 
1983     icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
1984     icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);
1985 
1986     if( epipole.x > imageSize.width * 0.5 )
1987     {/* Need to change points */
1988         tmpPoint2 = *point11;
1989         *point11 = *point21;
1990         *point21 = tmpPoint2;
1991 
1992         tmpPoint2 = *point12;
1993         *point12 = *point22;
1994         *point22 = tmpPoint2;
1995     }
1996 
1997     return;
1998 }
1999 /*---------------------------------------------------------------------------------------*/
2000 /* Get middle angle */
icvGetMiddleAnglePoint(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2,CvPoint2D64d * midPoint)2001 void icvGetMiddleAnglePoint(   CvPoint2D64d basePoint,
2002                             CvPoint2D64d point1,CvPoint2D64d point2,
2003                             CvPoint2D64d* midPoint)
2004 {/* !!! May be need to return error */
2005 
2006     double dist1;
2007     double dist2;
2008     icvGetPieceLength(basePoint,point1,&dist1);
2009     icvGetPieceLength(basePoint,point2,&dist2);
2010     CvPoint2D64d pointNew1;
2011     CvPoint2D64d pointNew2;
2012     double alpha = dist2/dist1;
2013 
2014     pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
2015     pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );
2016 
2017     pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
2018     pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );
2019 
2020     int res;
2021     icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);
2022 
2023     return;
2024 }
2025 
2026 /*---------------------------------------------------------------------------------------*/
2027 /* Get normal direct to direct in line */
icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)2028 void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
2029 {
2030     normDirect[0] =   direct[1];
2031     normDirect[1] = - direct[0];
2032     normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);
2033     return;
2034 }
2035 
2036 /*---------------------------------------------------------------------------------------*/
icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)2037 CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
2038 {
2039     return  (point1.x - basePoint.x)*(point2.y - basePoint.y) -
2040             (point2.x - basePoint.x)*(point1.y - basePoint.y);
2041 }
2042 /*---------------------------------------------------------------------------------------*/
2043 /* Test for point in sector           */
2044 /* Return 0 - point not inside sector */
2045 /* Return 1 - point inside sector     */
icvTestPoint(CvPoint2D64d testPoint,CvVect64d line1,CvVect64d line2,CvPoint2D64d basePoint,int * result)2046 void icvTestPoint( CvPoint2D64d testPoint,
2047                 CvVect64d line1,CvVect64d line2,
2048                 CvPoint2D64d basePoint,
2049                 int* result)
2050 {
2051     CvPoint2D64d point1,point2;
2052 
2053     icvProjectPointToDirect(testPoint,line1,&point1);
2054     icvProjectPointToDirect(testPoint,line2,&point2);
2055 
2056     double sign1 = icvGetVect(basePoint,point1,point2);
2057     double sign2 = icvGetVect(basePoint,point1,testPoint);
2058     if( sign1 * sign2 > 0 )
2059     {/* Correct for first line */
2060         sign1 = - sign1;
2061         sign2 = icvGetVect(basePoint,point2,testPoint);
2062         if( sign1 * sign2 > 0 )
2063         {/* Correct for both lines */
2064             *result = 1;
2065         }
2066         else
2067         {
2068             *result = 0;
2069         }
2070     }
2071     else
2072     {
2073         *result = 0;
2074     }
2075 
2076     return;
2077 }
2078 
2079 /*---------------------------------------------------------------------------------------*/
2080 /* Project point to line */
icvProjectPointToDirect(CvPoint2D64d point,CvVect64d lineCoeff,CvPoint2D64d * projectPoint)2081 void icvProjectPointToDirect(  CvPoint2D64d point,CvVect64d lineCoeff,
2082                             CvPoint2D64d* projectPoint)
2083 {
2084     double a = lineCoeff[0];
2085     double b = lineCoeff[1];
2086 
2087     double det =  1.0 / ( a*a + b*b );
2088     double delta =  a*point.y - b*point.x;
2089 
2090     projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
2091     projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;
2092 
2093     return;
2094 }
2095 
2096 /*---------------------------------------------------------------------------------------*/
2097 /* Get distance from point to direction */
icvGetDistanceFromPointToDirect(CvPoint2D64d point,CvVect64d lineCoef,double * dist)2098 void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
2099 {
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);
2105     return;
2106 }
2107 /*---------------------------------------------------------------------------------------*/
2108 
icvCreateIsometricImage(IplImage * src,IplImage * dst,int desired_depth,int desired_num_channels)2109 CV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
2110                                        int desired_depth, int desired_num_channels )
2111 {
2112     CvSize src_size ;
2113     src_size.width = src->width;
2114     src_size.height = src->height;
2115 
2116     CvSize dst_size = src_size;
2117 
2118     if( dst )
2119     {
2120         dst_size.width = dst->width;
2121         dst_size.height = dst->height;
2122     }
2123 
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 )
2128     {
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 );
2133 
2134     }
2135 
2136     return dst;
2137 }
2138 
2139 int
icvCvt_32f_64d(float * src,double * dst,int size)2140 icvCvt_32f_64d( float *src, double *dst, int size )
2141 {
2142     int t;
2143 
2144     if( !src || !dst )
2145         return CV_NULLPTR_ERR;
2146     if( size <= 0 )
2147         return CV_BADRANGE_ERR;
2148 
2149     for( t = 0; t < size; t++ )
2150     {
2151         dst[t] = (double) (src[t]);
2152     }
2153 
2154     return CV_OK;
2155 }
2156 
2157 /*======================================================================================*/
2158 /* Type conversion double -> float */
2159 int
icvCvt_64d_32f(double * src,float * dst,int size)2160 icvCvt_64d_32f( double *src, float *dst, int size )
2161 {
2162     int t;
2163 
2164     if( !src || !dst )
2165         return CV_NULLPTR_ERR;
2166     if( size <= 0 )
2167         return CV_BADRANGE_ERR;
2168 
2169     for( t = 0; t < size; t++ )
2170     {
2171         dst[t] = (float) (src[t]);
2172     }
2173 
2174     return CV_OK;
2175 }
2176 
2177 /*----------------------------------------------------------------------------------*/
2178 
2179 
2180 /* Find line which cross frame by line(a,b,c) */
FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f * start,CvPoint2D32f * end,int * result)2181 void FindLineForEpiline(    CvSize imageSize,
2182                             float a,float b,float c,
2183                             CvPoint2D32f *start,CvPoint2D32f *end,
2184                             int* result)
2185 {
2186     result = result;
2187     CvPoint2D32f frameBeg;
2188 
2189     CvPoint2D32f frameEnd;
2190     CvPoint2D32f cross[4];
2191     int     haveCross[4];
2192     float   dist;
2193 
2194     haveCross[0] = 0;
2195     haveCross[1] = 0;
2196     haveCross[2] = 0;
2197     haveCross[3] = 0;
2198 
2199     frameBeg.x = 0;
2200     frameBeg.y = 0;
2201     frameEnd.x = (float)(imageSize.width);
2202     frameEnd.y = 0;
2203     haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
2204 
2205     frameBeg.x = (float)(imageSize.width);
2206     frameBeg.y = 0;
2207     frameEnd.x = (float)(imageSize.width);
2208     frameEnd.y = (float)(imageSize.height);
2209     haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
2210 
2211     frameBeg.x = (float)(imageSize.width);
2212     frameBeg.y = (float)(imageSize.height);
2213     frameEnd.x = 0;
2214     frameEnd.y = (float)(imageSize.height);
2215     haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
2216 
2217     frameBeg.x = 0;
2218     frameBeg.y = (float)(imageSize.height);
2219     frameEnd.x = 0;
2220     frameEnd.y = 0;
2221     haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
2222 
2223     int n;
2224     float minDist = (float)(INT_MAX);
2225     float maxDist = (float)(INT_MIN);
2226 
2227     int maxN = -1;
2228     int minN = -1;
2229 
2230     double midPointX = imageSize.width  / 2.0;
2231     double midPointY = imageSize.height / 2.0;
2232 
2233     for( n = 0; n < 4; n++ )
2234     {
2235         if( haveCross[n] > 0 )
2236         {
2237             dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
2238                             (midPointY - cross[n].y)*(midPointY - cross[n].y));
2239 
2240             if( dist < minDist )
2241             {
2242                 minDist = dist;
2243                 minN = n;
2244             }
2245 
2246             if( dist > maxDist )
2247             {
2248                 maxDist = dist;
2249                 maxN = n;
2250             }
2251         }
2252     }
2253 
2254     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
2255     {
2256         *start = cross[minN];
2257         *end   = cross[maxN];
2258     }
2259     else
2260     {
2261         start->x = 0;
2262         start->y = 0;
2263         end->x = 0;
2264         end->y = 0;
2265     }
2266 
2267     return;
2268 
2269 }
2270 
2271 
2272 /*----------------------------------------------------------------------------------*/
2273 
GetAngleLinee(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)2274 int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
2275 {
2276     float width  = (float)(imageSize.width);
2277     float height = (float)(imageSize.height);
2278 
2279     /* Get crosslines with image corners */
2280 
2281     /* Find four lines */
2282 
2283     CvPoint2D32f pa,pb,pc,pd;
2284 
2285     pa.x = 0;
2286     pa.y = 0;
2287 
2288     pb.x = width;
2289     pb.y = 0;
2290 
2291     pd.x = width;
2292     pd.y = height;
2293 
2294     pc.x = 0;
2295     pc.y = height;
2296 
2297     /* We can compute points for angle */
2298     /* Test for place section */
2299 
2300     float x,y;
2301     x = epipole.x;
2302     y = epipole.y;
2303 
2304     if( x < 0 )
2305     {/* 1,4,7 */
2306         if( y < 0)
2307         {/* 1 */
2308             point1 = pb;
2309             point2 = pc;
2310         }
2311         else if( y > height )
2312         {/* 7 */
2313             point1 = pa;
2314             point2 = pd;
2315         }
2316         else
2317         {/* 4 */
2318             point1 = pa;
2319             point2 = pc;
2320         }
2321     }
2322     else if ( x > width )
2323     {/* 3,6,9 */
2324         if( y < 0 )
2325         {/* 3 */
2326             point1 = pa;
2327             point2 = pd;
2328         }
2329         else if ( y > height )
2330         {/* 9 */
2331             point1 = pc;
2332             point2 = pb;
2333         }
2334         else
2335         {/* 6 */
2336             point1 = pb;
2337             point2 = pd;
2338         }
2339     }
2340     else
2341     {/* 2,5,8 */
2342         if( y < 0 )
2343         {/* 2 */
2344             point1 = pa;
2345             point2 = pb;
2346         }
2347         else if( y > height )
2348         {/* 8 */
2349             point1 = pc;
2350             point2 = pd;
2351         }
2352         else
2353         {/* 5 - point in the image */
2354             return 2;
2355         }
2356 
2357 
2358     }
2359 
2360 
2361     return 0;
2362 }
2363 
2364 /*--------------------------------------------------------------------------------------*/
icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])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 */
2367 
2368 
2369     CV_FUNCNAME( "icvComputePerspectiveCoeffs" );
2370 
2371     __BEGIN__;
2372 
2373     double A[64];
2374     double b[8];
2375     double c[8];
2376     CvPoint2D32f pt[4];
2377     int i;
2378 
2379     pt[0] = srcQuad[0];
2380     pt[1] = srcQuad[1];
2381     pt[2] = srcQuad[2];
2382     pt[3] = srcQuad[3];
2383 
2384     for( i = 0; i < 4; i++ )
2385     {
2386 #if 0
2387         double x = dstQuad[i].x;
2388         double y = dstQuad[i].y;
2389         double X = pt[i].x;
2390         double Y = pt[i].y;
2391 #else
2392         double x = pt[i].x;
2393         double y = pt[i].y;
2394         double X = dstQuad[i].x;
2395         double Y = dstQuad[i].y;
2396 #endif
2397         double* a = A + i*16;
2398 
2399         a[0] = x;
2400         a[1] = y;
2401         a[2] = 1;
2402         a[3] = 0;
2403         a[4] = 0;
2404         a[5] = 0;
2405         a[6] = -X*x;
2406         a[7] = -X*y;
2407 
2408         a += 8;
2409 
2410         a[0] = 0;
2411         a[1] = 0;
2412         a[2] = 0;
2413         a[3] = x;
2414         a[4] = y;
2415         a[5] = 1;
2416         a[6] = -Y*x;
2417         a[7] = -Y*y;
2418 
2419         b[i*2] = X;
2420         b[i*2 + 1] = Y;
2421     }
2422 
2423     {
2424     double invA[64];
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 );
2429 
2430     CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2431     CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2432     }
2433 
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];
2442     coeffs[2][2] = 1.0;
2443 
2444     __END__;
2445 
2446     return;
2447 }
2448 
2449 /*--------------------------------------------------------------------------------------*/
2450 
cvComputePerspectiveMap(const double c[3][3],CvArr * rectMapX,CvArr * rectMapY)2451 CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY )
2452 {
2453     CV_FUNCNAME( "cvComputePerspectiveMap" );
2454 
2455     __BEGIN__;
2456 
2457     CvSize size;
2458     CvMat  stubx, *mapx = (CvMat*)rectMapX;
2459     CvMat  stuby, *mapy = (CvMat*)rectMapY;
2460     int i, j;
2461 
2462     CV_CALL( mapx = cvGetMat( mapx, &stubx ));
2463     CV_CALL( mapy = cvGetMat( mapy, &stuby ));
2464 
2465     if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 )
2466         CV_ERROR( CV_StsUnsupportedFormat, "" );
2467 
2468     size = cvGetMatSize(mapx);
2469     assert( fabs(c[2][2] - 1.) < FLT_EPSILON );
2470 
2471     for( i = 0; i < size.height; i++ )
2472     {
2473         float* mx = (float*)(mapx->data.ptr + mapx->step*i);
2474         float* my = (float*)(mapy->data.ptr + mapy->step*i);
2475 
2476         for( j = 0; j < size.width; j++ )
2477         {
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;
2481 
2482             mx[j] = (float)x;
2483             my[j] = (float)y;
2484         }
2485     }
2486 
2487     __END__;
2488 }
2489 
2490 /*--------------------------------------------------------------------------------------*/
2491 
cvInitPerspectiveTransform(CvSize size,const CvPoint2D32f quad[4],double matrix[3][3],CvArr * rectMap)2492 CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
2493                                               CvArr* rectMap )
2494 {
2495     /* Computes Perspective Transform coeffs and map if need
2496         for given image size and given result quad */
2497     CV_FUNCNAME( "cvInitPerspectiveTransform" );
2498 
2499     __BEGIN__;
2500 
2501     double A[64];
2502     double b[8];
2503     double c[8];
2504     CvPoint2D32f pt[4];
2505     CvMat  mapstub, *map = (CvMat*)rectMap;
2506     int i, j;
2507 
2508     if( map )
2509     {
2510         CV_CALL( map = cvGetMat( map, &mapstub ));
2511 
2512         if( CV_MAT_TYPE( map->type ) != CV_32FC2 )
2513             CV_ERROR( CV_StsUnsupportedFormat, "" );
2514 
2515         if( map->width != size.width || map->height != size.height )
2516             CV_ERROR( CV_StsUnmatchedSizes, "" );
2517     }
2518 
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 );
2523 
2524     for( i = 0; i < 4; i++ )
2525     {
2526 #if 0
2527         double x = quad[i].x;
2528         double y = quad[i].y;
2529         double X = pt[i].x;
2530         double Y = pt[i].y;
2531 #else
2532         double x = pt[i].x;
2533         double y = pt[i].y;
2534         double X = quad[i].x;
2535         double Y = quad[i].y;
2536 #endif
2537         double* a = A + i*16;
2538 
2539         a[0] = x;
2540         a[1] = y;
2541         a[2] = 1;
2542         a[3] = 0;
2543         a[4] = 0;
2544         a[5] = 0;
2545         a[6] = -X*x;
2546         a[7] = -X*y;
2547 
2548         a += 8;
2549 
2550         a[0] = 0;
2551         a[1] = 0;
2552         a[2] = 0;
2553         a[3] = x;
2554         a[4] = y;
2555         a[5] = 1;
2556         a[6] = -Y*x;
2557         a[7] = -Y*y;
2558 
2559         b[i*2] = X;
2560         b[i*2 + 1] = Y;
2561     }
2562 
2563     {
2564     double invA[64];
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 );
2569 
2570     CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2571     CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2572     }
2573 
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];
2582     matrix[2][2] = 1.0;
2583 
2584     if( map )
2585     {
2586         for( i = 0; i < size.height; i++ )
2587         {
2588             CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i);
2589             for( j = 0; j < size.width; j++ )
2590             {
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;
2594 
2595                 maprow[j].x = (float)x;
2596                 maprow[j].y = (float)y;
2597             }
2598         }
2599     }
2600 
2601     __END__;
2602 
2603     return;
2604 }
2605 
2606 
2607 /*-----------------------------------------------------------------------*/
2608 /* Compute projected infinite point for second image if first image point is known */
icvComputeeInfiniteProject1(CvMatr64d rotMatr,CvMatr64d camMatr1,CvMatr64d camMatr2,CvPoint2D32f point1,CvPoint2D32f * point2)2609 void icvComputeeInfiniteProject1(   CvMatr64d     rotMatr,
2610                                     CvMatr64d     camMatr1,
2611                                     CvMatr64d     camMatr2,
2612                                     CvPoint2D32f  point1,
2613                                     CvPoint2D32f* point2)
2614 {
2615     double invMatr1[9];
2616     icvInvertMatrix_64d(camMatr1,3,invMatr1);
2617     double P1[3];
2618     double p1[3];
2619     p1[0] = (double)(point1.x);
2620     p1[1] = (double)(point1.y);
2621     p1[2] = 1;
2622 
2623     icvMulMatrix_64d(   invMatr1,
2624                         3,3,
2625                         p1,
2626                         1,3,
2627                         P1);
2628 
2629     double invR[9];
2630     icvTransposeMatrix_64d( rotMatr, 3, 3, invR );
2631 
2632     /* Change system 1 to system 2 */
2633     double P2[3];
2634     icvMulMatrix_64d(   invR,
2635                         3,3,
2636                         P1,
2637                         1,3,
2638                         P2);
2639 
2640     /* Now we can project this point to image 2 */
2641     double projP[3];
2642 
2643     icvMulMatrix_64d(   camMatr2,
2644                         3,3,
2645                         P2,
2646                         1,3,
2647                         projP);
2648 
2649     point2->x = (float)(projP[0] / projP[2]);
2650     point2->y = (float)(projP[1] / projP[2]);
2651 
2652     return;
2653 }
2654 
2655 /*-----------------------------------------------------------------------*/
2656 /* Compute projected infinite point for second image if first image point is known */
icvComputeeInfiniteProject2(CvMatr64d rotMatr,CvMatr64d camMatr1,CvMatr64d camMatr2,CvPoint2D32f * point1,CvPoint2D32f point2)2657 void icvComputeeInfiniteProject2(   CvMatr64d     rotMatr,
2658                                     CvMatr64d     camMatr1,
2659                                     CvMatr64d     camMatr2,
2660                                     CvPoint2D32f*  point1,
2661                                     CvPoint2D32f point2)
2662 {
2663     double invMatr2[9];
2664     icvInvertMatrix_64d(camMatr2,3,invMatr2);
2665     double P2[3];
2666     double p2[3];
2667     p2[0] = (double)(point2.x);
2668     p2[1] = (double)(point2.y);
2669     p2[2] = 1;
2670 
2671     icvMulMatrix_64d(   invMatr2,
2672                         3,3,
2673                         p2,
2674                         1,3,
2675                         P2);
2676 
2677     /* Change system 1 to system 2 */
2678 
2679     double P1[3];
2680     icvMulMatrix_64d(   rotMatr,
2681                         3,3,
2682                         P2,
2683                         1,3,
2684                         P1);
2685 
2686     /* Now we can project this point to image 2 */
2687     double projP[3];
2688 
2689     icvMulMatrix_64d(   camMatr1,
2690                         3,3,
2691                         P1,
2692                         1,3,
2693                         projP);
2694 
2695     point1->x = (float)(projP[0] / projP[2]);
2696     point1->y = (float)(projP[1] / projP[2]);
2697 
2698     return;
2699 }
2700 
2701 /* Select best R and t for given cameras, points, ... */
2702 /* For both cameras */
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)2703 int icvSelectBestRt(           int           numImages,
2704                                     int*          numPoints,
2705                                     CvPoint2D32f* imagePoints1,
2706                                     CvPoint2D32f* imagePoints2,
2707                                     CvPoint3D32f* objectPoints,
2708 
2709                                     CvMatr32f     cameraMatrix1,
2710                                     CvVect32f     distortion1,
2711                                     CvMatr32f     rotMatrs1,
2712                                     CvVect32f     transVects1,
2713 
2714                                     CvMatr32f     cameraMatrix2,
2715                                     CvVect32f     distortion2,
2716                                     CvMatr32f     rotMatrs2,
2717                                     CvVect32f     transVects2,
2718 
2719                                     CvMatr32f     bestRotMatr,
2720                                     CvVect32f     bestTransVect
2721                                     )
2722 {
2723 
2724     /* Need to convert input data 32 -> 64 */
2725     CvPoint3D64d* objectPoints_64d;
2726 
2727     double* rotMatrs1_64d;
2728     double* rotMatrs2_64d;
2729 
2730     double* transVects1_64d;
2731     double* transVects2_64d;
2732 
2733     double cameraMatrix1_64d[9];
2734     double cameraMatrix2_64d[9];
2735 
2736     double distortion1_64d[4];
2737     double distortion2_64d[4];
2738 
2739     /* allocate memory for 64d data */
2740     int totalNum = 0;
2741 
2742     int i;
2743     for( i = 0; i < numImages; i++ )
2744     {
2745         totalNum += numPoints[i];
2746     }
2747 
2748     objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));
2749 
2750     rotMatrs1_64d    = (double*)calloc(numImages,sizeof(double)*9);
2751     rotMatrs2_64d    = (double*)calloc(numImages,sizeof(double)*9);
2752 
2753     transVects1_64d  = (double*)calloc(numImages,sizeof(double)*3);
2754     transVects2_64d  = (double*)calloc(numImages,sizeof(double)*3);
2755 
2756     /* Convert input data to 64d */
2757 
2758     icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d,  totalNum*3);
2759 
2760     icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d,  numImages*9);
2761     icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d,  numImages*9);
2762 
2763     icvCvt_32f_64d(transVects1, transVects1_64d,  numImages*3);
2764     icvCvt_32f_64d(transVects2, transVects2_64d,  numImages*3);
2765 
2766     /* Convert to arrays */
2767     icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
2768     icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
2769 
2770     icvCvt_32f_64d(distortion1, distortion1_64d, 4);
2771     icvCvt_32f_64d(distortion2, distortion2_64d, 4);
2772 
2773 
2774     /* for each R and t compute error for image pair */
2775     float* errors;
2776 
2777     errors = (float*)calloc(numImages*numImages,sizeof(float));
2778     if( errors == 0 )
2779     {
2780         return CV_OUTOFMEM_ERR;
2781     }
2782 
2783     int currImagePair;
2784     int currRt;
2785     for( currRt = 0; currRt < numImages; currRt++ )
2786     {
2787         int begPoint = 0;
2788         for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2789         {
2790             /* For current R,t R,t compute relative position of cameras */
2791 
2792             double convRotMatr[9];
2793             double convTransVect[3];
2794 
2795             icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
2796                                       transVects1_64d + currRt*3,
2797                                       rotMatrs2_64d + currRt*9,
2798                                       transVects2_64d + currRt*3,
2799                                       convRotMatr,
2800                                       convTransVect);
2801 
2802             /* Project points using relative position of cameras */
2803 
2804             double convRotMatr2[9];
2805             double convTransVect2[3];
2806 
2807             convRotMatr2[0] = 1;
2808             convRotMatr2[1] = 0;
2809             convRotMatr2[2] = 0;
2810 
2811             convRotMatr2[3] = 0;
2812             convRotMatr2[4] = 1;
2813             convRotMatr2[5] = 0;
2814 
2815             convRotMatr2[6] = 0;
2816             convRotMatr2[7] = 0;
2817             convRotMatr2[8] = 1;
2818 
2819             convTransVect2[0] = 0;
2820             convTransVect2[1] = 0;
2821             convTransVect2[2] = 0;
2822 
2823             /* Compute error for given pair and Rt */
2824             /* We must project points to image and compute error */
2825 
2826             CvPoint2D64d* projImagePoints1;
2827             CvPoint2D64d* projImagePoints2;
2828 
2829             CvPoint3D64d* points1;
2830             CvPoint3D64d* points2;
2831 
2832             int numberPnt;
2833             numberPnt = numPoints[currImagePair];
2834             projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2835             projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2836 
2837             points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2838             points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2839 
2840             /* Transform object points to first camera position */
2841             int i;
2842             for( i = 0; i < numberPnt; i++ )
2843             {
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);
2849 
2850                 icvConvertPointSystem(  tmpPoint,
2851                                         points2+i,
2852                                         rotMatrs2_64d + currImagePair*9,
2853                                         transVects2_64d + currImagePair*3);
2854 
2855                 /* Create first camera point using R, t */
2856                 icvConvertPointSystem(  points2[i],
2857                                         points1+i,
2858                                         convRotMatr,
2859                                         convTransVect);
2860 
2861                 CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
2862                 icvConvertPointSystem(  tmpPoint,
2863                                         &tmpPoint2,
2864                                         rotMatrs1_64d + currImagePair*9,
2865                                         transVects1_64d + currImagePair*3);
2866                 double err;
2867                 double dx,dy,dz;
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);
2872 
2873 
2874             }
2875 
2876 #if 0
2877             cvProjectPointsSimple(  numPoints[currImagePair],
2878                                     objectPoints_64d + begPoint,
2879                                     rotMatrs1_64d + currRt*9,
2880                                     transVects1_64d + currRt*3,
2881                                     cameraMatrix1_64d,
2882                                     distortion1_64d,
2883                                     projImagePoints1);
2884 
2885             cvProjectPointsSimple(  numPoints[currImagePair],
2886                                     objectPoints_64d + begPoint,
2887                                     rotMatrs2_64d + currRt*9,
2888                                     transVects2_64d + currRt*3,
2889                                     cameraMatrix2_64d,
2890                                     distortion2_64d,
2891                                     projImagePoints2);
2892 #endif
2893 
2894             /* Project with no translate and no rotation */
2895 
2896 #if 0
2897             {
2898                 double nodist[4] = {0,0,0,0};
2899                 cvProjectPointsSimple(  numPoints[currImagePair],
2900                                         points1,
2901                                         convRotMatr2,
2902                                         convTransVect2,
2903                                         cameraMatrix1_64d,
2904                                         nodist,
2905                                         projImagePoints1);
2906 
2907                 cvProjectPointsSimple(  numPoints[currImagePair],
2908                                         points2,
2909                                         convRotMatr2,
2910                                         convTransVect2,
2911                                         cameraMatrix2_64d,
2912                                         nodist,
2913                                         projImagePoints2);
2914 
2915             }
2916 #endif
2917 
2918             cvProjectPointsSimple(  numPoints[currImagePair],
2919                                     points1,
2920                                     convRotMatr2,
2921                                     convTransVect2,
2922                                     cameraMatrix1_64d,
2923                                     distortion1_64d,
2924                                     projImagePoints1);
2925 
2926             cvProjectPointsSimple(  numPoints[currImagePair],
2927                                     points2,
2928                                     convRotMatr2,
2929                                     convTransVect2,
2930                                     cameraMatrix2_64d,
2931                                     distortion2_64d,
2932                                     projImagePoints2);
2933 
2934             /* points are projected. Compute error */
2935 
2936             int currPoint;
2937             double err1 = 0;
2938             double err2 = 0;
2939             double err;
2940             for( currPoint = 0; currPoint < numberPnt; currPoint++ )
2941             {
2942                 double len1,len2;
2943                 double dx1,dy1;
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);
2947                 err1 += len1;
2948 
2949                 double dx2,dy2;
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);
2953                 err2 += len2;
2954             }
2955 
2956             err1 /= (float)(numberPnt);
2957             err2 /= (float)(numberPnt);
2958 
2959             err = (err1+err2) * 0.5;
2960             begPoint += numberPnt;
2961 
2962             /* Set this error to */
2963             errors[numImages*currImagePair+currRt] = (float)err;
2964 
2965             free(points1);
2966             free(points2);
2967             free(projImagePoints1);
2968             free(projImagePoints2);
2969         }
2970     }
2971 
2972     /* Just select R and t with minimal average error */
2973 
2974     int bestnumRt = 0;
2975     float minError = 0;/* Just for no warnings. Uses 'first' flag. */
2976     int first = 1;
2977     for( currRt = 0; currRt < numImages; currRt++ )
2978     {
2979         float avErr = 0;
2980         for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2981         {
2982             avErr += errors[numImages*currImagePair+currRt];
2983         }
2984         avErr /= (float)(numImages);
2985 
2986         if( first )
2987         {
2988             bestnumRt = 0;
2989             minError = avErr;
2990             first = 0;
2991         }
2992         else
2993         {
2994             if( avErr < minError )
2995             {
2996                 bestnumRt = currRt;
2997                 minError = avErr;
2998             }
2999         }
3000 
3001     }
3002 
3003     double bestRotMatr_64d[9];
3004     double bestTransVect_64d[3];
3005 
3006     icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
3007                               transVects1_64d + bestnumRt * 3,
3008                               rotMatrs2_64d + bestnumRt * 9,
3009                               transVects2_64d + bestnumRt * 3,
3010                               bestRotMatr_64d,
3011                               bestTransVect_64d);
3012 
3013     icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
3014     icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);
3015 
3016 
3017     free(errors);
3018 
3019     return CV_OK;
3020 }
3021 
3022 
3023 /* ----------------- Stereo calibration functions --------------------- */
3024 
icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)3025 float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
3026 {
3027     float ax = point2.x - point1.x;
3028     float ay = point2.y - point1.y;
3029 
3030     float bx = point.x - point1.x;
3031     float by = point.y - point1.y;
3032 
3033     return (ax*by - ay*bx);
3034 }
3035 
3036 /* Convert function for stereo warping */
icvConvertWarpCoordinates(double coeffs[3][3],CvPoint2D32f * cameraPoint,CvPoint2D32f * warpPoint,int direction)3037 int icvConvertWarpCoordinates(double coeffs[3][3],
3038                                 CvPoint2D32f* cameraPoint,
3039                                 CvPoint2D32f* warpPoint,
3040                                 int direction)
3041 {
3042     double x,y;
3043     double det;
3044     if( direction == CV_WARP_TO_CAMERA )
3045     {/* convert from camera image to warped image coordinates */
3046         x = warpPoint->x;
3047         y = warpPoint->y;
3048 
3049         det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
3050         if( fabs(det) > 1e-8 )
3051         {
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);
3054             return CV_OK;
3055         }
3056     }
3057     else if( direction == CV_CAMERA_TO_WARP )
3058     {/* convert from warped image to camera image coordinates */
3059         x = cameraPoint->x;
3060         y = cameraPoint->y;
3061 
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]);
3063 
3064         if( fabs(det) > 1e-8 )
3065         {
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);
3068             return CV_OK;
3069         }
3070     }
3071 
3072     return CV_BADFACTOR_ERR;
3073 }
3074 
3075 /* Compute stereo params using some camera params */
3076 /* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
icvComputeRestStereoParams(CvStereoCamera * stereoparams)3077 int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
3078 {
3079 
3080 
3081     icvGetQuadsTransformStruct(stereoparams);
3082 
3083     cvInitPerspectiveTransform( stereoparams->warpSize,
3084                                 stereoparams->quad[0],
3085                                 stereoparams->coeffs[0],
3086                                 0);
3087 
3088     cvInitPerspectiveTransform( stereoparams->warpSize,
3089                                 stereoparams->quad[1],
3090                                 stereoparams->coeffs[1],
3091                                 0);
3092 
3093     /* Create border for warped images */
3094     CvPoint2D32f corns[4];
3095     corns[0].x = 0;
3096     corns[0].y = 0;
3097 
3098     corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3099     corns[1].y = 0;
3100 
3101     corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3102     corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3103 
3104     corns[3].x = 0;
3105     corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3106 
3107     int i;
3108     for( i = 0; i < 4; i++ )
3109     {
3110         /* For first camera */
3111         icvConvertWarpCoordinates( stereoparams->coeffs[0],
3112                                 corns+i,
3113                                 stereoparams->border[0]+i,
3114                                 CV_CAMERA_TO_WARP);
3115 
3116         /* For second camera */
3117         icvConvertWarpCoordinates( stereoparams->coeffs[1],
3118                                 corns+i,
3119                                 stereoparams->border[1]+i,
3120                                 CV_CAMERA_TO_WARP);
3121     }
3122 
3123     /* Test compute  */
3124     {
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);
3130 
3131         CvPoint2D32f camPoints1[4];
3132         CvPoint2D32f camPoints2[4];
3133 
3134         for( int i = 0; i < 4; i++ )
3135         {
3136             icvConvertWarpCoordinates(stereoparams->coeffs[0],
3137                                 camPoints1+i,
3138                                 warpPoints+i,
3139                                 CV_WARP_TO_CAMERA);
3140 
3141             icvConvertWarpCoordinates(stereoparams->coeffs[1],
3142                                 camPoints2+i,
3143                                 warpPoints+i,
3144                                 CV_WARP_TO_CAMERA);
3145         }
3146     }
3147 
3148 
3149     /* Allocate memory for scanlines coeffs */
3150 
3151     stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));
3152 
3153     /* Compute coeffs for epilines  */
3154 
3155     icvComputeCoeffForStereo( stereoparams);
3156 
3157     /* all coeffs are known */
3158     return CV_OK;
3159 }
3160 
3161 /*-------------------------------------------------------------------------------------------*/
3162 
icvStereoCalibration(int numImages,int * nums,CvSize imageSize,CvPoint2D32f * imagePoints1,CvPoint2D32f * imagePoints2,CvPoint3D32f * objectPoints,CvStereoCamera * stereoparams)3163 int icvStereoCalibration( int numImages,
3164                             int* nums,
3165                             CvSize imageSize,
3166                             CvPoint2D32f* imagePoints1,
3167                             CvPoint2D32f* imagePoints2,
3168                             CvPoint3D32f* objectPoints,
3169                             CvStereoCamera* stereoparams
3170                            )
3171 {
3172     /* Firstly we must calibrate both cameras */
3173     /*  Alocate memory for data */
3174     /* Allocate for translate vectors */
3175     float* transVects1;
3176     float* transVects2;
3177     float* rotMatrs1;
3178     float* rotMatrs2;
3179 
3180     transVects1 = (float*)calloc(numImages,sizeof(float)*3);
3181     transVects2 = (float*)calloc(numImages,sizeof(float)*3);
3182 
3183     rotMatrs1   = (float*)calloc(numImages,sizeof(float)*9);
3184     rotMatrs2   = (float*)calloc(numImages,sizeof(float)*9);
3185 
3186     /* Calibrate first camera */
3187     cvCalibrateCamera(  numImages,
3188                         nums,
3189                         imageSize,
3190                         imagePoints1,
3191                         objectPoints,
3192                         stereoparams->camera[0]->distortion,
3193                         stereoparams->camera[0]->matrix,
3194                         transVects1,
3195                         rotMatrs1,
3196                         1);
3197 
3198     /* Calibrate second camera */
3199     cvCalibrateCamera(  numImages,
3200                         nums,
3201                         imageSize,
3202                         imagePoints2,
3203                         objectPoints,
3204                         stereoparams->camera[1]->distortion,
3205                         stereoparams->camera[1]->matrix,
3206                         transVects2,
3207                         rotMatrs2,
3208                         1);
3209 
3210     /* Cameras are calibrated */
3211 
3212     stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
3213     stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
3214 
3215     stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
3216     stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
3217 
3218     icvSelectBestRt(    numImages,
3219                         nums,
3220                         imagePoints1,
3221                         imagePoints2,
3222                         objectPoints,
3223                         stereoparams->camera[0]->matrix,
3224                         stereoparams->camera[0]->distortion,
3225                         rotMatrs1,
3226                         transVects1,
3227                         stereoparams->camera[1]->matrix,
3228                         stereoparams->camera[1]->distortion,
3229                         rotMatrs2,
3230                         transVects2,
3231                         stereoparams->rotMatrix,
3232                         stereoparams->transVector
3233                         );
3234 
3235     /* Free memory */
3236     free(transVects1);
3237     free(transVects2);
3238     free(rotMatrs1);
3239     free(rotMatrs2);
3240 
3241     icvComputeRestStereoParams(stereoparams);
3242 
3243     return CV_NO_ERR;
3244 }
3245 
3246 /* Find line from epipole */
FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f * start,CvPoint2D32f * end)3247 void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
3248 {
3249     CvPoint2D32f frameBeg;
3250     CvPoint2D32f frameEnd;
3251     CvPoint2D32f cross[4];
3252     int     haveCross[4];
3253     float   dist;
3254 
3255     haveCross[0] = 0;
3256     haveCross[1] = 0;
3257     haveCross[2] = 0;
3258     haveCross[3] = 0;
3259 
3260     frameBeg.x = 0;
3261     frameBeg.y = 0;
3262     frameEnd.x = (float)(imageSize.width);
3263     frameEnd.y = 0;
3264     haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);
3265 
3266     frameBeg.x = (float)(imageSize.width);
3267     frameBeg.y = 0;
3268     frameEnd.x = (float)(imageSize.width);
3269     frameEnd.y = (float)(imageSize.height);
3270     haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);
3271 
3272     frameBeg.x = (float)(imageSize.width);
3273     frameBeg.y = (float)(imageSize.height);
3274     frameEnd.x = 0;
3275     frameEnd.y = (float)(imageSize.height);
3276     haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);
3277 
3278     frameBeg.x = 0;
3279     frameBeg.y = (float)(imageSize.height);
3280     frameEnd.x = 0;
3281     frameEnd.y = 0;
3282     haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);
3283 
3284     int n;
3285     float minDist = (float)(INT_MAX);
3286     float maxDist = (float)(INT_MIN);
3287 
3288     int maxN = -1;
3289     int minN = -1;
3290 
3291     for( n = 0; n < 4; n++ )
3292     {
3293         if( haveCross[n] > 0 )
3294         {
3295             dist =  (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
3296                     (epipole.y - cross[n].y)*(epipole.y - cross[n].y);
3297 
3298             if( dist < minDist )
3299             {
3300                 minDist = dist;
3301                 minN = n;
3302             }
3303 
3304             if( dist > maxDist )
3305             {
3306                 maxDist = dist;
3307                 maxN = n;
3308             }
3309         }
3310     }
3311 
3312     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3313     {
3314         *start = cross[minN];
3315         *end   = cross[maxN];
3316     }
3317     else
3318     {
3319         start->x = 0;
3320         start->y = 0;
3321         end->x = 0;
3322         end->y = 0;
3323     }
3324 
3325     return;
3326 }
3327 
3328 
3329 /* Find line which cross frame by line(a,b,c) */
FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f * start,CvPoint2D32f * end)3330 void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
3331 {
3332     CvPoint2D32f frameBeg;
3333     CvPoint2D32f frameEnd;
3334     CvPoint2D32f cross[4];
3335     int     haveCross[4];
3336     float   dist;
3337 
3338     haveCross[0] = 0;
3339     haveCross[1] = 0;
3340     haveCross[2] = 0;
3341     haveCross[3] = 0;
3342 
3343     frameBeg.x = 0;
3344     frameBeg.y = 0;
3345     frameEnd.x = (float)(imageSize.width);
3346     frameEnd.y = 0;
3347     haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
3348 
3349     frameBeg.x = (float)(imageSize.width);
3350     frameBeg.y = 0;
3351     frameEnd.x = (float)(imageSize.width);
3352     frameEnd.y = (float)(imageSize.height);
3353     haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
3354 
3355     frameBeg.x = (float)(imageSize.width);
3356     frameBeg.y = (float)(imageSize.height);
3357     frameEnd.x = 0;
3358     frameEnd.y = (float)(imageSize.height);
3359     haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
3360 
3361     frameBeg.x = 0;
3362     frameBeg.y = (float)(imageSize.height);
3363     frameEnd.x = 0;
3364     frameEnd.y = 0;
3365     haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
3366 
3367     int n;
3368     float minDist = (float)(INT_MAX);
3369     float maxDist = (float)(INT_MIN);
3370 
3371     int maxN = -1;
3372     int minN = -1;
3373 
3374     double midPointX = imageSize.width  / 2.0;
3375     double midPointY = imageSize.height / 2.0;
3376 
3377     for( n = 0; n < 4; n++ )
3378     {
3379         if( haveCross[n] > 0 )
3380         {
3381             dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
3382                             (midPointY - cross[n].y)*(midPointY - cross[n].y));
3383 
3384             if( dist < minDist )
3385             {
3386                 minDist = dist;
3387                 minN = n;
3388             }
3389 
3390             if( dist > maxDist )
3391             {
3392                 maxDist = dist;
3393                 maxN = n;
3394             }
3395         }
3396     }
3397 
3398     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3399     {
3400         *start = cross[minN];
3401         *end   = cross[maxN];
3402     }
3403     else
3404     {
3405         start->x = 0;
3406         start->y = 0;
3407         end->x = 0;
3408         end->y = 0;
3409     }
3410 
3411     return;
3412 
3413 }
3414 
3415 /* Cross lines */
GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f * cross)3416 int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
3417 {
3418     double ex1,ey1,ex2,ey2;
3419     double px1,py1,px2,py2;
3420     double del;
3421     double delA,delB,delX,delY;
3422     double alpha,betta;
3423 
3424     ex1 = p1_start.x;
3425     ey1 = p1_start.y;
3426     ex2 = p1_end.x;
3427     ey2 = p1_end.y;
3428 
3429     px1 = p2_start.x;
3430     py1 = p2_start.y;
3431     px2 = p2_end.x;
3432     py2 = p2_end.y;
3433 
3434     del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3435     if( del == 0)
3436     {
3437         return -1;
3438     }
3439 
3440     delA =  (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3441     delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3442 
3443     alpha =  delA / del;
3444     betta = -delB / del;
3445 
3446     if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
3447     {
3448         return -1;
3449     }
3450 
3451     delX =  (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3452             (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3453 
3454     delY =  (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3455             (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3456 
3457     cross->x = (float)( delX / del);
3458     cross->y = (float)(-delY / del);
3459     return 1;
3460 }
3461 
3462 
icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f * cross)3463 int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
3464 {
3465     double ex1,ey1,ex2,ey2;
3466     double px1,py1,px2,py2;
3467     double del;
3468     double delA,delB,delX,delY;
3469     double alpha,betta;
3470 
3471     ex1 = p1_start.x;
3472     ey1 = p1_start.y;
3473     ex2 = p1_end.x;
3474     ey2 = p1_end.y;
3475 
3476     px1 = v2_start.x;
3477     py1 = v2_start.y;
3478     px2 = v2_end.x;
3479     py2 = v2_end.y;
3480 
3481     del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3482     if( del == 0)
3483     {
3484         return -1;
3485     }
3486 
3487     delA =  (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3488     delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3489 
3490     alpha =  delA / del;
3491     betta = -delB / del;
3492 
3493     if( alpha < 0 || alpha > 1.0 )
3494     {
3495         return -1;
3496     }
3497 
3498     delX =  (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3499             (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3500 
3501     delY =  (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3502             (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3503 
3504     cross->x = (float)( delX / del);
3505     cross->y = (float)(-delY / del);
3506     return 1;
3507 }
3508 
3509 
icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f * cross)3510 int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
3511 {
3512     double del;
3513     double delX,delY,delA;
3514 
3515     double px1,px2,py1,py2;
3516     double X,Y,alpha;
3517 
3518     px1 = p1.x;
3519     py1 = p1.y;
3520 
3521     px2 = p2.x;
3522     py2 = p2.y;
3523 
3524     del = a * (px2 - px1) + b * (py2-py1);
3525     if( del == 0 )
3526     {
3527         return -1;
3528     }
3529 
3530     delA = - c - a*px1 - b*py1;
3531     alpha = delA / del;
3532 
3533     if( alpha < 0 || alpha > 1.0 )
3534     {
3535         return -1;/* no cross */
3536     }
3537 
3538     delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
3539     delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);
3540 
3541     X = delX / del;
3542     Y = delY / del;
3543 
3544     cross->x = (float)X;
3545     cross->y = (float)Y;
3546 
3547     return 1;
3548 }
3549 
cvComputeEpipoles(CvMatr32f camMatr1,CvMatr32f camMatr2,CvMatr32f rotMatr1,CvMatr32f rotMatr2,CvVect32f transVect1,CvVect32f transVect2,CvVect32f epipole1,CvVect32f epipole2)3550 int cvComputeEpipoles( CvMatr32f camMatr1,  CvMatr32f camMatr2,
3551                             CvMatr32f rotMatr1,  CvMatr32f rotMatr2,
3552                             CvVect32f transVect1,CvVect32f transVect2,
3553                             CvVect32f epipole1,
3554                             CvVect32f epipole2)
3555 {
3556 
3557     /* Copy matrix */
3558 
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);
3567 
3568 
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);
3585 
3586     /* Compute first */
3587     cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
3588     cvmInvert( &cmatrP1,&cinvP1 );
3589     cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );
3590 
3591     /* Compute second */
3592     cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
3593     cvmInvert( &cmatrP2,&cinvP2 );
3594     cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );
3595 
3596     cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
3597     cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
3598     cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);
3599 
3600     cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
3601     cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
3602     cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);
3603 
3604 
3605     /* Need scale */
3606 
3607     cvmScale(&ctmpE1,&cepipole1,1.0);
3608     cvmScale(&ctmpE2,&cepipole2,1.0);
3609 
3610     cvmFree(&cmatrP1);
3611     cvmFree(&cmatrP1);
3612     cvmFree(&cvectp1);
3613     cvmFree(&cvectp2);
3614     cvmFree(&ctmpF1);
3615     cvmFree(&ctmpM1);
3616     cvmFree(&ctmpM2);
3617     cvmFree(&cinvP1);
3618     cvmFree(&cinvP2);
3619     cvmFree(&ctmpMatr);
3620     cvmFree(&ctmpVect1);
3621     cvmFree(&ctmpVect2);
3622     cvmFree(&cmatrF1);
3623     cvmFree(&ctmpF);
3624     cvmFree(&ctmpE1);
3625     cvmFree(&ctmpE2);
3626 
3627     return CV_NO_ERR;
3628 }/* cvComputeEpipoles */
3629 
3630 
3631 /* Compute epipoles for fundamental matrix */
cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,CvPoint3D32f * epipole1,CvPoint3D32f * epipole2)3632 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
3633                                          CvPoint3D32f* epipole1,
3634                                          CvPoint3D32f* epipole2)
3635 {
3636     /* Decompose fundamental matrix using SVD ( A = U W V') */
3637     CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
3638 
3639     CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
3640     CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
3641     CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);
3642 
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);
3646 
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];
3651 
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];
3656 
3657     cvReleaseMat(&matrW);
3658     cvReleaseMat(&matrU);
3659     cvReleaseMat(&matrV);
3660     return CV_OK;
3661 }
3662 
cvConvertEssential2Fundamental(CvMatr32f essMatr,CvMatr32f fundMatr,CvMatr32f cameraMatr1,CvMatr32f cameraMatr2)3663 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
3664                                          CvMatr32f fundMatr,
3665                                          CvMatr32f cameraMatr1,
3666                                          CvMatr32f cameraMatr2)
3667 {/* Fund = inv(CM1') * Ess * inv(CM2) */
3668 
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);
3673 
3674     CvMat* invCM2  = cvCreateMat(3,3,CV_MAT32F);
3675     CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
3676     CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);
3677 
3678     cvTranspose(&cameraMatr1C,tmpMatr);
3679     cvInvert(tmpMatr,invCM1T);
3680     cvmMul(invCM1T,&essMatrC,tmpMatr);
3681     cvInvert(&cameraMatr2C,invCM2);
3682     cvmMul(tmpMatr,invCM2,&fundMatrC);
3683 
3684     /* Scale fundamental matrix */
3685     double scale;
3686     scale = 1.0/fundMatrC.data.fl[8];
3687     cvConvertScale(&fundMatrC,&fundMatrC,scale);
3688 
3689     cvReleaseMat(&invCM2);
3690     cvReleaseMat(&tmpMatr);
3691     cvReleaseMat(&invCM1T);
3692 
3693     return CV_OK;
3694 }
3695 
3696 /* Compute essential matrix */
3697 
cvComputeEssentialMatrix(CvMatr32f rotMatr,CvMatr32f transVect,CvMatr32f essMatr)3698 int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
3699                                     CvMatr32f transVect,
3700                                     CvMatr32f essMatr)
3701 {
3702     float transMatr[9];
3703 
3704     /* Make antisymmetric matrix from transpose vector */
3705     transMatr[0] =   0;
3706     transMatr[1] = - transVect[2];
3707     transMatr[2] =   transVect[1];
3708 
3709     transMatr[3] =   transVect[2];
3710     transMatr[4] =   0;
3711     transMatr[5] = - transVect[0];
3712 
3713     transMatr[6] = - transVect[1];
3714     transMatr[7] =   transVect[0];
3715     transMatr[8] =   0;
3716 
3717     icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);
3718 
3719     return CV_OK;
3720 }
3721 
3722 
3723