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