1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
44 //#define REAL_ZERO(x) ( (x) < 1e-8 && (x) > -1e-8)
47 icvGetNormalVector3( CvMatrix3 * Matrix, float *v )
49 /* return vector v that is any 3-vector perpendicular
50 to all the row vectors of Matrix */
52 double *solutions = 0;
54 double B[3] = { 0.f, 0.f, 0.f };
57 if( Matrix == 0 || v == 0 )
58 return CV_NULLPTR_ERR;
60 for( i = 0; i < 3; i++ )
62 for( j = 0; j < 3; j++ )
63 M[i * 3 + j] = (double) (Matrix->m[i][j]);
66 res = icvGaussMxN( M, B, 3, 3, &solutions );
69 return CV_BADFACTOR_ERR;
71 if( res > 0 && solutions )
73 v[0] = (float) solutions[0];
74 v[1] = (float) solutions[1];
75 v[2] = (float) solutions[2];
85 return CV_BADFACTOR_ERR;
89 } /* icvgetNormalVector3 */
92 /*=====================================================================================*/
95 icvMultMatrixVector3( CvMatrix3 * m, float *src, float *dst )
97 if( m == 0 || src == 0 || dst == 0 )
98 return CV_NULLPTR_ERR;
100 dst[0] = m->m[0][0] * src[0] + m->m[0][1] * src[1] + m->m[0][2] * src[2];
101 dst[1] = m->m[1][0] * src[0] + m->m[1][1] * src[1] + m->m[1][2] * src[2];
102 dst[2] = m->m[2][0] * src[0] + m->m[2][1] * src[1] + m->m[2][2] * src[2];
106 } /* icvMultMatrixVector3 */
109 /*=====================================================================================*/
112 icvMultMatrixTVector3( CvMatrix3 * m, float *src, float *dst )
114 if( m == 0 || src == 0 || dst == 0 )
115 return CV_NULLPTR_ERR;
117 dst[0] = m->m[0][0] * src[0] + m->m[1][0] * src[1] + m->m[2][0] * src[2];
118 dst[1] = m->m[0][1] * src[0] + m->m[1][1] * src[1] + m->m[2][1] * src[2];
119 dst[2] = m->m[0][2] * src[0] + m->m[1][2] * src[1] + m->m[2][2] * src[2];
123 } /* icvMultMatrixTVector3 */
125 /*=====================================================================================*/
128 icvCrossLines( float *line1, float *line2, float *cross_point )
132 if( line1 == 0 && line2 == 0 && cross_point == 0 )
133 return CV_NULLPTR_ERR;
135 delta = line1[0] * line2[1] - line1[1] * line2[0];
137 if( REAL_ZERO( delta ))
138 return CV_BADFACTOR_ERR;
140 cross_point[0] = (-line1[2] * line2[1] + line1[1] * line2[2]) / delta;
141 cross_point[1] = (-line1[0] * line2[2] + line1[2] * line2[0]) / delta;
145 } /* icvCrossLines */
149 /*======================================================================================*/
152 icvMakeScanlines( CvMatrix3 * matrix,
154 int *scanlines_1, int *scanlines_2, int *lens_1, int *lens_2, int *numlines )
159 error = icvGetCoefficient( matrix, imgSize, scanlines_2, scanlines_1, numlines );
161 /* Make Length of scanlines */
163 if( scanlines_1 == 0 && scanlines_2 == 0 )
166 icvMakeScanlinesLengths( scanlines_1, *numlines, lens_1 );
168 icvMakeScanlinesLengths( scanlines_2, *numlines, lens_2 );
174 } /* icvMakeScanlines */
177 /*======================================================================================*/
180 icvMakeScanlinesLengths( int *scanlines, int numlines, int *lens )
183 int x1, y1, x2, y2, dx, dy;
188 for( index = 0; index < numlines; index++ )
191 x1 = scanlines[curr++];
192 y1 = scanlines[curr++];
193 x2 = scanlines[curr++];
194 y2 = scanlines[curr++];
196 dx = abs( x1 - x2 ) + 1;
197 dy = abs( y1 - y2 ) + 1;
199 lens[index] = MAX( dx, dy );
205 /*======================================================================================*/
208 icvMakeAlphaScanlines( int *scanlines_1,
210 int *scanlines_a, int *lens, int numlines, float alpha )
220 for( index = 0; index < numlines; index++ )
223 x1 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
225 scanlines_a[curr++] = x1;
227 y1 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
229 scanlines_a[curr++] = y1;
231 x2 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
233 scanlines_a[curr++] = x2;
235 y2 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
237 scanlines_a[curr++] = y2;
239 dx = abs( x1 - x2 ) + 1;
240 dy = abs( y1 - y2 ) + 1;
242 lens[curr_len++] = MAX( dx, dy );
249 /*======================================================================================*/
257 /* //////////////////////////////////////////////////////////////////////////////////// */
260 icvGetCoefficient( CvMatrix3 * matrix,
261 CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines )
277 error = icvGetCoefficientDefault( matrix,
278 imgSize, scanlines_1, scanlines_2, numlines );
283 for( i = 0; i < 3; i++ )
284 for( j = 0; j < 3; j++ )
285 Ft.m[i][j] = F->m[j][i];
288 error = icvGetNormalVector3( &Ft, l_epipole );
289 if( error == CV_NO_ERR && !REAL_ZERO( l_epipole[2] ) && !REAL_ZERO( l_epipole[2] - 1 ))
292 l_epipole[0] /= l_epipole[2];
293 l_epipole[1] /= l_epipole[2];
297 error = icvGetNormalVector3( F, r_epipole );
298 if( error == CV_NO_ERR && !REAL_ZERO( r_epipole[2] ) && !REAL_ZERO( r_epipole[2] - 1 ))
301 r_epipole[0] /= r_epipole[2];
302 r_epipole[1] /= r_epipole[2];
306 if( REAL_ZERO( l_epipole[2] - 1 ) && REAL_ZERO( r_epipole[2] - 1 ))
308 error = icvGetCoefficientStereo( matrix,
311 r_epipole, scanlines_1, scanlines_2, numlines );
312 if( error == CV_NO_ERR )
317 if( REAL_ZERO( l_epipole[2] ) && REAL_ZERO( r_epipole[2] ))
319 error = icvGetCoefficientOrto( matrix,
320 imgSize, scanlines_1, scanlines_2, numlines );
321 if( error == CV_NO_ERR )
327 error = icvGetCoefficientDefault( matrix, imgSize, scanlines_1, scanlines_2, numlines );
331 } /* icvlGetCoefficient */
333 /*===========================================================================*/
335 icvGetCoefficientDefault( CvMatrix3 * matrix,
336 CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines )
341 *numlines = imgSize.height;
343 if( scanlines_1 == 0 && scanlines_2 == 0 )
347 for( y = 0; y < imgSize.height; y++ )
349 scanlines_1[curr] = 0;
350 scanlines_1[curr + 1] = y;
351 scanlines_1[curr + 2] = imgSize.width - 1;
352 scanlines_1[curr + 3] = y;
354 scanlines_2[curr] = 0;
355 scanlines_2[curr + 1] = y;
356 scanlines_2[curr + 2] = imgSize.width - 1;
357 scanlines_2[curr + 3] = y;
365 } /* icvlGetCoefficientDefault */
367 /*===========================================================================*/
369 icvGetCoefficientOrto( CvMatrix3 * matrix,
370 CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines )
372 float l_start_end[4], r_start_end[4];
379 if( F->m[0][2] * F->m[1][2] < 0 )
382 if( F->m[2][0] * F->m[2][1] < 0 )
384 error = icvGetStartEnd1( F, imgSize, l_start_end, r_start_end );
390 error = icvGetStartEnd2( F, imgSize, l_start_end, r_start_end );
397 if( F->m[2][0] * F->m[2][1] < 0 )
399 error = icvGetStartEnd3( F, imgSize, l_start_end, r_start_end );
403 error = icvGetStartEnd4( F, imgSize, l_start_end, r_start_end );
407 if( error != CV_NO_ERR )
410 a = fabs( l_start_end[0] - l_start_end[2] );
411 b = fabs( r_start_end[0] - r_start_end[2] );
415 error = icvBuildScanlineLeft( F,
417 scanlines_1, scanlines_2, l_start_end, numlines );
423 error = icvBuildScanlineRight( F,
425 scanlines_1, scanlines_2, r_start_end, numlines );
431 } /* icvlGetCoefficientOrto */
433 /*===========================================================================*/
435 icvGetStartEnd1( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
442 float l_point[3], r_point[3], epiline[3];
443 CvStatus error = CV_OK;
446 width = imgSize.width - 1;
447 height = imgSize.height - 1;
449 l_diagonal[0] = (float) 1 / width;
450 l_diagonal[1] = (float) 1 / height;
453 r_diagonal[0] = (float) 1 / width;
454 r_diagonal[1] = (float) 1 / height;
457 r_point[0] = (float) width;
461 icvMultMatrixVector3( F, r_point, epiline );
462 error = icvCrossLines( l_diagonal, epiline, l_point );
464 assert( error == CV_NO_ERR );
466 if( l_point[0] >= 0 && l_point[0] <= width )
469 l_start_end[0] = l_point[0];
470 l_start_end[1] = l_point[1];
472 r_start_end[0] = r_point[0];
473 r_start_end[1] = r_point[1];
483 l_point[1] = (float) height;
486 icvMultMatrixTVector3( F, l_point, epiline );
487 error = icvCrossLines( r_diagonal, epiline, r_point );
488 assert( error == CV_NO_ERR );
490 if( r_point[0] >= 0 && r_point[0] <= width )
492 l_start_end[0] = l_point[0];
493 l_start_end[1] = l_point[1];
495 r_start_end[0] = r_point[0];
496 r_start_end[1] = r_point[1];
499 return CV_BADFACTOR_ERR;
503 { /* if( l_point[0] > width ) */
505 l_point[0] = (float) width;
509 icvMultMatrixTVector3( F, l_point, epiline );
510 error = icvCrossLines( r_diagonal, epiline, r_point );
511 assert( error == CV_NO_ERR );
513 if( r_point[0] >= 0 && r_point[0] <= width )
516 l_start_end[0] = l_point[0];
517 l_start_end[1] = l_point[1];
519 r_start_end[0] = r_point[0];
520 r_start_end[1] = r_point[1];
523 return CV_BADFACTOR_ERR;
529 r_point[1] = (float) height;
532 icvMultMatrixVector3( F, r_point, epiline );
533 error = icvCrossLines( l_diagonal, epiline, l_point );
534 assert( error == CV_NO_ERR );
536 if( l_point[0] >= 0 && l_point[0] <= width )
539 l_start_end[2] = l_point[0];
540 l_start_end[3] = l_point[1];
542 r_start_end[2] = r_point[0];
543 r_start_end[3] = r_point[1];
553 l_point[1] = (float) height;
556 icvMultMatrixTVector3( F, l_point, epiline );
557 error = icvCrossLines( r_diagonal, epiline, r_point );
558 assert( error == CV_NO_ERR );
560 if( r_point[0] >= 0 && r_point[0] <= width )
563 l_start_end[2] = l_point[0];
564 l_start_end[3] = l_point[1];
566 r_start_end[2] = r_point[0];
567 r_start_end[3] = r_point[1];
570 return CV_BADFACTOR_ERR;
574 { /* if( l_point[0] > width ) */
576 l_point[0] = (float) width;
580 icvMultMatrixTVector3( F, l_point, epiline );
581 error = icvCrossLines( r_diagonal, epiline, r_point );
582 assert( error == CV_NO_ERR );
584 if( r_point[0] >= 0 && r_point[0] <= width )
587 l_start_end[2] = l_point[0];
588 l_start_end[3] = l_point[1];
590 r_start_end[2] = r_point[0];
591 r_start_end[3] = r_point[1];
594 return CV_BADFACTOR_ERR;
600 } /* icvlGetStartEnd1 */
602 /*===========================================================================*/
604 icvGetStartEnd2( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
612 float l_point[3], r_point[3], epiline[3];
613 CvStatus error = CV_OK;
617 width = imgSize.width - 1;
618 height = imgSize.height - 1;
620 l_diagonal[0] = (float) 1 / width;
621 l_diagonal[1] = (float) 1 / height;
624 r_diagonal[0] = (float) height / width;
632 icvMultMatrixVector3( F, r_point, epiline );
634 error = icvCrossLines( l_diagonal, epiline, l_point );
636 assert( error == CV_NO_ERR );
638 if( l_point[0] >= 0 && l_point[0] <= width )
641 l_start_end[0] = l_point[0];
642 l_start_end[1] = l_point[1];
644 r_start_end[0] = r_point[0];
645 r_start_end[1] = r_point[1];
655 l_point[1] = (float) height;
658 icvMultMatrixTVector3( F, l_point, epiline );
659 error = icvCrossLines( r_diagonal, epiline, r_point );
661 assert( error == CV_NO_ERR );
663 if( r_point[0] >= 0 && r_point[0] <= width )
666 l_start_end[0] = l_point[0];
667 l_start_end[1] = l_point[1];
669 r_start_end[0] = r_point[0];
670 r_start_end[1] = r_point[1];
673 return CV_BADFACTOR_ERR;
677 { /* if( l_point[0] > width ) */
679 l_point[0] = (float) width;
683 icvMultMatrixTVector3( F, l_point, epiline );
684 error = icvCrossLines( r_diagonal, epiline, r_point );
685 assert( error == CV_NO_ERR );
687 if( r_point[0] >= 0 && r_point[0] <= width )
690 l_start_end[0] = l_point[0];
691 l_start_end[1] = l_point[1];
693 r_start_end[0] = r_point[0];
694 r_start_end[1] = r_point[1];
697 return CV_BADFACTOR_ERR;
701 r_point[0] = (float) width;
702 r_point[1] = (float) height;
705 icvMultMatrixVector3( F, r_point, epiline );
706 error = icvCrossLines( l_diagonal, epiline, l_point );
707 assert( error == CV_NO_ERR );
709 if( l_point[0] >= 0 && l_point[0] <= width )
712 l_start_end[2] = l_point[0];
713 l_start_end[3] = l_point[1];
715 r_start_end[2] = r_point[0];
716 r_start_end[3] = r_point[1];
726 l_point[1] = (float) height;
729 icvMultMatrixTVector3( F, l_point, epiline );
730 error = icvCrossLines( r_diagonal, epiline, r_point );
731 assert( error == CV_NO_ERR );
733 if( r_point[0] >= 0 && r_point[0] <= width )
736 l_start_end[2] = l_point[0];
737 l_start_end[3] = l_point[1];
739 r_start_end[2] = r_point[0];
740 r_start_end[3] = r_point[1];
743 return CV_BADFACTOR_ERR;
747 { /* if( l_point[0] > width ) */
749 l_point[0] = (float) width;
753 icvMultMatrixTVector3( F, l_point, epiline );
754 error = icvCrossLines( r_diagonal, epiline, r_point );
755 assert( error == CV_NO_ERR );
757 if( r_point[0] >= 0 && r_point[0] <= width )
760 l_start_end[2] = l_point[0];
761 l_start_end[3] = l_point[1];
763 r_start_end[2] = r_point[0];
764 r_start_end[3] = r_point[1];
767 return CV_BADFACTOR_ERR;
773 } /* icvlGetStartEnd2 */
775 /*===========================================================================*/
777 icvGetStartEnd3( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
784 float l_point[3], r_point[3], epiline[3];
785 CvStatus error = CV_OK;
789 width = imgSize.width - 1;
790 height = imgSize.height - 1;
792 l_diagonal[0] = (float) height / width;
796 r_diagonal[0] = (float) 1 / width;
797 r_diagonal[1] = (float) 1 / height;
804 icvMultMatrixVector3( F, r_point, epiline );
806 error = icvCrossLines( l_diagonal, epiline, l_point );
808 assert( error == CV_NO_ERR );
810 if( l_point[0] >= 0 && l_point[0] <= width )
813 l_start_end[0] = l_point[0];
814 l_start_end[1] = l_point[1];
816 r_start_end[0] = r_point[0];
817 r_start_end[1] = r_point[1];
827 l_point[1] = (float) height;
830 icvMultMatrixTVector3( F, l_point, epiline );
831 error = icvCrossLines( r_diagonal, epiline, r_point );
832 assert( error == CV_NO_ERR );
834 if( r_point[0] >= 0 && r_point[0] <= width )
837 l_start_end[0] = l_point[0];
838 l_start_end[1] = l_point[1];
840 r_start_end[0] = r_point[0];
841 r_start_end[1] = r_point[1];
844 return CV_BADFACTOR_ERR;
848 { /* if( l_point[0] > width ) */
850 l_point[0] = (float) width;
854 icvMultMatrixTVector3( F, l_point, epiline );
855 error = icvCrossLines( r_diagonal, epiline, r_point );
856 assert( error == CV_NO_ERR );
858 if( r_point[0] >= 0 && r_point[0] <= width )
861 l_start_end[0] = l_point[0];
862 l_start_end[1] = l_point[1];
864 r_start_end[0] = r_point[0];
865 r_start_end[1] = r_point[1];
868 return CV_BADFACTOR_ERR;
872 r_point[0] = (float) width;
873 r_point[1] = (float) height;
876 icvMultMatrixVector3( F, r_point, epiline );
877 error = icvCrossLines( l_diagonal, epiline, l_point );
878 assert( error == CV_NO_ERR );
880 if( l_point[0] >= 0 && l_point[0] <= width )
883 l_start_end[2] = l_point[0];
884 l_start_end[3] = l_point[1];
886 r_start_end[2] = r_point[0];
887 r_start_end[3] = r_point[1];
897 l_point[1] = (float) height;
900 icvMultMatrixTVector3( F, l_point, epiline );
902 error = icvCrossLines( r_diagonal, epiline, r_point );
904 assert( error == CV_NO_ERR );
906 if( r_point[0] >= 0 && r_point[0] <= width )
909 l_start_end[2] = l_point[0];
910 l_start_end[3] = l_point[1];
912 r_start_end[2] = r_point[0];
913 r_start_end[3] = r_point[1];
916 return CV_BADFACTOR_ERR;
920 { /* if( l_point[0] > width ) */
922 l_point[0] = (float) width;
926 icvMultMatrixTVector3( F, l_point, epiline );
928 error = icvCrossLines( r_diagonal, epiline, r_point );
930 assert( error == CV_NO_ERR );
932 if( r_point[0] >= 0 && r_point[0] <= width )
935 l_start_end[2] = l_point[0];
936 l_start_end[3] = l_point[1];
938 r_start_end[2] = r_point[0];
939 r_start_end[3] = r_point[1];
942 return CV_BADFACTOR_ERR;
948 } /* icvlGetStartEnd3 */
950 /*===========================================================================*/
952 icvGetStartEnd4( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
958 float l_point[3], r_point[3], epiline[3];
963 width = imgSize.width - 1;
964 height = imgSize.height - 1;
966 l_diagonal[0] = (float) height / width;
970 r_diagonal[0] = (float) height / width;
978 icvMultMatrixVector3( F, r_point, epiline );
979 error = icvCrossLines( l_diagonal, epiline, l_point );
981 if( error != CV_NO_ERR )
984 if( l_point[0] >= 0 && l_point[0] <= width )
987 l_start_end[0] = l_point[0];
988 l_start_end[1] = l_point[1];
990 r_start_end[0] = r_point[0];
991 r_start_end[1] = r_point[1];
1004 icvMultMatrixTVector3( F, l_point, epiline );
1005 error = icvCrossLines( r_diagonal, epiline, r_point );
1006 assert( error == CV_NO_ERR );
1008 if( r_point[0] >= 0 && r_point[0] <= width )
1011 l_start_end[0] = l_point[0];
1012 l_start_end[1] = l_point[1];
1014 r_start_end[0] = r_point[0];
1015 r_start_end[1] = r_point[1];
1018 return CV_BADFACTOR_ERR;
1022 { /* if( l_point[0] > width ) */
1024 l_point[0] = (float) width;
1025 l_point[1] = (float) height;
1028 icvMultMatrixTVector3( F, l_point, epiline );
1029 error = icvCrossLines( r_diagonal, epiline, r_point );
1030 assert( error == CV_NO_ERR );
1032 if( r_point[0] >= 0 && r_point[0] <= width )
1035 l_start_end[0] = l_point[0];
1036 l_start_end[1] = l_point[1];
1038 r_start_end[0] = r_point[0];
1039 r_start_end[1] = r_point[1];
1042 return CV_BADFACTOR_ERR;
1046 r_point[0] = (float) width;
1047 r_point[1] = (float) height;
1050 icvMultMatrixVector3( F, r_point, epiline );
1051 error = icvCrossLines( l_diagonal, epiline, l_point );
1052 assert( error == CV_NO_ERR );
1054 if( l_point[0] >= 0 && l_point[0] <= width )
1057 l_start_end[2] = l_point[0];
1058 l_start_end[3] = l_point[1];
1060 r_start_end[2] = r_point[0];
1061 r_start_end[3] = r_point[1];
1067 if( l_point[0] < 0 )
1074 icvMultMatrixTVector3( F, l_point, epiline );
1075 error = icvCrossLines( r_diagonal, epiline, r_point );
1076 assert( error == CV_NO_ERR );
1078 if( r_point[0] >= 0 && r_point[0] <= width )
1081 l_start_end[2] = l_point[0];
1082 l_start_end[3] = l_point[1];
1084 r_start_end[2] = r_point[0];
1085 r_start_end[3] = r_point[1];
1088 return CV_BADFACTOR_ERR;
1092 { /* if( l_point[0] > width ) */
1094 l_point[0] = (float) width;
1095 l_point[1] = (float) height;
1098 icvMultMatrixTVector3( F, l_point, epiline );
1099 error = icvCrossLines( r_diagonal, epiline, r_point );
1100 assert( error == CV_NO_ERR );
1102 if( r_point[0] >= 0 && r_point[0] <= width )
1105 l_start_end[2] = l_point[0];
1106 l_start_end[3] = l_point[1];
1108 r_start_end[2] = r_point[0];
1109 r_start_end[3] = r_point[1];
1112 return CV_BADFACTOR_ERR;
1118 } /* icvlGetStartEnd4 */
1120 /*===========================================================================*/
1122 icvBuildScanlineLeft( CvMatrix3 * matrix,
1124 int *scanlines_1, int *scanlines_2, float *l_start_end, int *numlines )
1132 CvStatus error = CV_OK;
1139 assert( l_start_end != 0 );
1141 a = fabs( l_start_end[2] - l_start_end[0] );
1142 b = fabs( l_start_end[3] - l_start_end[1] );
1143 prewarp_height = cvRound( MAX(a, b) );
1145 *numlines = prewarp_height;
1147 if( scanlines_1 == 0 && scanlines_2 == 0 )
1154 height = (float) prewarp_height;
1156 delta_x = (l_start_end[2] - l_start_end[0]) / height;
1158 l_start_end[0] += delta_x;
1159 l_start_end[2] -= delta_x;
1161 delta_x = (l_start_end[2] - l_start_end[0]) / height;
1162 delta_y = (l_start_end[3] - l_start_end[1]) / height;
1164 l_start_end[1] += delta_y;
1165 l_start_end[3] -= delta_y;
1167 delta_y = (l_start_end[3] - l_start_end[1]) / height;
1169 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1172 l_point[0] = l_start_end[0] + i * delta_x;
1173 l_point[1] = l_start_end[1] + i * delta_y;
1175 icvMultMatrixTVector3( F, l_point, epiline );
1177 error = icvGetCrossEpilineFrame( imgSize, epiline,
1178 scanlines_2 + offset,
1179 scanlines_2 + offset + 1,
1180 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1184 assert( error == CV_NO_ERR );
1186 r_point[0] = -(float) (*(scanlines_2 + offset));
1187 r_point[1] = -(float) (*(scanlines_2 + offset + 1));
1190 icvMultMatrixVector3( F, r_point, epiline );
1192 error = icvGetCrossEpilineFrame( imgSize, epiline,
1193 scanlines_1 + offset,
1194 scanlines_1 + offset + 1,
1195 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1197 assert( error == CV_NO_ERR );
1200 *numlines = prewarp_height;
1204 } /*icvlBuildScanlineLeft */
1206 /*===========================================================================*/
1208 icvBuildScanlineRight( CvMatrix3 * matrix,
1210 int *scanlines_1, int *scanlines_2, float *r_start_end, int *numlines )
1218 CvStatus error = CV_OK;
1225 assert( r_start_end != 0 );
1227 a = fabs( r_start_end[2] - r_start_end[0] );
1228 b = fabs( r_start_end[3] - r_start_end[1] );
1229 prewarp_height = cvRound( MAX(a, b) );
1231 *numlines = prewarp_height;
1233 if( scanlines_1 == 0 && scanlines_2 == 0 )
1239 height = (float) prewarp_height;
1241 delta_x = (r_start_end[2] - r_start_end[0]) / height;
1243 r_start_end[0] += delta_x;
1244 r_start_end[2] -= delta_x;
1246 delta_x = (r_start_end[2] - r_start_end[0]) / height;
1247 delta_y = (r_start_end[3] - r_start_end[1]) / height;
1249 r_start_end[1] += delta_y;
1250 r_start_end[3] -= delta_y;
1252 delta_y = (r_start_end[3] - r_start_end[1]) / height;
1254 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1257 r_point[0] = r_start_end[0] + i * delta_x;
1258 r_point[1] = r_start_end[1] + i * delta_y;
1260 icvMultMatrixVector3( F, r_point, epiline );
1262 error = icvGetCrossEpilineFrame( imgSize, epiline,
1263 scanlines_1 + offset,
1264 scanlines_1 + offset + 1,
1265 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1268 assert( error == CV_NO_ERR );
1270 l_point[0] = -(float) (*(scanlines_1 + offset));
1271 l_point[1] = -(float) (*(scanlines_1 + offset + 1));
1275 icvMultMatrixTVector3( F, l_point, epiline );
1276 error = icvGetCrossEpilineFrame( imgSize, epiline,
1277 scanlines_2 + offset,
1278 scanlines_2 + offset + 1,
1279 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1282 assert( error == CV_NO_ERR );
1285 *numlines = prewarp_height;
1289 } /*icvlBuildScanlineRight */
1291 /*===========================================================================*/
1292 #define Abs(x) ( (x)<0 ? -(x):(x) )
1293 #define Sgn(x) ( (x)<0 ? -1:1 ) /* Sgn(0) = 1 ! */
1296 icvBuildScanline( CvSize imgSize, float *epiline, float *kx, float *cx, float *ky, float *cy )
1298 float point[4][2], d;
1301 float width, height;
1303 if( REAL_ZERO( epiline[0] ) && REAL_ZERO( epiline[1] ))
1304 return CV_BADFACTOR_ERR;
1306 width = (float) imgSize.width - 1;
1307 height = (float) imgSize.height - 1;
1309 sign[0] = Sgn( epiline[2] );
1310 sign[1] = Sgn( epiline[0] * width + epiline[2] );
1311 sign[2] = Sgn( epiline[1] * height + epiline[2] );
1312 sign[3] = Sgn( epiline[0] * width + epiline[1] * height + epiline[2] );
1316 if( sign[0] * sign[1] < 0 )
1319 point[i][0] = -epiline[2] / epiline[0];
1324 if( sign[0] * sign[2] < 0 )
1328 point[i][1] = -epiline[2] / epiline[1];
1332 if( sign[1] * sign[3] < 0 )
1335 point[i][0] = width;
1336 point[i][1] = -(epiline[0] * width + epiline[2]) / epiline[1];
1340 if( sign[2] * sign[3] < 0 )
1343 point[i][0] = -(epiline[1] * height + epiline[2]) / epiline[0];
1344 point[i][1] = height;
1347 if( sign[0] == sign[1] && sign[0] == sign[2] && sign[0] == sign[3] )
1348 return CV_BADFACTOR_ERR;
1350 if( !kx && !ky && !cx && !cy )
1351 return CV_BADFACTOR_ERR;
1359 d = (float) MAX( Abs( *kx ), Abs( *ky ));
1368 if( (point[0][0] - point[1][0]) * epiline[1] +
1369 (point[1][1] - point[0][1]) * epiline[0] > 0 )
1386 } /* icvlBuildScanline */
1388 /*===========================================================================*/
1390 icvGetCoefficientStereo( CvMatrix3 * matrix,
1393 float *r_epipole, int *scanlines_1, int *scanlines_2, int *numlines )
1396 float width, height;
1397 float l_angle[2], r_angle[2];
1398 float l_radius, r_radius;
1399 float r_point[3], l_point[3];
1400 float l_epiline[3], r_epiline[3], x, y;
1403 float radius1, radius2, radius3, radius4;
1405 float l_start_end[4], r_start_end[4];
1408 float Region[3][3][4] = {
1409 {{0.f, 0.f, 1.f, 1.f}, {0.f, 1.f, 1.f, 1.f}, {0.f, 1.f, 1.f, 0.f}},
1410 {{0.f, 0.f, 0.f, 1.f}, {2.f, 2.f, 2.f, 2.f}, {1.f, 1.f, 1.f, 0.f}},
1411 {{1.f, 0.f, 0.f, 1.f}, {1.f, 0.f, 0.f, 0.f}, {1.f, 1.f, 0.f, 0.f}}
1415 width = (float) imgSize.width - 1;
1416 height = (float) imgSize.height - 1;
1420 if( F->m[0][0] * F->m[1][1] - F->m[1][0] * F->m[0][1] > 0 )
1425 if( l_epipole[0] < 0 )
1427 else if( l_epipole[0] < width )
1432 if( l_epipole[1] < 0 )
1434 else if( l_epipole[1] < height )
1439 l_start_end[0] = Region[j][i][0];
1440 l_start_end[1] = Region[j][i][1];
1441 l_start_end[2] = Region[j][i][2];
1442 l_start_end[3] = Region[j][i][3];
1444 if( r_epipole[0] < 0 )
1446 else if( r_epipole[0] < width )
1451 if( r_epipole[1] < 0 )
1453 else if( r_epipole[1] < height )
1458 r_start_end[0] = Region[j][i][0];
1459 r_start_end[1] = Region[j][i][1];
1460 r_start_end[2] = Region[j][i][2];
1461 r_start_end[3] = Region[j][i][3];
1463 radius1 = l_epipole[0] * l_epipole[0] + (l_epipole[1] - height) * (l_epipole[1] - height);
1465 radius2 = (l_epipole[0] - width) * (l_epipole[0] - width) +
1466 (l_epipole[1] - height) * (l_epipole[1] - height);
1468 radius3 = l_epipole[0] * l_epipole[0] + l_epipole[1] * l_epipole[1];
1470 radius4 = (l_epipole[0] - width) * (l_epipole[0] - width) + l_epipole[1] * l_epipole[1];
1473 l_radius = (float) sqrt( (double)MAX( MAX( radius1, radius2 ), MAX( radius3, radius4 )));
1475 radius1 = r_epipole[0] * r_epipole[0] + (r_epipole[1] - height) * (r_epipole[1] - height);
1477 radius2 = (r_epipole[0] - width) * (r_epipole[0] - width) +
1478 (r_epipole[1] - height) * (r_epipole[1] - height);
1480 radius3 = r_epipole[0] * r_epipole[0] + r_epipole[1] * r_epipole[1];
1482 radius4 = (r_epipole[0] - width) * (r_epipole[0] - width) + r_epipole[1] * r_epipole[1];
1485 r_radius = (float) sqrt( (double)MAX( MAX( radius1, radius2 ), MAX( radius3, radius4 )));
1487 if( l_start_end[0] == 2 && r_start_end[0] == 2 )
1489 if( l_radius > r_radius )
1493 l_angle[1] = (float) CV_PI;
1495 error = icvBuildScanlineLeftStereo( imgSize,
1499 l_radius, scanlines_1, scanlines_2, numlines );
1508 r_angle[1] = (float) CV_PI;
1510 error = icvBuildScanlineRightStereo( imgSize,
1515 scanlines_1, scanlines_2, numlines );
1520 if( l_start_end[0] == 2 )
1523 r_angle[0] = (float) atan2( r_start_end[1] * height - r_epipole[1],
1524 r_start_end[0] * width - r_epipole[0] );
1525 r_angle[1] = (float) atan2( r_start_end[3] * height - r_epipole[1],
1526 r_start_end[2] * width - r_epipole[0] );
1528 if( r_angle[0] > r_angle[1] )
1529 r_angle[1] += (float) (CV_PI * 2);
1531 error = icvBuildScanlineRightStereo( imgSize,
1535 r_radius, scanlines_1, scanlines_2, numlines );
1540 if( r_start_end[0] == 2 )
1543 l_point[0] = l_start_end[0] * width;
1544 l_point[1] = l_start_end[1] * height;
1547 icvMultMatrixTVector3( F, l_point, r_epiline );
1549 l_angle[0] = (float) atan2( l_start_end[1] * height - l_epipole[1],
1550 l_start_end[0] * width - l_epipole[0] );
1551 l_angle[1] = (float) atan2( l_start_end[3] * height - l_epipole[1],
1552 l_start_end[2] * width - l_epipole[0] );
1554 if( l_angle[0] > l_angle[1] )
1555 l_angle[1] += (float) (CV_PI * 2);
1557 error = icvBuildScanlineLeftStereo( imgSize,
1561 l_radius, scanlines_1, scanlines_2, numlines );
1567 l_start_end[0] *= width;
1568 l_start_end[1] *= height;
1569 l_start_end[2] *= width;
1570 l_start_end[3] *= height;
1572 r_start_end[0] *= width;
1573 r_start_end[1] *= height;
1574 r_start_end[2] *= width;
1575 r_start_end[3] *= height;
1577 r_point[0] = r_start_end[0];
1578 r_point[1] = r_start_end[1];
1581 icvMultMatrixVector3( F, r_point, l_epiline );
1582 error = icvBuildScanline( imgSize, l_epiline, 0, &x, 0, &y );
1584 if( error == CV_NO_ERR )
1587 l_angle[0] = (float) atan2( y - l_epipole[1], x - l_epipole[0] );
1589 r_angle[0] = (float) atan2( r_point[1] - r_epipole[1], r_point[0] - r_epipole[0] );
1598 l_point[0] = l_start_end[0];
1599 l_point[1] = l_start_end[1];
1605 l_point[0] = l_start_end[2];
1606 l_point[1] = l_start_end[3];
1611 icvMultMatrixTVector3( F, l_point, r_epiline );
1612 error = icvBuildScanline( imgSize, r_epiline, 0, &x, 0, &y );
1614 if( error == CV_NO_ERR )
1617 r_angle[0] = (float) atan2( y - r_epipole[1], x - r_epipole[0] );
1619 l_angle[0] = (float) atan2( l_point[1] - l_epipole[1], l_point[0] - l_epipole[0] );
1623 return CV_BADFACTOR_ERR;
1626 r_point[0] = r_start_end[2];
1627 r_point[1] = r_start_end[3];
1630 icvMultMatrixVector3( F, r_point, l_epiline );
1631 error = icvBuildScanline( imgSize, l_epiline, 0, &x, 0, &y );
1633 if( error == CV_NO_ERR )
1636 l_angle[1] = (float) atan2( y - l_epipole[1], x - l_epipole[0] );
1638 r_angle[1] = (float) atan2( r_point[1] - r_epipole[1], r_point[0] - r_epipole[0] );
1647 l_point[0] = l_start_end[2];
1648 l_point[1] = l_start_end[3];
1654 l_point[0] = l_start_end[0];
1655 l_point[1] = l_start_end[1];
1660 icvMultMatrixTVector3( F, l_point, r_epiline );
1661 error = icvBuildScanline( imgSize, r_epiline, 0, &x, 0, &y );
1663 if( error == CV_NO_ERR )
1666 r_angle[1] = (float) atan2( y - r_epipole[1], x - r_epipole[0] );
1668 l_angle[1] = (float) atan2( l_point[1] - l_epipole[1], l_point[0] - l_epipole[0] );
1672 return CV_BADFACTOR_ERR;
1675 if( l_angle[0] > l_angle[1] )
1679 l_angle[0] = l_angle[1];
1683 if( l_angle[1] - l_angle[0] > CV_PI )
1687 l_angle[0] = l_angle[1];
1688 l_angle[1] = swap + (float) (CV_PI * 2);
1691 if( r_angle[0] > r_angle[1] )
1695 r_angle[0] = r_angle[1];
1699 if( r_angle[1] - r_angle[0] > CV_PI )
1703 r_angle[0] = r_angle[1];
1704 r_angle[1] = swap + (float) (CV_PI * 2);
1707 if( l_radius * (l_angle[1] - l_angle[0]) > r_radius * (r_angle[1] - r_angle[0]) )
1708 error = icvBuildScanlineLeftStereo( imgSize,
1712 l_radius, scanlines_1, scanlines_2, numlines );
1715 error = icvBuildScanlineRightStereo( imgSize,
1719 r_radius, scanlines_1, scanlines_2, numlines );
1724 } /* icvGetCoefficientStereo */
1726 /*===========================================================================*/
1728 icvBuildScanlineLeftStereo( CvSize imgSize,
1732 float l_radius, int *scanlines_1, int *scanlines_2, int *numlines )
1734 //int prewarp_width;
1744 CvStatus error = CV_OK;
1748 assert( l_angle != 0 && !REAL_ZERO( l_radius ));
1750 /*prewarp_width = (int) (sqrt( image_width * image_width +
1751 image_height * image_height ) + 1);*/
1753 prewarp_height = (int) (l_radius * (l_angle[1] - l_angle[0]));
1755 *numlines = prewarp_height;
1757 if( scanlines_1 == 0 && scanlines_2 == 0 )
1763 height = (float) prewarp_height;
1765 delta = (l_angle[1] - l_angle[0]) / height;
1767 l_angle[0] += delta;
1768 l_angle[1] -= delta;
1770 delta = (l_angle[1] - l_angle[0]) / height;
1772 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1775 angle = l_angle[0] + i * delta;
1777 l_point[0] = l_epipole[0] + l_radius * (float) cos( angle );
1778 l_point[1] = l_epipole[1] + l_radius * (float) sin( angle );
1780 icvMultMatrixTVector3( F, l_point, r_epiline );
1782 error = icvGetCrossEpilineFrame( imgSize, r_epiline,
1783 scanlines_2 + offset,
1784 scanlines_2 + offset + 1,
1785 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1788 l_epiline[0] = l_point[1] - l_epipole[1];
1789 l_epiline[1] = l_epipole[0] - l_point[0];
1790 l_epiline[2] = l_point[0] * l_epipole[1] - l_point[1] * l_epipole[0];
1792 if( Sgn( l_epiline[0] * r_epiline[0] + l_epiline[1] * r_epiline[1] ) < 0 )
1795 l_epiline[0] = -l_epiline[0];
1796 l_epiline[1] = -l_epiline[1];
1797 l_epiline[2] = -l_epiline[2];
1800 error = icvGetCrossEpilineFrame( imgSize, l_epiline,
1801 scanlines_1 + offset,
1802 scanlines_1 + offset + 1,
1803 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1807 *numlines = prewarp_height;
1811 } /* icvlBuildScanlineLeftStereo */
1813 /*===========================================================================*/
1815 icvBuildScanlineRightStereo( CvSize imgSize,
1820 int *scanlines_1, int *scanlines_2, int *numlines )
1822 //int prewarp_width;
1832 CvStatus error = CV_OK;
1835 assert( r_angle != 0 && !REAL_ZERO( r_radius ));
1837 /*prewarp_width = (int) (sqrt( image_width * image_width +
1838 image_height * image_height ) + 1);*/
1840 prewarp_height = (int) (r_radius * (r_angle[1] - r_angle[0]));
1842 *numlines = prewarp_height;
1844 if( scanlines_1 == 0 && scanlines_2 == 0 )
1850 height = (float) prewarp_height;
1852 delta = (r_angle[1] - r_angle[0]) / height;
1854 r_angle[0] += delta;
1855 r_angle[1] -= delta;
1857 delta = (r_angle[1] - r_angle[0]) / height;
1859 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1862 angle = r_angle[0] + i * delta;
1864 r_point[0] = r_epipole[0] + r_radius * (float) cos( angle );
1865 r_point[1] = r_epipole[1] + r_radius * (float) sin( angle );
1867 icvMultMatrixVector3( F, r_point, l_epiline );
1869 error = icvGetCrossEpilineFrame( imgSize, l_epiline,
1870 scanlines_1 + offset,
1871 scanlines_1 + offset + 1,
1872 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1874 assert( error == CV_NO_ERR );
1876 r_epiline[0] = r_point[1] - r_epipole[1];
1877 r_epiline[1] = r_epipole[0] - r_point[0];
1878 r_epiline[2] = r_point[0] * r_epipole[1] - r_point[1] * r_epipole[0];
1880 if( Sgn( l_epiline[0] * r_epiline[0] + l_epiline[1] * r_epiline[1] ) < 0 )
1883 r_epiline[0] = -r_epiline[0];
1884 r_epiline[1] = -r_epiline[1];
1885 r_epiline[2] = -r_epiline[2];
1888 error = icvGetCrossEpilineFrame( imgSize, r_epiline,
1889 scanlines_2 + offset,
1890 scanlines_2 + offset + 1,
1891 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1893 assert( error == CV_NO_ERR );
1896 *numlines = prewarp_height;
1900 } /* icvlBuildScanlineRightStereo */
1902 /*===========================================================================*/
1904 icvGetCrossEpilineFrame( CvSize imgSize, float *epiline, int *x1, int *y1, int *x2, int *y2 )
1909 float width, height;
1912 if( REAL_ZERO( epiline[0] ) && REAL_ZERO( epiline[1] ))
1913 return CV_BADFACTOR_ERR;
1915 width = (float) imgSize.width - 1;
1916 height = (float) imgSize.height - 1;
1918 tmpvalue = epiline[2];
1919 sign[0] = SIGN( tmpvalue );
1921 tmpvalue = epiline[0] * width + epiline[2];
1922 sign[1] = SIGN( tmpvalue );
1924 tmpvalue = epiline[1] * height + epiline[2];
1925 sign[2] = SIGN( tmpvalue );
1927 tmpvalue = epiline[0] * width + epiline[1] * height + epiline[2];
1928 sign[3] = SIGN( tmpvalue );
1931 for( tx = 0; tx < 2; tx++ )
1933 for( ty = 0; ty < 2; ty++ )
1936 if( sign[ty * 2 + tx] == 0 )
1939 point[i][0] = width * tx;
1940 point[i][1] = height * ty;
1947 if( sign[0] * sign[1] < 0 )
1949 point[i][0] = -epiline[2] / epiline[0];
1954 if( sign[0] * sign[2] < 0 )
1957 point[i][1] = -epiline[2] / epiline[1];
1961 if( sign[1] * sign[3] < 0 )
1963 point[i][0] = width;
1964 point[i][1] = -(epiline[0] * width + epiline[2]) / epiline[1];
1968 if( sign[2] * sign[3] < 0 )
1970 point[i][0] = -(epiline[1] * height + epiline[2]) / epiline[0];
1971 point[i][1] = height;
1974 if( sign[0] == sign[1] && sign[0] == sign[2] && sign[0] == sign[3] )
1975 return CV_BADFACTOR_ERR;
1977 if( (point[0][0] - point[1][0]) * epiline[1] +
1978 (point[1][1] - point[0][1]) * epiline[0] > 0 )
1980 *x1 = (int) point[0][0];
1981 *y1 = (int) point[0][1];
1982 *x2 = (int) point[1][0];
1983 *y2 = (int) point[1][1];
1987 *x1 = (int) point[1][0];
1988 *y1 = (int) point[1][1];
1989 *x2 = (int) point[0][0];
1990 *y2 = (int) point[0][1];
1994 } /* icvlGetCrossEpilineFrame */
1996 /*=====================================================================================*/
1999 cvMakeScanlines( const CvMatrix3* matrix, CvSize imgSize,
2000 int *scanlines_1, int *scanlines_2,
2001 int *lens_1, int *lens_2, int *numlines )
2003 CV_FUNCNAME( "cvMakeScanlines" );
2007 IPPI_CALL( icvMakeScanlines( (CvMatrix3*)matrix, imgSize, scanlines_1,
2008 scanlines_2, lens_1, lens_2, numlines ));
2012 /*F///////////////////////////////////////////////////////////////////////////////////////
2013 // Name: cvDeleteMoire
2014 // Purpose: The functions
2021 cvMakeAlphaScanlines( int *scanlines_1,
2023 int *scanlines_a, int *lens, int numlines, float alpha )
2025 CV_FUNCNAME( "cvMakeAlphaScanlines" );
2029 IPPI_CALL( icvMakeAlphaScanlines( scanlines_1, scanlines_2, scanlines_a,
2030 lens, numlines, alpha ));