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 #include "_cvaux.h"
42 #include "_cvvm.h"
43
44 //#define REAL_ZERO(x) ( (x) < 1e-8 && (x) > -1e-8)
45
46 static CvStatus
icvGetNormalVector3(CvMatrix3 * Matrix,float * v)47 icvGetNormalVector3( CvMatrix3 * Matrix, float *v )
48 {
49 /* return vector v that is any 3-vector perpendicular
50 to all the row vectors of Matrix */
51
52 double *solutions = 0;
53 double M[3 * 3];
54 double B[3] = { 0.f, 0.f, 0.f };
55 int i, j, res;
56
57 if( Matrix == 0 || v == 0 )
58 return CV_NULLPTR_ERR;
59
60 for( i = 0; i < 3; i++ )
61 {
62 for( j = 0; j < 3; j++ )
63 M[i * 3 + j] = (double) (Matrix->m[i][j]);
64 } /* for */
65
66 res = icvGaussMxN( M, B, 3, 3, &solutions );
67
68 if( res == -1 )
69 return CV_BADFACTOR_ERR;
70
71 if( res > 0 && solutions )
72 {
73 v[0] = (float) solutions[0];
74 v[1] = (float) solutions[1];
75 v[2] = (float) solutions[2];
76 res = 0;
77 }
78 else
79 res = 1;
80
81 if( solutions )
82 cvFree( &solutions );
83
84 if( res )
85 return CV_BADFACTOR_ERR;
86 else
87 return CV_NO_ERR;
88
89 } /* icvgetNormalVector3 */
90
91
92 /*=====================================================================================*/
93
94 static CvStatus
icvMultMatrixVector3(CvMatrix3 * m,float * src,float * dst)95 icvMultMatrixVector3( CvMatrix3 * m, float *src, float *dst )
96 {
97 if( m == 0 || src == 0 || dst == 0 )
98 return CV_NULLPTR_ERR;
99
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];
103
104 return CV_NO_ERR;
105
106 } /* icvMultMatrixVector3 */
107
108
109 /*=====================================================================================*/
110
111 static CvStatus
icvMultMatrixTVector3(CvMatrix3 * m,float * src,float * dst)112 icvMultMatrixTVector3( CvMatrix3 * m, float *src, float *dst )
113 {
114 if( m == 0 || src == 0 || dst == 0 )
115 return CV_NULLPTR_ERR;
116
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];
120
121 return CV_NO_ERR;
122
123 } /* icvMultMatrixTVector3 */
124
125 /*=====================================================================================*/
126
127 static CvStatus
icvCrossLines(float * line1,float * line2,float * cross_point)128 icvCrossLines( float *line1, float *line2, float *cross_point )
129 {
130 float delta;
131
132 if( line1 == 0 && line2 == 0 && cross_point == 0 )
133 return CV_NULLPTR_ERR;
134
135 delta = line1[0] * line2[1] - line1[1] * line2[0];
136
137 if( REAL_ZERO( delta ))
138 return CV_BADFACTOR_ERR;
139
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;
142 cross_point[2] = 1;
143
144 return CV_NO_ERR;
145 } /* icvCrossLines */
146
147
148
149 /*======================================================================================*/
150
151 static CvStatus
icvMakeScanlines(CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,int * lens_1,int * lens_2,int * numlines)152 icvMakeScanlines( CvMatrix3 * matrix,
153 CvSize imgSize,
154 int *scanlines_1, int *scanlines_2, int *lens_1, int *lens_2, int *numlines )
155 {
156
157 CvStatus error;
158
159 error = icvGetCoefficient( matrix, imgSize, scanlines_2, scanlines_1, numlines );
160
161 /* Make Length of scanlines */
162
163 if( scanlines_1 == 0 && scanlines_2 == 0 )
164 return error;
165
166 icvMakeScanlinesLengths( scanlines_1, *numlines, lens_1 );
167
168 icvMakeScanlinesLengths( scanlines_2, *numlines, lens_2 );
169
170 matrix = matrix;
171 return CV_NO_ERR;
172
173
174 } /* icvMakeScanlines */
175
176
177 /*======================================================================================*/
178
179 CvStatus
icvMakeScanlinesLengths(int * scanlines,int numlines,int * lens)180 icvMakeScanlinesLengths( int *scanlines, int numlines, int *lens )
181 {
182 int index;
183 int x1, y1, x2, y2, dx, dy;
184 int curr;
185
186 curr = 0;
187
188 for( index = 0; index < numlines; index++ )
189 {
190
191 x1 = scanlines[curr++];
192 y1 = scanlines[curr++];
193 x2 = scanlines[curr++];
194 y2 = scanlines[curr++];
195
196 dx = abs( x1 - x2 ) + 1;
197 dy = abs( y1 - y2 ) + 1;
198
199 lens[index] = MAX( dx, dy );
200
201 }
202 return CV_NO_ERR;
203 }
204
205 /*======================================================================================*/
206
207 static CvStatus
icvMakeAlphaScanlines(int * scanlines_1,int * scanlines_2,int * scanlines_a,int * lens,int numlines,float alpha)208 icvMakeAlphaScanlines( int *scanlines_1,
209 int *scanlines_2,
210 int *scanlines_a, int *lens, int numlines, float alpha )
211 {
212 int index;
213 int x1, y1, x2, y2;
214 int curr;
215 int dx, dy;
216 int curr_len;
217
218 curr = 0;
219 curr_len = 0;
220 for( index = 0; index < numlines; index++ )
221 {
222
223 x1 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
224
225 scanlines_a[curr++] = x1;
226
227 y1 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
228
229 scanlines_a[curr++] = y1;
230
231 x2 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
232
233 scanlines_a[curr++] = x2;
234
235 y2 = (int) (scanlines_1[curr] * alpha + scanlines_2[curr] * (1.0 - alpha));
236
237 scanlines_a[curr++] = y2;
238
239 dx = abs( x1 - x2 ) + 1;
240 dy = abs( y1 - y2 ) + 1;
241
242 lens[curr_len++] = MAX( dx, dy );
243
244 }
245
246 return CV_NO_ERR;
247 }
248
249 /*======================================================================================*/
250
251
252
253
254
255
256
257 /* //////////////////////////////////////////////////////////////////////////////////// */
258
259 CvStatus
icvGetCoefficient(CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,int * numlines)260 icvGetCoefficient( CvMatrix3 * matrix,
261 CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines )
262 {
263 float l_epipole[3];
264 float r_epipole[3];
265 CvMatrix3 *F;
266 CvMatrix3 Ft;
267 CvStatus error;
268 int i, j;
269
270 F = matrix;
271
272 l_epipole[2] = -1;
273 r_epipole[2] = -1;
274
275 if( F == 0 )
276 {
277 error = icvGetCoefficientDefault( matrix,
278 imgSize, scanlines_1, scanlines_2, numlines );
279 return error;
280 }
281
282
283 for( i = 0; i < 3; i++ )
284 for( j = 0; j < 3; j++ )
285 Ft.m[i][j] = F->m[j][i];
286
287
288 error = icvGetNormalVector3( &Ft, l_epipole );
289 if( error == CV_NO_ERR && !REAL_ZERO( l_epipole[2] ) && !REAL_ZERO( l_epipole[2] - 1 ))
290 {
291
292 l_epipole[0] /= l_epipole[2];
293 l_epipole[1] /= l_epipole[2];
294 l_epipole[2] = 1;
295 } /* if */
296
297 error = icvGetNormalVector3( F, r_epipole );
298 if( error == CV_NO_ERR && !REAL_ZERO( r_epipole[2] ) && !REAL_ZERO( r_epipole[2] - 1 ))
299 {
300
301 r_epipole[0] /= r_epipole[2];
302 r_epipole[1] /= r_epipole[2];
303 r_epipole[2] = 1;
304 } /* if */
305
306 if( REAL_ZERO( l_epipole[2] - 1 ) && REAL_ZERO( r_epipole[2] - 1 ))
307 {
308 error = icvGetCoefficientStereo( matrix,
309 imgSize,
310 l_epipole,
311 r_epipole, scanlines_1, scanlines_2, numlines );
312 if( error == CV_NO_ERR )
313 return CV_NO_ERR;
314 }
315 else
316 {
317 if( REAL_ZERO( l_epipole[2] ) && REAL_ZERO( r_epipole[2] ))
318 {
319 error = icvGetCoefficientOrto( matrix,
320 imgSize, scanlines_1, scanlines_2, numlines );
321 if( error == CV_NO_ERR )
322 return CV_NO_ERR;
323 }
324 }
325
326
327 error = icvGetCoefficientDefault( matrix, imgSize, scanlines_1, scanlines_2, numlines );
328
329 return error;
330
331 } /* icvlGetCoefficient */
332
333 /*===========================================================================*/
334 CvStatus
icvGetCoefficientDefault(CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,int * numlines)335 icvGetCoefficientDefault( CvMatrix3 * matrix,
336 CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines )
337 {
338 int curr;
339 int y;
340
341 *numlines = imgSize.height;
342
343 if( scanlines_1 == 0 && scanlines_2 == 0 )
344 return CV_NO_ERR;
345
346 curr = 0;
347 for( y = 0; y < imgSize.height; y++ )
348 {
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;
353
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;
358
359 curr += 4;
360 }
361
362 matrix = matrix;
363 return CV_NO_ERR;
364
365 } /* icvlGetCoefficientDefault */
366
367 /*===========================================================================*/
368 CvStatus
icvGetCoefficientOrto(CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,int * numlines)369 icvGetCoefficientOrto( CvMatrix3 * matrix,
370 CvSize imgSize, int *scanlines_1, int *scanlines_2, int *numlines )
371 {
372 float l_start_end[4], r_start_end[4];
373 double a, b;
374 CvStatus error;
375 CvMatrix3 *F;
376
377 F = matrix;
378
379 if( F->m[0][2] * F->m[1][2] < 0 )
380 { /* on left / */
381
382 if( F->m[2][0] * F->m[2][1] < 0 )
383 { /* on right / */
384 error = icvGetStartEnd1( F, imgSize, l_start_end, r_start_end );
385
386
387 }
388 else
389 { /* on right \ */
390 error = icvGetStartEnd2( F, imgSize, l_start_end, r_start_end );
391 } /* if */
392
393 }
394 else
395 { /* on left \ */
396
397 if( F->m[2][0] * F->m[2][1] < 0 )
398 { /* on right / */
399 error = icvGetStartEnd3( F, imgSize, l_start_end, r_start_end );
400 }
401 else
402 { /* on right \ */
403 error = icvGetStartEnd4( F, imgSize, l_start_end, r_start_end );
404 } /* if */
405 } /* if */
406
407 if( error != CV_NO_ERR )
408 return error;
409
410 a = fabs( l_start_end[0] - l_start_end[2] );
411 b = fabs( r_start_end[0] - r_start_end[2] );
412 if( a > b )
413 {
414
415 error = icvBuildScanlineLeft( F,
416 imgSize,
417 scanlines_1, scanlines_2, l_start_end, numlines );
418
419 }
420 else
421 {
422
423 error = icvBuildScanlineRight( F,
424 imgSize,
425 scanlines_1, scanlines_2, r_start_end, numlines );
426
427 } /* if */
428
429 return error;
430
431 } /* icvlGetCoefficientOrto */
432
433 /*===========================================================================*/
434 CvStatus
icvGetStartEnd1(CvMatrix3 * matrix,CvSize imgSize,float * l_start_end,float * r_start_end)435 icvGetStartEnd1( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
436 {
437
438 CvMatrix3 *F;
439 int width, height;
440 float l_diagonal[3];
441 float r_diagonal[3];
442 float l_point[3], r_point[3], epiline[3];
443 CvStatus error = CV_OK;
444
445 F = matrix;
446 width = imgSize.width - 1;
447 height = imgSize.height - 1;
448
449 l_diagonal[0] = (float) 1 / width;
450 l_diagonal[1] = (float) 1 / height;
451 l_diagonal[2] = -1;
452
453 r_diagonal[0] = (float) 1 / width;
454 r_diagonal[1] = (float) 1 / height;
455 r_diagonal[2] = -1;
456
457 r_point[0] = (float) width;
458 r_point[1] = 0;
459 r_point[2] = 1;
460
461 icvMultMatrixVector3( F, r_point, epiline );
462 error = icvCrossLines( l_diagonal, epiline, l_point );
463
464 assert( error == CV_NO_ERR );
465
466 if( l_point[0] >= 0 && l_point[0] <= width )
467 {
468
469 l_start_end[0] = l_point[0];
470 l_start_end[1] = l_point[1];
471
472 r_start_end[0] = r_point[0];
473 r_start_end[1] = r_point[1];
474
475 }
476 else
477 {
478
479 if( l_point[0] < 0 )
480 {
481
482 l_point[0] = 0;
483 l_point[1] = (float) height;
484 l_point[2] = 1;
485
486 icvMultMatrixTVector3( F, l_point, epiline );
487 error = icvCrossLines( r_diagonal, epiline, r_point );
488 assert( error == CV_NO_ERR );
489
490 if( r_point[0] >= 0 && r_point[0] <= width )
491 {
492 l_start_end[0] = l_point[0];
493 l_start_end[1] = l_point[1];
494
495 r_start_end[0] = r_point[0];
496 r_start_end[1] = r_point[1];
497 }
498 else
499 return CV_BADFACTOR_ERR;
500
501 }
502 else
503 { /* if( l_point[0] > width ) */
504
505 l_point[0] = (float) width;
506 l_point[1] = 0;
507 l_point[2] = 1;
508
509 icvMultMatrixTVector3( F, l_point, epiline );
510 error = icvCrossLines( r_diagonal, epiline, r_point );
511 assert( error == CV_NO_ERR );
512
513 if( r_point[0] >= 0 && r_point[0] <= width )
514 {
515
516 l_start_end[0] = l_point[0];
517 l_start_end[1] = l_point[1];
518
519 r_start_end[0] = r_point[0];
520 r_start_end[1] = r_point[1];
521 }
522 else
523 return CV_BADFACTOR_ERR;
524
525 } /* if */
526 } /* if */
527
528 r_point[0] = 0;
529 r_point[1] = (float) height;
530 r_point[2] = 1;
531
532 icvMultMatrixVector3( F, r_point, epiline );
533 error = icvCrossLines( l_diagonal, epiline, l_point );
534 assert( error == CV_NO_ERR );
535
536 if( l_point[0] >= 0 && l_point[0] <= width )
537 {
538
539 l_start_end[2] = l_point[0];
540 l_start_end[3] = l_point[1];
541
542 r_start_end[2] = r_point[0];
543 r_start_end[3] = r_point[1];
544
545 }
546 else
547 {
548
549 if( l_point[0] < 0 )
550 {
551
552 l_point[0] = 0;
553 l_point[1] = (float) height;
554 l_point[2] = 1;
555
556 icvMultMatrixTVector3( F, l_point, epiline );
557 error = icvCrossLines( r_diagonal, epiline, r_point );
558 assert( error == CV_NO_ERR );
559
560 if( r_point[0] >= 0 && r_point[0] <= width )
561 {
562
563 l_start_end[2] = l_point[0];
564 l_start_end[3] = l_point[1];
565
566 r_start_end[2] = r_point[0];
567 r_start_end[3] = r_point[1];
568 }
569 else
570 return CV_BADFACTOR_ERR;
571
572 }
573 else
574 { /* if( l_point[0] > width ) */
575
576 l_point[0] = (float) width;
577 l_point[1] = 0;
578 l_point[2] = 1;
579
580 icvMultMatrixTVector3( F, l_point, epiline );
581 error = icvCrossLines( r_diagonal, epiline, r_point );
582 assert( error == CV_NO_ERR );
583
584 if( r_point[0] >= 0 && r_point[0] <= width )
585 {
586
587 l_start_end[2] = l_point[0];
588 l_start_end[3] = l_point[1];
589
590 r_start_end[2] = r_point[0];
591 r_start_end[3] = r_point[1];
592 }
593 else
594 return CV_BADFACTOR_ERR;
595 } /* if */
596 } /* if */
597
598 return error;
599
600 } /* icvlGetStartEnd1 */
601
602 /*===========================================================================*/
603 CvStatus
icvGetStartEnd2(CvMatrix3 * matrix,CvSize imgSize,float * l_start_end,float * r_start_end)604 icvGetStartEnd2( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
605 {
606
607
608 CvMatrix3 *F;
609 int width, height;
610 float l_diagonal[3];
611 float r_diagonal[3];
612 float l_point[3], r_point[3], epiline[3];
613 CvStatus error = CV_OK;
614
615 F = matrix;
616
617 width = imgSize.width - 1;
618 height = imgSize.height - 1;
619
620 l_diagonal[0] = (float) 1 / width;
621 l_diagonal[1] = (float) 1 / height;
622 l_diagonal[2] = -1;
623
624 r_diagonal[0] = (float) height / width;
625 r_diagonal[1] = -1;
626 r_diagonal[2] = 0;
627
628 r_point[0] = 0;
629 r_point[1] = 0;
630 r_point[2] = 1;
631
632 icvMultMatrixVector3( F, r_point, epiline );
633
634 error = icvCrossLines( l_diagonal, epiline, l_point );
635
636 assert( error == CV_NO_ERR );
637
638 if( l_point[0] >= 0 && l_point[0] <= width )
639 {
640
641 l_start_end[0] = l_point[0];
642 l_start_end[1] = l_point[1];
643
644 r_start_end[0] = r_point[0];
645 r_start_end[1] = r_point[1];
646
647 }
648 else
649 {
650
651 if( l_point[0] < 0 )
652 {
653
654 l_point[0] = 0;
655 l_point[1] = (float) height;
656 l_point[2] = 1;
657
658 icvMultMatrixTVector3( F, l_point, epiline );
659 error = icvCrossLines( r_diagonal, epiline, r_point );
660
661 assert( error == CV_NO_ERR );
662
663 if( r_point[0] >= 0 && r_point[0] <= width )
664 {
665
666 l_start_end[0] = l_point[0];
667 l_start_end[1] = l_point[1];
668
669 r_start_end[0] = r_point[0];
670 r_start_end[1] = r_point[1];
671 }
672 else
673 return CV_BADFACTOR_ERR;
674
675 }
676 else
677 { /* if( l_point[0] > width ) */
678
679 l_point[0] = (float) width;
680 l_point[1] = 0;
681 l_point[2] = 1;
682
683 icvMultMatrixTVector3( F, l_point, epiline );
684 error = icvCrossLines( r_diagonal, epiline, r_point );
685 assert( error == CV_NO_ERR );
686
687 if( r_point[0] >= 0 && r_point[0] <= width )
688 {
689
690 l_start_end[0] = l_point[0];
691 l_start_end[1] = l_point[1];
692
693 r_start_end[0] = r_point[0];
694 r_start_end[1] = r_point[1];
695 }
696 else
697 return CV_BADFACTOR_ERR;
698 } /* if */
699 } /* if */
700
701 r_point[0] = (float) width;
702 r_point[1] = (float) height;
703 r_point[2] = 1;
704
705 icvMultMatrixVector3( F, r_point, epiline );
706 error = icvCrossLines( l_diagonal, epiline, l_point );
707 assert( error == CV_NO_ERR );
708
709 if( l_point[0] >= 0 && l_point[0] <= width )
710 {
711
712 l_start_end[2] = l_point[0];
713 l_start_end[3] = l_point[1];
714
715 r_start_end[2] = r_point[0];
716 r_start_end[3] = r_point[1];
717
718 }
719 else
720 {
721
722 if( l_point[0] < 0 )
723 {
724
725 l_point[0] = 0;
726 l_point[1] = (float) height;
727 l_point[2] = 1;
728
729 icvMultMatrixTVector3( F, l_point, epiline );
730 error = icvCrossLines( r_diagonal, epiline, r_point );
731 assert( error == CV_NO_ERR );
732
733 if( r_point[0] >= 0 && r_point[0] <= width )
734 {
735
736 l_start_end[2] = l_point[0];
737 l_start_end[3] = l_point[1];
738
739 r_start_end[2] = r_point[0];
740 r_start_end[3] = r_point[1];
741 }
742 else
743 return CV_BADFACTOR_ERR;
744
745 }
746 else
747 { /* if( l_point[0] > width ) */
748
749 l_point[0] = (float) width;
750 l_point[1] = 0;
751 l_point[2] = 1;
752
753 icvMultMatrixTVector3( F, l_point, epiline );
754 error = icvCrossLines( r_diagonal, epiline, r_point );
755 assert( error == CV_NO_ERR );
756
757 if( r_point[0] >= 0 && r_point[0] <= width )
758 {
759
760 l_start_end[2] = l_point[0];
761 l_start_end[3] = l_point[1];
762
763 r_start_end[2] = r_point[0];
764 r_start_end[3] = r_point[1];
765 }
766 else
767 return CV_BADFACTOR_ERR;
768 }
769 } /* if */
770
771 return error;
772
773 } /* icvlGetStartEnd2 */
774
775 /*===========================================================================*/
776 CvStatus
icvGetStartEnd3(CvMatrix3 * matrix,CvSize imgSize,float * l_start_end,float * r_start_end)777 icvGetStartEnd3( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
778 {
779
780 CvMatrix3 *F;
781 int width, height;
782 float l_diagonal[3];
783 float r_diagonal[3];
784 float l_point[3], r_point[3], epiline[3];
785 CvStatus error = CV_OK;
786
787 F = matrix;
788
789 width = imgSize.width - 1;
790 height = imgSize.height - 1;
791
792 l_diagonal[0] = (float) height / width;
793 l_diagonal[1] = -1;
794 l_diagonal[2] = 0;
795
796 r_diagonal[0] = (float) 1 / width;
797 r_diagonal[1] = (float) 1 / height;
798 r_diagonal[2] = -1;
799
800 r_point[0] = 0;
801 r_point[1] = 0;
802 r_point[2] = 1;
803
804 icvMultMatrixVector3( F, r_point, epiline );
805
806 error = icvCrossLines( l_diagonal, epiline, l_point );
807
808 assert( error == CV_NO_ERR );
809
810 if( l_point[0] >= 0 && l_point[0] <= width )
811 {
812
813 l_start_end[0] = l_point[0];
814 l_start_end[1] = l_point[1];
815
816 r_start_end[0] = r_point[0];
817 r_start_end[1] = r_point[1];
818
819 }
820 else
821 {
822
823 if( l_point[0] < 0 )
824 {
825
826 l_point[0] = 0;
827 l_point[1] = (float) height;
828 l_point[2] = 1;
829
830 icvMultMatrixTVector3( F, l_point, epiline );
831 error = icvCrossLines( r_diagonal, epiline, r_point );
832 assert( error == CV_NO_ERR );
833
834 if( r_point[0] >= 0 && r_point[0] <= width )
835 {
836
837 l_start_end[0] = l_point[0];
838 l_start_end[1] = l_point[1];
839
840 r_start_end[0] = r_point[0];
841 r_start_end[1] = r_point[1];
842 }
843 else
844 return CV_BADFACTOR_ERR;
845
846 }
847 else
848 { /* if( l_point[0] > width ) */
849
850 l_point[0] = (float) width;
851 l_point[1] = 0;
852 l_point[2] = 1;
853
854 icvMultMatrixTVector3( F, l_point, epiline );
855 error = icvCrossLines( r_diagonal, epiline, r_point );
856 assert( error == CV_NO_ERR );
857
858 if( r_point[0] >= 0 && r_point[0] <= width )
859 {
860
861 l_start_end[0] = l_point[0];
862 l_start_end[1] = l_point[1];
863
864 r_start_end[0] = r_point[0];
865 r_start_end[1] = r_point[1];
866 }
867 else
868 return CV_BADFACTOR_ERR;
869 } /* if */
870 } /* if */
871
872 r_point[0] = (float) width;
873 r_point[1] = (float) height;
874 r_point[2] = 1;
875
876 icvMultMatrixVector3( F, r_point, epiline );
877 error = icvCrossLines( l_diagonal, epiline, l_point );
878 assert( error == CV_NO_ERR );
879
880 if( l_point[0] >= 0 && l_point[0] <= width )
881 {
882
883 l_start_end[2] = l_point[0];
884 l_start_end[3] = l_point[1];
885
886 r_start_end[2] = r_point[0];
887 r_start_end[3] = r_point[1];
888
889 }
890 else
891 {
892
893 if( l_point[0] < 0 )
894 {
895
896 l_point[0] = 0;
897 l_point[1] = (float) height;
898 l_point[2] = 1;
899
900 icvMultMatrixTVector3( F, l_point, epiline );
901
902 error = icvCrossLines( r_diagonal, epiline, r_point );
903
904 assert( error == CV_NO_ERR );
905
906 if( r_point[0] >= 0 && r_point[0] <= width )
907 {
908
909 l_start_end[2] = l_point[0];
910 l_start_end[3] = l_point[1];
911
912 r_start_end[2] = r_point[0];
913 r_start_end[3] = r_point[1];
914 }
915 else
916 return CV_BADFACTOR_ERR;
917
918 }
919 else
920 { /* if( l_point[0] > width ) */
921
922 l_point[0] = (float) width;
923 l_point[1] = 0;
924 l_point[2] = 1;
925
926 icvMultMatrixTVector3( F, l_point, epiline );
927
928 error = icvCrossLines( r_diagonal, epiline, r_point );
929
930 assert( error == CV_NO_ERR );
931
932 if( r_point[0] >= 0 && r_point[0] <= width )
933 {
934
935 l_start_end[2] = l_point[0];
936 l_start_end[3] = l_point[1];
937
938 r_start_end[2] = r_point[0];
939 r_start_end[3] = r_point[1];
940 }
941 else
942 return CV_BADFACTOR_ERR;
943 } /* if */
944 } /* if */
945
946 return error;
947
948 } /* icvlGetStartEnd3 */
949
950 /*===========================================================================*/
951 CvStatus
icvGetStartEnd4(CvMatrix3 * matrix,CvSize imgSize,float * l_start_end,float * r_start_end)952 icvGetStartEnd4( CvMatrix3 * matrix, CvSize imgSize, float *l_start_end, float *r_start_end )
953 {
954 CvMatrix3 *F;
955 int width, height;
956 float l_diagonal[3];
957 float r_diagonal[3];
958 float l_point[3], r_point[3], epiline[3];
959 CvStatus error;
960
961 F = matrix;
962
963 width = imgSize.width - 1;
964 height = imgSize.height - 1;
965
966 l_diagonal[0] = (float) height / width;
967 l_diagonal[1] = -1;
968 l_diagonal[2] = 0;
969
970 r_diagonal[0] = (float) height / width;
971 r_diagonal[1] = -1;
972 r_diagonal[2] = 0;
973
974 r_point[0] = 0;
975 r_point[1] = 0;
976 r_point[2] = 1;
977
978 icvMultMatrixVector3( F, r_point, epiline );
979 error = icvCrossLines( l_diagonal, epiline, l_point );
980
981 if( error != CV_NO_ERR )
982 return error;
983
984 if( l_point[0] >= 0 && l_point[0] <= width )
985 {
986
987 l_start_end[0] = l_point[0];
988 l_start_end[1] = l_point[1];
989
990 r_start_end[0] = r_point[0];
991 r_start_end[1] = r_point[1];
992
993 }
994 else
995 {
996
997 if( l_point[0] < 0 )
998 {
999
1000 l_point[0] = 0;
1001 l_point[1] = 0;
1002 l_point[2] = 1;
1003
1004 icvMultMatrixTVector3( F, l_point, epiline );
1005 error = icvCrossLines( r_diagonal, epiline, r_point );
1006 assert( error == CV_NO_ERR );
1007
1008 if( r_point[0] >= 0 && r_point[0] <= width )
1009 {
1010
1011 l_start_end[0] = l_point[0];
1012 l_start_end[1] = l_point[1];
1013
1014 r_start_end[0] = r_point[0];
1015 r_start_end[1] = r_point[1];
1016 }
1017 else
1018 return CV_BADFACTOR_ERR;
1019
1020 }
1021 else
1022 { /* if( l_point[0] > width ) */
1023
1024 l_point[0] = (float) width;
1025 l_point[1] = (float) height;
1026 l_point[2] = 1;
1027
1028 icvMultMatrixTVector3( F, l_point, epiline );
1029 error = icvCrossLines( r_diagonal, epiline, r_point );
1030 assert( error == CV_NO_ERR );
1031
1032 if( r_point[0] >= 0 && r_point[0] <= width )
1033 {
1034
1035 l_start_end[0] = l_point[0];
1036 l_start_end[1] = l_point[1];
1037
1038 r_start_end[0] = r_point[0];
1039 r_start_end[1] = r_point[1];
1040 }
1041 else
1042 return CV_BADFACTOR_ERR;
1043 } /* if */
1044 } /* if */
1045
1046 r_point[0] = (float) width;
1047 r_point[1] = (float) height;
1048 r_point[2] = 1;
1049
1050 icvMultMatrixVector3( F, r_point, epiline );
1051 error = icvCrossLines( l_diagonal, epiline, l_point );
1052 assert( error == CV_NO_ERR );
1053
1054 if( l_point[0] >= 0 && l_point[0] <= width )
1055 {
1056
1057 l_start_end[2] = l_point[0];
1058 l_start_end[3] = l_point[1];
1059
1060 r_start_end[2] = r_point[0];
1061 r_start_end[3] = r_point[1];
1062
1063 }
1064 else
1065 {
1066
1067 if( l_point[0] < 0 )
1068 {
1069
1070 l_point[0] = 0;
1071 l_point[1] = 0;
1072 l_point[2] = 1;
1073
1074 icvMultMatrixTVector3( F, l_point, epiline );
1075 error = icvCrossLines( r_diagonal, epiline, r_point );
1076 assert( error == CV_NO_ERR );
1077
1078 if( r_point[0] >= 0 && r_point[0] <= width )
1079 {
1080
1081 l_start_end[2] = l_point[0];
1082 l_start_end[3] = l_point[1];
1083
1084 r_start_end[2] = r_point[0];
1085 r_start_end[3] = r_point[1];
1086 }
1087 else
1088 return CV_BADFACTOR_ERR;
1089
1090 }
1091 else
1092 { /* if( l_point[0] > width ) */
1093
1094 l_point[0] = (float) width;
1095 l_point[1] = (float) height;
1096 l_point[2] = 1;
1097
1098 icvMultMatrixTVector3( F, l_point, epiline );
1099 error = icvCrossLines( r_diagonal, epiline, r_point );
1100 assert( error == CV_NO_ERR );
1101
1102 if( r_point[0] >= 0 && r_point[0] <= width )
1103 {
1104
1105 l_start_end[2] = l_point[0];
1106 l_start_end[3] = l_point[1];
1107
1108 r_start_end[2] = r_point[0];
1109 r_start_end[3] = r_point[1];
1110 }
1111 else
1112 return CV_BADFACTOR_ERR;
1113 } /* if */
1114 } /* if */
1115
1116 return CV_NO_ERR;
1117
1118 } /* icvlGetStartEnd4 */
1119
1120 /*===========================================================================*/
1121 CvStatus
icvBuildScanlineLeft(CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,float * l_start_end,int * numlines)1122 icvBuildScanlineLeft( CvMatrix3 * matrix,
1123 CvSize imgSize,
1124 int *scanlines_1, int *scanlines_2, float *l_start_end, int *numlines )
1125 {
1126 int prewarp_height;
1127 float l_point[3];
1128 float r_point[3];
1129 float height;
1130 float delta_x;
1131 float delta_y;
1132 CvStatus error = CV_OK;
1133 CvMatrix3 *F;
1134 float i;
1135 int offset;
1136 float epiline[3];
1137 double a, b;
1138
1139 assert( l_start_end != 0 );
1140
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) );
1144
1145 *numlines = prewarp_height;
1146
1147 if( scanlines_1 == 0 && scanlines_2 == 0 )
1148 return CV_NO_ERR;
1149
1150 F = matrix;
1151
1152
1153 l_point[2] = 1;
1154 height = (float) prewarp_height;
1155
1156 delta_x = (l_start_end[2] - l_start_end[0]) / height;
1157
1158 l_start_end[0] += delta_x;
1159 l_start_end[2] -= delta_x;
1160
1161 delta_x = (l_start_end[2] - l_start_end[0]) / height;
1162 delta_y = (l_start_end[3] - l_start_end[1]) / height;
1163
1164 l_start_end[1] += delta_y;
1165 l_start_end[3] -= delta_y;
1166
1167 delta_y = (l_start_end[3] - l_start_end[1]) / height;
1168
1169 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1170 {
1171
1172 l_point[0] = l_start_end[0] + i * delta_x;
1173 l_point[1] = l_start_end[1] + i * delta_y;
1174
1175 icvMultMatrixTVector3( F, l_point, epiline );
1176
1177 error = icvGetCrossEpilineFrame( imgSize, epiline,
1178 scanlines_2 + offset,
1179 scanlines_2 + offset + 1,
1180 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1181
1182
1183
1184 assert( error == CV_NO_ERR );
1185
1186 r_point[0] = -(float) (*(scanlines_2 + offset));
1187 r_point[1] = -(float) (*(scanlines_2 + offset + 1));
1188 r_point[2] = -1;
1189
1190 icvMultMatrixVector3( F, r_point, epiline );
1191
1192 error = icvGetCrossEpilineFrame( imgSize, epiline,
1193 scanlines_1 + offset,
1194 scanlines_1 + offset + 1,
1195 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1196
1197 assert( error == CV_NO_ERR );
1198 } /* for */
1199
1200 *numlines = prewarp_height;
1201
1202 return error;
1203
1204 } /*icvlBuildScanlineLeft */
1205
1206 /*===========================================================================*/
1207 CvStatus
icvBuildScanlineRight(CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,float * r_start_end,int * numlines)1208 icvBuildScanlineRight( CvMatrix3 * matrix,
1209 CvSize imgSize,
1210 int *scanlines_1, int *scanlines_2, float *r_start_end, int *numlines )
1211 {
1212 int prewarp_height;
1213 float l_point[3];
1214 float r_point[3];
1215 float height;
1216 float delta_x;
1217 float delta_y;
1218 CvStatus error = CV_OK;
1219 CvMatrix3 *F;
1220 float i;
1221 int offset;
1222 float epiline[3];
1223 double a, b;
1224
1225 assert( r_start_end != 0 );
1226
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) );
1230
1231 *numlines = prewarp_height;
1232
1233 if( scanlines_1 == 0 && scanlines_2 == 0 )
1234 return CV_NO_ERR;
1235
1236 F = matrix;
1237
1238 r_point[2] = 1;
1239 height = (float) prewarp_height;
1240
1241 delta_x = (r_start_end[2] - r_start_end[0]) / height;
1242
1243 r_start_end[0] += delta_x;
1244 r_start_end[2] -= delta_x;
1245
1246 delta_x = (r_start_end[2] - r_start_end[0]) / height;
1247 delta_y = (r_start_end[3] - r_start_end[1]) / height;
1248
1249 r_start_end[1] += delta_y;
1250 r_start_end[3] -= delta_y;
1251
1252 delta_y = (r_start_end[3] - r_start_end[1]) / height;
1253
1254 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1255 {
1256
1257 r_point[0] = r_start_end[0] + i * delta_x;
1258 r_point[1] = r_start_end[1] + i * delta_y;
1259
1260 icvMultMatrixVector3( F, r_point, epiline );
1261
1262 error = icvGetCrossEpilineFrame( imgSize, epiline,
1263 scanlines_1 + offset,
1264 scanlines_1 + offset + 1,
1265 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1266
1267
1268 assert( error == CV_NO_ERR );
1269
1270 l_point[0] = -(float) (*(scanlines_1 + offset));
1271 l_point[1] = -(float) (*(scanlines_1 + offset + 1));
1272
1273 l_point[2] = -1;
1274
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 );
1280
1281
1282 assert( error == CV_NO_ERR );
1283 } /* for */
1284
1285 *numlines = prewarp_height;
1286
1287 return error;
1288
1289 } /*icvlBuildScanlineRight */
1290
1291 /*===========================================================================*/
1292 #define Abs(x) ( (x)<0 ? -(x):(x) )
1293 #define Sgn(x) ( (x)<0 ? -1:1 ) /* Sgn(0) = 1 ! */
1294
1295 static CvStatus
icvBuildScanline(CvSize imgSize,float * epiline,float * kx,float * cx,float * ky,float * cy)1296 icvBuildScanline( CvSize imgSize, float *epiline, float *kx, float *cx, float *ky, float *cy )
1297 {
1298 float point[4][2], d;
1299 int sign[4], i;
1300
1301 float width, height;
1302
1303 if( REAL_ZERO( epiline[0] ) && REAL_ZERO( epiline[1] ))
1304 return CV_BADFACTOR_ERR;
1305
1306 width = (float) imgSize.width - 1;
1307 height = (float) imgSize.height - 1;
1308
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] );
1313
1314 i = 0;
1315
1316 if( sign[0] * sign[1] < 0 )
1317 {
1318
1319 point[i][0] = -epiline[2] / epiline[0];
1320 point[i][1] = 0;
1321 i++;
1322 } /* if */
1323
1324 if( sign[0] * sign[2] < 0 )
1325 {
1326
1327 point[i][0] = 0;
1328 point[i][1] = -epiline[2] / epiline[1];
1329 i++;
1330 } /* if */
1331
1332 if( sign[1] * sign[3] < 0 )
1333 {
1334
1335 point[i][0] = width;
1336 point[i][1] = -(epiline[0] * width + epiline[2]) / epiline[1];
1337 i++;
1338 } /* if */
1339
1340 if( sign[2] * sign[3] < 0 )
1341 {
1342
1343 point[i][0] = -(epiline[1] * height + epiline[2]) / epiline[0];
1344 point[i][1] = height;
1345 } /* if */
1346
1347 if( sign[0] == sign[1] && sign[0] == sign[2] && sign[0] == sign[3] )
1348 return CV_BADFACTOR_ERR;
1349
1350 if( !kx && !ky && !cx && !cy )
1351 return CV_BADFACTOR_ERR;
1352
1353 if( kx && ky )
1354 {
1355
1356 *kx = -epiline[1];
1357 *ky = epiline[0];
1358
1359 d = (float) MAX( Abs( *kx ), Abs( *ky ));
1360
1361 *kx /= d;
1362 *ky /= d;
1363 } /* if */
1364
1365 if( cx && cy )
1366 {
1367
1368 if( (point[0][0] - point[1][0]) * epiline[1] +
1369 (point[1][1] - point[0][1]) * epiline[0] > 0 )
1370 {
1371
1372 *cx = point[0][0];
1373 *cy = point[0][1];
1374
1375 }
1376 else
1377 {
1378
1379 *cx = point[1][0];
1380 *cy = point[1][1];
1381 } /* if */
1382 } /* if */
1383
1384 return CV_NO_ERR;
1385
1386 } /* icvlBuildScanline */
1387
1388 /*===========================================================================*/
1389 CvStatus
icvGetCoefficientStereo(CvMatrix3 * matrix,CvSize imgSize,float * l_epipole,float * r_epipole,int * scanlines_1,int * scanlines_2,int * numlines)1390 icvGetCoefficientStereo( CvMatrix3 * matrix,
1391 CvSize imgSize,
1392 float *l_epipole,
1393 float *r_epipole, int *scanlines_1, int *scanlines_2, int *numlines )
1394 {
1395 int i, j, turn;
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;
1401 float swap;
1402
1403 float radius1, radius2, radius3, radius4;
1404
1405 float l_start_end[4], r_start_end[4];
1406 CvMatrix3 *F;
1407 CvStatus error;
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}}
1412 };
1413
1414
1415 width = (float) imgSize.width - 1;
1416 height = (float) imgSize.height - 1;
1417
1418 F = matrix;
1419
1420 if( F->m[0][0] * F->m[1][1] - F->m[1][0] * F->m[0][1] > 0 )
1421 turn = 1;
1422 else
1423 turn = -1;
1424
1425 if( l_epipole[0] < 0 )
1426 i = 0;
1427 else if( l_epipole[0] < width )
1428 i = 1;
1429 else
1430 i = 2;
1431
1432 if( l_epipole[1] < 0 )
1433 j = 2;
1434 else if( l_epipole[1] < height )
1435 j = 1;
1436 else
1437 j = 0;
1438
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];
1443
1444 if( r_epipole[0] < 0 )
1445 i = 0;
1446 else if( r_epipole[0] < width )
1447 i = 1;
1448 else
1449 i = 2;
1450
1451 if( r_epipole[1] < 0 )
1452 j = 2;
1453 else if( r_epipole[1] < height )
1454 j = 1;
1455 else
1456 j = 0;
1457
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];
1462
1463 radius1 = l_epipole[0] * l_epipole[0] + (l_epipole[1] - height) * (l_epipole[1] - height);
1464
1465 radius2 = (l_epipole[0] - width) * (l_epipole[0] - width) +
1466 (l_epipole[1] - height) * (l_epipole[1] - height);
1467
1468 radius3 = l_epipole[0] * l_epipole[0] + l_epipole[1] * l_epipole[1];
1469
1470 radius4 = (l_epipole[0] - width) * (l_epipole[0] - width) + l_epipole[1] * l_epipole[1];
1471
1472
1473 l_radius = (float) sqrt( (double)MAX( MAX( radius1, radius2 ), MAX( radius3, radius4 )));
1474
1475 radius1 = r_epipole[0] * r_epipole[0] + (r_epipole[1] - height) * (r_epipole[1] - height);
1476
1477 radius2 = (r_epipole[0] - width) * (r_epipole[0] - width) +
1478 (r_epipole[1] - height) * (r_epipole[1] - height);
1479
1480 radius3 = r_epipole[0] * r_epipole[0] + r_epipole[1] * r_epipole[1];
1481
1482 radius4 = (r_epipole[0] - width) * (r_epipole[0] - width) + r_epipole[1] * r_epipole[1];
1483
1484
1485 r_radius = (float) sqrt( (double)MAX( MAX( radius1, radius2 ), MAX( radius3, radius4 )));
1486
1487 if( l_start_end[0] == 2 && r_start_end[0] == 2 )
1488
1489 if( l_radius > r_radius )
1490 {
1491
1492 l_angle[0] = 0.0f;
1493 l_angle[1] = (float) CV_PI;
1494
1495 error = icvBuildScanlineLeftStereo( imgSize,
1496 matrix,
1497 l_epipole,
1498 l_angle,
1499 l_radius, scanlines_1, scanlines_2, numlines );
1500
1501 return error;
1502
1503 }
1504 else
1505 {
1506
1507 r_angle[0] = 0.0f;
1508 r_angle[1] = (float) CV_PI;
1509
1510 error = icvBuildScanlineRightStereo( imgSize,
1511 matrix,
1512 r_epipole,
1513 r_angle,
1514 r_radius,
1515 scanlines_1, scanlines_2, numlines );
1516
1517 return error;
1518 } /* if */
1519
1520 if( l_start_end[0] == 2 )
1521 {
1522
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] );
1527
1528 if( r_angle[0] > r_angle[1] )
1529 r_angle[1] += (float) (CV_PI * 2);
1530
1531 error = icvBuildScanlineRightStereo( imgSize,
1532 matrix,
1533 r_epipole,
1534 r_angle,
1535 r_radius, scanlines_1, scanlines_2, numlines );
1536
1537 return error;
1538 } /* if */
1539
1540 if( r_start_end[0] == 2 )
1541 {
1542
1543 l_point[0] = l_start_end[0] * width;
1544 l_point[1] = l_start_end[1] * height;
1545 l_point[2] = 1;
1546
1547 icvMultMatrixTVector3( F, l_point, r_epiline );
1548
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] );
1553
1554 if( l_angle[0] > l_angle[1] )
1555 l_angle[1] += (float) (CV_PI * 2);
1556
1557 error = icvBuildScanlineLeftStereo( imgSize,
1558 matrix,
1559 l_epipole,
1560 l_angle,
1561 l_radius, scanlines_1, scanlines_2, numlines );
1562
1563 return error;
1564
1565 } /* if */
1566
1567 l_start_end[0] *= width;
1568 l_start_end[1] *= height;
1569 l_start_end[2] *= width;
1570 l_start_end[3] *= height;
1571
1572 r_start_end[0] *= width;
1573 r_start_end[1] *= height;
1574 r_start_end[2] *= width;
1575 r_start_end[3] *= height;
1576
1577 r_point[0] = r_start_end[0];
1578 r_point[1] = r_start_end[1];
1579 r_point[2] = 1;
1580
1581 icvMultMatrixVector3( F, r_point, l_epiline );
1582 error = icvBuildScanline( imgSize, l_epiline, 0, &x, 0, &y );
1583
1584 if( error == CV_NO_ERR )
1585 {
1586
1587 l_angle[0] = (float) atan2( y - l_epipole[1], x - l_epipole[0] );
1588
1589 r_angle[0] = (float) atan2( r_point[1] - r_epipole[1], r_point[0] - r_epipole[0] );
1590
1591 }
1592 else
1593 {
1594
1595 if( turn == 1 )
1596 {
1597
1598 l_point[0] = l_start_end[0];
1599 l_point[1] = l_start_end[1];
1600
1601 }
1602 else
1603 {
1604
1605 l_point[0] = l_start_end[2];
1606 l_point[1] = l_start_end[3];
1607 } /* if */
1608
1609 l_point[2] = 1;
1610
1611 icvMultMatrixTVector3( F, l_point, r_epiline );
1612 error = icvBuildScanline( imgSize, r_epiline, 0, &x, 0, &y );
1613
1614 if( error == CV_NO_ERR )
1615 {
1616
1617 r_angle[0] = (float) atan2( y - r_epipole[1], x - r_epipole[0] );
1618
1619 l_angle[0] = (float) atan2( l_point[1] - l_epipole[1], l_point[0] - l_epipole[0] );
1620
1621 }
1622 else
1623 return CV_BADFACTOR_ERR;
1624 } /* if */
1625
1626 r_point[0] = r_start_end[2];
1627 r_point[1] = r_start_end[3];
1628 r_point[2] = 1;
1629
1630 icvMultMatrixVector3( F, r_point, l_epiline );
1631 error = icvBuildScanline( imgSize, l_epiline, 0, &x, 0, &y );
1632
1633 if( error == CV_NO_ERR )
1634 {
1635
1636 l_angle[1] = (float) atan2( y - l_epipole[1], x - l_epipole[0] );
1637
1638 r_angle[1] = (float) atan2( r_point[1] - r_epipole[1], r_point[0] - r_epipole[0] );
1639
1640 }
1641 else
1642 {
1643
1644 if( turn == 1 )
1645 {
1646
1647 l_point[0] = l_start_end[2];
1648 l_point[1] = l_start_end[3];
1649
1650 }
1651 else
1652 {
1653
1654 l_point[0] = l_start_end[0];
1655 l_point[1] = l_start_end[1];
1656 } /* if */
1657
1658 l_point[2] = 1;
1659
1660 icvMultMatrixTVector3( F, l_point, r_epiline );
1661 error = icvBuildScanline( imgSize, r_epiline, 0, &x, 0, &y );
1662
1663 if( error == CV_NO_ERR )
1664 {
1665
1666 r_angle[1] = (float) atan2( y - r_epipole[1], x - r_epipole[0] );
1667
1668 l_angle[1] = (float) atan2( l_point[1] - l_epipole[1], l_point[0] - l_epipole[0] );
1669
1670 }
1671 else
1672 return CV_BADFACTOR_ERR;
1673 } /* if */
1674
1675 if( l_angle[0] > l_angle[1] )
1676 {
1677
1678 swap = l_angle[0];
1679 l_angle[0] = l_angle[1];
1680 l_angle[1] = swap;
1681 } /* if */
1682
1683 if( l_angle[1] - l_angle[0] > CV_PI )
1684 {
1685
1686 swap = l_angle[0];
1687 l_angle[0] = l_angle[1];
1688 l_angle[1] = swap + (float) (CV_PI * 2);
1689 } /* if */
1690
1691 if( r_angle[0] > r_angle[1] )
1692 {
1693
1694 swap = r_angle[0];
1695 r_angle[0] = r_angle[1];
1696 r_angle[1] = swap;
1697 } /* if */
1698
1699 if( r_angle[1] - r_angle[0] > CV_PI )
1700 {
1701
1702 swap = r_angle[0];
1703 r_angle[0] = r_angle[1];
1704 r_angle[1] = swap + (float) (CV_PI * 2);
1705 } /* if */
1706
1707 if( l_radius * (l_angle[1] - l_angle[0]) > r_radius * (r_angle[1] - r_angle[0]) )
1708 error = icvBuildScanlineLeftStereo( imgSize,
1709 matrix,
1710 l_epipole,
1711 l_angle,
1712 l_radius, scanlines_1, scanlines_2, numlines );
1713
1714 else
1715 error = icvBuildScanlineRightStereo( imgSize,
1716 matrix,
1717 r_epipole,
1718 r_angle,
1719 r_radius, scanlines_1, scanlines_2, numlines );
1720
1721
1722 return error;
1723
1724 } /* icvGetCoefficientStereo */
1725
1726 /*===========================================================================*/
1727 CvStatus
icvBuildScanlineLeftStereo(CvSize imgSize,CvMatrix3 * matrix,float * l_epipole,float * l_angle,float l_radius,int * scanlines_1,int * scanlines_2,int * numlines)1728 icvBuildScanlineLeftStereo( CvSize imgSize,
1729 CvMatrix3 * matrix,
1730 float *l_epipole,
1731 float *l_angle,
1732 float l_radius, int *scanlines_1, int *scanlines_2, int *numlines )
1733 {
1734 //int prewarp_width;
1735 int prewarp_height;
1736 float i;
1737 int offset;
1738 float height;
1739 float delta;
1740 float angle;
1741 float l_point[3];
1742 float l_epiline[3];
1743 float r_epiline[3];
1744 CvStatus error = CV_OK;
1745 CvMatrix3 *F;
1746
1747
1748 assert( l_angle != 0 && !REAL_ZERO( l_radius ));
1749
1750 /*prewarp_width = (int) (sqrt( image_width * image_width +
1751 image_height * image_height ) + 1);*/
1752
1753 prewarp_height = (int) (l_radius * (l_angle[1] - l_angle[0]));
1754
1755 *numlines = prewarp_height;
1756
1757 if( scanlines_1 == 0 && scanlines_2 == 0 )
1758 return CV_NO_ERR;
1759
1760 F = matrix;
1761
1762 l_point[2] = 1;
1763 height = (float) prewarp_height;
1764
1765 delta = (l_angle[1] - l_angle[0]) / height;
1766
1767 l_angle[0] += delta;
1768 l_angle[1] -= delta;
1769
1770 delta = (l_angle[1] - l_angle[0]) / height;
1771
1772 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1773 {
1774
1775 angle = l_angle[0] + i * delta;
1776
1777 l_point[0] = l_epipole[0] + l_radius * (float) cos( angle );
1778 l_point[1] = l_epipole[1] + l_radius * (float) sin( angle );
1779
1780 icvMultMatrixTVector3( F, l_point, r_epiline );
1781
1782 error = icvGetCrossEpilineFrame( imgSize, r_epiline,
1783 scanlines_2 + offset,
1784 scanlines_2 + offset + 1,
1785 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1786
1787
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];
1791
1792 if( Sgn( l_epiline[0] * r_epiline[0] + l_epiline[1] * r_epiline[1] ) < 0 )
1793 {
1794
1795 l_epiline[0] = -l_epiline[0];
1796 l_epiline[1] = -l_epiline[1];
1797 l_epiline[2] = -l_epiline[2];
1798 } /* if */
1799
1800 error = icvGetCrossEpilineFrame( imgSize, l_epiline,
1801 scanlines_1 + offset,
1802 scanlines_1 + offset + 1,
1803 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1804
1805 } /* for */
1806
1807 *numlines = prewarp_height;
1808
1809 return error;
1810
1811 } /* icvlBuildScanlineLeftStereo */
1812
1813 /*===========================================================================*/
1814 CvStatus
icvBuildScanlineRightStereo(CvSize imgSize,CvMatrix3 * matrix,float * r_epipole,float * r_angle,float r_radius,int * scanlines_1,int * scanlines_2,int * numlines)1815 icvBuildScanlineRightStereo( CvSize imgSize,
1816 CvMatrix3 * matrix,
1817 float *r_epipole,
1818 float *r_angle,
1819 float r_radius,
1820 int *scanlines_1, int *scanlines_2, int *numlines )
1821 {
1822 //int prewarp_width;
1823 int prewarp_height;
1824 float i;
1825 int offset;
1826 float height;
1827 float delta;
1828 float angle;
1829 float r_point[3];
1830 float l_epiline[3];
1831 float r_epiline[3];
1832 CvStatus error = CV_OK;
1833 CvMatrix3 *F;
1834
1835 assert( r_angle != 0 && !REAL_ZERO( r_radius ));
1836
1837 /*prewarp_width = (int) (sqrt( image_width * image_width +
1838 image_height * image_height ) + 1);*/
1839
1840 prewarp_height = (int) (r_radius * (r_angle[1] - r_angle[0]));
1841
1842 *numlines = prewarp_height;
1843
1844 if( scanlines_1 == 0 && scanlines_2 == 0 )
1845 return CV_NO_ERR;
1846
1847 F = matrix;
1848
1849 r_point[2] = 1;
1850 height = (float) prewarp_height;
1851
1852 delta = (r_angle[1] - r_angle[0]) / height;
1853
1854 r_angle[0] += delta;
1855 r_angle[1] -= delta;
1856
1857 delta = (r_angle[1] - r_angle[0]) / height;
1858
1859 for( i = 0, offset = 0; i < height; i++, offset += 4 )
1860 {
1861
1862 angle = r_angle[0] + i * delta;
1863
1864 r_point[0] = r_epipole[0] + r_radius * (float) cos( angle );
1865 r_point[1] = r_epipole[1] + r_radius * (float) sin( angle );
1866
1867 icvMultMatrixVector3( F, r_point, l_epiline );
1868
1869 error = icvGetCrossEpilineFrame( imgSize, l_epiline,
1870 scanlines_1 + offset,
1871 scanlines_1 + offset + 1,
1872 scanlines_1 + offset + 2, scanlines_1 + offset + 3 );
1873
1874 assert( error == CV_NO_ERR );
1875
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];
1879
1880 if( Sgn( l_epiline[0] * r_epiline[0] + l_epiline[1] * r_epiline[1] ) < 0 )
1881 {
1882
1883 r_epiline[0] = -r_epiline[0];
1884 r_epiline[1] = -r_epiline[1];
1885 r_epiline[2] = -r_epiline[2];
1886 } /* if */
1887
1888 error = icvGetCrossEpilineFrame( imgSize, r_epiline,
1889 scanlines_2 + offset,
1890 scanlines_2 + offset + 1,
1891 scanlines_2 + offset + 2, scanlines_2 + offset + 3 );
1892
1893 assert( error == CV_NO_ERR );
1894 } /* for */
1895
1896 *numlines = prewarp_height;
1897
1898 return error;
1899
1900 } /* icvlBuildScanlineRightStereo */
1901
1902 /*===========================================================================*/
1903 CvStatus
icvGetCrossEpilineFrame(CvSize imgSize,float * epiline,int * x1,int * y1,int * x2,int * y2)1904 icvGetCrossEpilineFrame( CvSize imgSize, float *epiline, int *x1, int *y1, int *x2, int *y2 )
1905 {
1906 int tx, ty;
1907 float point[2][2];
1908 int sign[4], i;
1909 float width, height;
1910 double tmpvalue;
1911
1912 if( REAL_ZERO( epiline[0] ) && REAL_ZERO( epiline[1] ))
1913 return CV_BADFACTOR_ERR;
1914
1915 width = (float) imgSize.width - 1;
1916 height = (float) imgSize.height - 1;
1917
1918 tmpvalue = epiline[2];
1919 sign[0] = SIGN( tmpvalue );
1920
1921 tmpvalue = epiline[0] * width + epiline[2];
1922 sign[1] = SIGN( tmpvalue );
1923
1924 tmpvalue = epiline[1] * height + epiline[2];
1925 sign[2] = SIGN( tmpvalue );
1926
1927 tmpvalue = epiline[0] * width + epiline[1] * height + epiline[2];
1928 sign[3] = SIGN( tmpvalue );
1929
1930 i = 0;
1931 for( tx = 0; tx < 2; tx++ )
1932 {
1933 for( ty = 0; ty < 2; ty++ )
1934 {
1935
1936 if( sign[ty * 2 + tx] == 0 )
1937 {
1938
1939 point[i][0] = width * tx;
1940 point[i][1] = height * ty;
1941 i++;
1942
1943 } /* if */
1944 } /* for */
1945 } /* for */
1946
1947 if( sign[0] * sign[1] < 0 )
1948 {
1949 point[i][0] = -epiline[2] / epiline[0];
1950 point[i][1] = 0;
1951 i++;
1952 } /* if */
1953
1954 if( sign[0] * sign[2] < 0 )
1955 {
1956 point[i][0] = 0;
1957 point[i][1] = -epiline[2] / epiline[1];
1958 i++;
1959 } /* if */
1960
1961 if( sign[1] * sign[3] < 0 )
1962 {
1963 point[i][0] = width;
1964 point[i][1] = -(epiline[0] * width + epiline[2]) / epiline[1];
1965 i++;
1966 } /* if */
1967
1968 if( sign[2] * sign[3] < 0 )
1969 {
1970 point[i][0] = -(epiline[1] * height + epiline[2]) / epiline[0];
1971 point[i][1] = height;
1972 } /* if */
1973
1974 if( sign[0] == sign[1] && sign[0] == sign[2] && sign[0] == sign[3] )
1975 return CV_BADFACTOR_ERR;
1976
1977 if( (point[0][0] - point[1][0]) * epiline[1] +
1978 (point[1][1] - point[0][1]) * epiline[0] > 0 )
1979 {
1980 *x1 = (int) point[0][0];
1981 *y1 = (int) point[0][1];
1982 *x2 = (int) point[1][0];
1983 *y2 = (int) point[1][1];
1984 }
1985 else
1986 {
1987 *x1 = (int) point[1][0];
1988 *y1 = (int) point[1][1];
1989 *x2 = (int) point[0][0];
1990 *y2 = (int) point[0][1];
1991 } /* if */
1992
1993 return CV_NO_ERR;
1994 } /* icvlGetCrossEpilineFrame */
1995
1996 /*=====================================================================================*/
1997
1998 CV_IMPL void
cvMakeScanlines(const CvMatrix3 * matrix,CvSize imgSize,int * scanlines_1,int * scanlines_2,int * lens_1,int * lens_2,int * numlines)1999 cvMakeScanlines( const CvMatrix3* matrix, CvSize imgSize,
2000 int *scanlines_1, int *scanlines_2,
2001 int *lens_1, int *lens_2, int *numlines )
2002 {
2003 CV_FUNCNAME( "cvMakeScanlines" );
2004
2005 __BEGIN__;
2006
2007 IPPI_CALL( icvMakeScanlines( (CvMatrix3*)matrix, imgSize, scanlines_1,
2008 scanlines_2, lens_1, lens_2, numlines ));
2009 __END__;
2010 }
2011
2012 /*F///////////////////////////////////////////////////////////////////////////////////////
2013 // Name: cvDeleteMoire
2014 // Purpose: The functions
2015 // Context:
2016 // Parameters:
2017 //
2018 // Notes:
2019 //F*/
2020 CV_IMPL void
cvMakeAlphaScanlines(int * scanlines_1,int * scanlines_2,int * scanlines_a,int * lens,int numlines,float alpha)2021 cvMakeAlphaScanlines( int *scanlines_1,
2022 int *scanlines_2,
2023 int *scanlines_a, int *lens, int numlines, float alpha )
2024 {
2025 CV_FUNCNAME( "cvMakeAlphaScanlines" );
2026
2027 __BEGIN__;
2028
2029 IPPI_CALL( icvMakeAlphaScanlines( scanlines_1, scanlines_2, scanlines_a,
2030 lens, numlines, alpha ));
2031
2032 __END__;
2033 }
2034