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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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
