vcs.maemo.org Git - opencv/blob - cvaux/src/cvscanlines.cpp

   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 }