modules/legacy/src/epilines.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
  42
  43 #include "precomp.hpp"
  44 #include <float.h>
  45 #include <limits.h>
  46
  47 /* Valery Mosyagin */
  48
  49 #undef quad
  50
  51 #define EPS64D 1e-9
  52
  53 int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
  54                                     CvMatr32f transVect,
  55                                     CvMatr32f essMatr);
  56
  57 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
  58                                          CvMatr32f fundMatr,
  59                                          CvMatr32f cameraMatr1,
  60                                          CvMatr32f cameraMatr2);
  61
  62 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
  63                                          CvPoint3D32f* epipole1,
  64                                          CvPoint3D32f* epipole2);
  65
  66 void icvTestPoint( CvPoint2D64d testPoint,
  67                 CvVect64d line1,CvVect64d line2,
  68                 CvPoint2D64d basePoint,
  69                 int* result);
  70
  71
  72
  73 int icvGetSymPoint3D(  CvPoint3D64d pointCorner,
  74                             CvPoint3D64d point1,
  75                             CvPoint3D64d point2,
  76                             CvPoint3D64d *pointSym2)
  77 {
  78     double len1,len2;
  79     double alpha;
  80     icvGetPieceLength3D(pointCorner,point1,&len1);
  81     if( len1 < EPS64D )
  82     {
  83         return CV_BADARG_ERR;
  84     }
  85     icvGetPieceLength3D(pointCorner,point2,&len2);
  86     alpha = len2 / len1;
  87
  88     pointSym2->x = pointCorner.x + alpha*(point1.x - pointCorner.x);
  89     pointSym2->y = pointCorner.y + alpha*(point1.y - pointCorner.y);
  90     pointSym2->z = pointCorner.z + alpha*(point1.z - pointCorner.z);
  91     return CV_NO_ERR;
  92 }
  93
  94 /*  author Valery Mosyagin */
  95
  96 /* Compute 3D point for scanline and alpha betta */
  97 int icvCompute3DPoint( double alpha,double betta,
  98                             CvStereoLineCoeff* coeffs,
  99                             CvPoint3D64d* point)
 100 {
 101
 102     double partX;
 103     double partY;
 104     double partZ;
 105     double partAll;
 106     double invPartAll;
 107
 108     double alphabetta = alpha*betta;
 109
 110     partAll = alpha - betta;
 111     if( fabs(partAll) > 0.00001  ) /* alpha must be > betta */
 112     {
 113
 114         partX   = coeffs->Xcoef        + coeffs->XcoefA *alpha +
 115                   coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta;
 116
 117         partY   = coeffs->Ycoef        + coeffs->YcoefA *alpha +
 118                   coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta;
 119
 120         partZ   = coeffs->Zcoef        + coeffs->ZcoefA *alpha +
 121                   coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta;
 122
 123         invPartAll = 1.0 / partAll;
 124
 125         point->x = partX * invPartAll;
 126         point->y = partY * invPartAll;
 127         point->z = partZ * invPartAll;
 128         return CV_NO_ERR;
 129     }
 130     else
 131     {
 132         return CV_BADFACTOR_ERR;
 133     }
 134 }
 135
 136 /*--------------------------------------------------------------------------------------*/
 137
 138 /* Compute rotate matrix and trans vector for change system */
 139 int icvCreateConvertMatrVect( CvMatr64d     rotMatr1,
 140                                 CvMatr64d     transVect1,
 141                                 CvMatr64d     rotMatr2,
 142                                 CvMatr64d     transVect2,
 143                                 CvMatr64d     convRotMatr,
 144                                 CvMatr64d     convTransVect)
 145 {
 146     double invRotMatr2[9];
 147     double tmpVect[3];
 148
 149
 150     icvInvertMatrix_64d(rotMatr2,3,invRotMatr2);
 151     /* Test for error */
 152
 153     icvMulMatrix_64d(   rotMatr1,
 154                         3,3,
 155                         invRotMatr2,
 156                         3,3,
 157                         convRotMatr);
 158
 159     icvMulMatrix_64d(   convRotMatr,
 160                         3,3,
 161                         transVect2,
 162                         1,3,
 163                         tmpVect);
 164
 165     icvSubVector_64d(transVect1,tmpVect,convTransVect,3);
 166
 167
 168     return CV_NO_ERR;
 169 }
 170
 171 /*--------------------------------------------------------------------------------------*/
 172
 173 /* Compute point coordinates in other system */
 174 int icvConvertPointSystem(CvPoint3D64d  M2,
 175                             CvPoint3D64d* M1,
 176                             CvMatr64d     rotMatr,
 177                             CvMatr64d     transVect
 178                             )
 179 {
 180     double tmpVect[3];
 181
 182     icvMulMatrix_64d(   rotMatr,
 183                         3,3,
 184                         (double*)&M2,
 185                         1,3,
 186                         tmpVect);
 187
 188     icvAddVector_64d(tmpVect,transVect,(double*)M1,3);
 189
 190     return CV_NO_ERR;
 191 }
 192 /*--------------------------------------------------------------------------------------*/
 193 int icvComputeCoeffForStereoV3( double quad1[4][2],
 194                                 double quad2[4][2],
 195                                 int    numScanlines,
 196                                 CvMatr64d    camMatr1,
 197                                 CvMatr64d    rotMatr1,
 198                                 CvMatr64d    transVect1,
 199                                 CvMatr64d    camMatr2,
 200                                 CvMatr64d    rotMatr2,
 201                                 CvMatr64d    transVect2,
 202                                 CvStereoLineCoeff*    startCoeffs,
 203                                 int* needSwapCamera)
 204 {
 205     /* For each pair */
 206     /* In this function we must define position of cameras */
 207
 208     CvPoint2D64d point1;
 209     CvPoint2D64d point2;
 210     CvPoint2D64d point3;
 211     CvPoint2D64d point4;
 212
 213     int currLine;
 214     *needSwapCamera = 0;
 215     for( currLine = 0; currLine < numScanlines; currLine++ )
 216     {
 217         /* Compute points */
 218         double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */
 219
 220         point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
 221         point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];
 222
 223         point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
 224         point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];
 225
 226         point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
 227         point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];
 228
 229         point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
 230         point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];
 231
 232         /* We can compute coeffs for this line */
 233         icvComCoeffForLine(    point1,
 234                             point2,
 235                             point3,
 236                             point4,
 237                             camMatr1,
 238                             rotMatr1,
 239                             transVect1,
 240                             camMatr2,
 241                             rotMatr2,
 242                             transVect2,
 243                             &startCoeffs[currLine],
 244                             needSwapCamera);
 245     }
 246     return CV_NO_ERR;
 247 }
 248 /*--------------------------------------------------------------------------------------*/
 249 int icvComputeCoeffForStereoNew(   double quad1[4][2],
 250                                         double quad2[4][2],
 251                                         int    numScanlines,
 252                                         CvMatr32f    camMatr1,
 253                                         CvMatr32f    rotMatr1,
 254                                         CvMatr32f    transVect1,
 255                                         CvMatr32f    camMatr2,
 256                                         CvStereoLineCoeff*    startCoeffs,
 257                                         int* needSwapCamera)
 258 {
 259     /* Convert data */
 260
 261     double camMatr1_64d[9];
 262     double camMatr2_64d[9];
 263
 264     double rotMatr1_64d[9];
 265     double transVect1_64d[3];
 266
 267     double rotMatr2_64d[9];
 268     double transVect2_64d[3];
 269
 270     icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
 271     icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
 272
 273     icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
 274     icvCvt_32f_64d(transVect1,transVect1_64d,3);
 275
 276     rotMatr2_64d[0] = 1;
 277     rotMatr2_64d[1] = 0;
 278     rotMatr2_64d[2] = 0;
 279     rotMatr2_64d[3] = 0;
 280     rotMatr2_64d[4] = 1;
 281     rotMatr2_64d[5] = 0;
 282     rotMatr2_64d[6] = 0;
 283     rotMatr2_64d[7] = 0;
 284     rotMatr2_64d[8] = 1;
 285
 286     transVect2_64d[0] = 0;
 287     transVect2_64d[1] = 0;
 288     transVect2_64d[2] = 0;
 289
 290     int status = icvComputeCoeffForStereoV3( quad1,
 291                                                 quad2,
 292                                                 numScanlines,
 293                                                 camMatr1_64d,
 294                                                 rotMatr1_64d,
 295                                                 transVect1_64d,
 296                                                 camMatr2_64d,
 297                                                 rotMatr2_64d,
 298                                                 transVect2_64d,
 299                                                 startCoeffs,
 300                                                 needSwapCamera);
 301
 302
 303     return status;
 304
 305 }
 306 /*--------------------------------------------------------------------------------------*/
 307 int icvComputeCoeffForStereo(  CvStereoCamera* stereoCamera)
 308 {
 309     double quad1[4][2];
 310     double quad2[4][2];
 311
 312     int i;
 313     for( i = 0; i < 4; i++ )
 314     {
 315         quad1[i][0] = stereoCamera->quad[0][i].x;
 316         quad1[i][1] = stereoCamera->quad[0][i].y;
 317
 318         quad2[i][0] = stereoCamera->quad[1][i].x;
 319         quad2[i][1] = stereoCamera->quad[1][i].y;
 320     }
 321
 322     icvComputeCoeffForStereoNew(        quad1,
 323                                         quad2,
 324                                         stereoCamera->warpSize.height,
 325                                         stereoCamera->camera[0]->matrix,
 326                                         stereoCamera->rotMatrix,
 327                                         stereoCamera->transVector,
 328                                         stereoCamera->camera[1]->matrix,
 329                                         stereoCamera->lineCoeffs,
 330                                         &(stereoCamera->needSwapCameras));
 331     return CV_OK;
 332 }
 333
 334
 335 /*--------------------------------------------------------------------------------------*/
 336 int icvComCoeffForLine(   CvPoint2D64d point1,
 337                             CvPoint2D64d point2,
 338                             CvPoint2D64d point3,
 339                             CvPoint2D64d point4,
 340                             CvMatr64d    camMatr1,
 341                             CvMatr64d    rotMatr1,
 342                             CvMatr64d    transVect1,
 343                             CvMatr64d    camMatr2,
 344                             CvMatr64d    rotMatr2,
 345                             CvMatr64d    transVect2,
 346                             CvStereoLineCoeff* coeffs,
 347                             int* needSwapCamera)
 348 {
 349     /* Get direction for all points */
 350     /* Direction for camera 1 */
 351
 352     CvPoint3D64f direct1;
 353     CvPoint3D64f direct2;
 354     CvPoint3D64f camPoint1;
 355
 356     CvPoint3D64f directS3;
 357     CvPoint3D64f directS4;
 358     CvPoint3D64f direct3;
 359     CvPoint3D64f direct4;
 360     CvPoint3D64f camPoint2;
 361
 362     icvGetDirectionForPoint(   point1,
 363                             camMatr1,
 364                             &direct1);
 365
 366     icvGetDirectionForPoint(   point2,
 367                             camMatr1,
 368                             &direct2);
 369
 370     /* Direction for camera 2 */
 371
 372     icvGetDirectionForPoint(   point3,
 373                             camMatr2,
 374                             &directS3);
 375
 376     icvGetDirectionForPoint(   point4,
 377                             camMatr2,
 378                             &directS4);
 379
 380     /* Create convertion for camera 2: two direction and camera point */
 381
 382     double convRotMatr[9];
 383     double convTransVect[3];
 384
 385     icvCreateConvertMatrVect(  rotMatr1,
 386                             transVect1,
 387                             rotMatr2,
 388                             transVect2,
 389                             convRotMatr,
 390                             convTransVect);
 391
 392     CvPoint3D64f zeroVect;
 393     zeroVect.x = zeroVect.y = zeroVect.z = 0.0;
 394     camPoint1.x = camPoint1.y = camPoint1.z = 0.0;
 395
 396     icvConvertPointSystem(directS3,&direct3,convRotMatr,convTransVect);
 397     icvConvertPointSystem(directS4,&direct4,convRotMatr,convTransVect);
 398     icvConvertPointSystem(zeroVect,&camPoint2,convRotMatr,convTransVect);
 399
 400     CvPoint3D64f pointB;
 401
 402     int postype = 0;
 403
 404     /* Changed order */
 405     /* Compute point B: xB,yB,zB */
 406     icvGetCrossLines(camPoint1,direct2,
 407                   camPoint2,direct3,
 408                   &pointB);
 409
 410     if( pointB.z < 0 )/* If negative use other lines for cross */
 411     {
 412         postype = 1;
 413         icvGetCrossLines(camPoint1,direct1,
 414                       camPoint2,direct4,
 415                       &pointB);
 416     }
 417
 418     CvPoint3D64d pointNewA;
 419     CvPoint3D64d pointNewC;
 420
 421     pointNewA.x = pointNewA.y = pointNewA.z = 0;
 422     pointNewC.x = pointNewC.y = pointNewC.z = 0;
 423
 424     if( postype == 0 )
 425     {
 426         icvGetSymPoint3D(   camPoint1,
 427                             direct1,
 428                             pointB,
 429                             &pointNewA);
 430
 431         icvGetSymPoint3D(   camPoint2,
 432                             direct4,
 433                             pointB,
 434                             &pointNewC);
 435     }
 436     else
 437     {/* In this case we must change cameras */
 438         *needSwapCamera = 1;
 439         icvGetSymPoint3D(   camPoint2,
 440                             direct3,
 441                             pointB,
 442                             &pointNewA);
 443
 444         icvGetSymPoint3D(   camPoint1,
 445                             direct2,
 446                             pointB,
 447                             &pointNewC);
 448     }
 449
 450
 451     double gamma;
 452
 453     double xA,yA,zA;
 454     double xB,yB,zB;
 455     double xC,yC,zC;
 456
 457     xA = pointNewA.x;
 458     yA = pointNewA.y;
 459     zA = pointNewA.z;
 460
 461     xB = pointB.x;
 462     yB = pointB.y;
 463     zB = pointB.z;
 464
 465     xC = pointNewC.x;
 466     yC = pointNewC.y;
 467     zC = pointNewC.z;
 468
 469     double len1,len2;
 470     len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) );
 471     len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) );
 472     gamma = len2 / len1;
 473
 474     icvComputeStereoLineCoeffs( pointNewA,
 475                                 pointB,
 476                                 camPoint1,
 477                                 gamma,
 478                                 coeffs);
 479
 480     return CV_NO_ERR;
 481 }
 482
 483
 484 /*--------------------------------------------------------------------------------------*/
 485
 486 int icvGetDirectionForPoint(  CvPoint2D64d point,
 487                                 CvMatr64d camMatr,
 488                                 CvPoint3D64d* direct)
 489 {
 490     /*  */
 491     double invMatr[9];
 492
 493     /* Invert matrix */
 494
 495     icvInvertMatrix_64d(camMatr,3,invMatr);
 496     /* TEST FOR ERRORS */
 497
 498     double vect[3];
 499     vect[0] = point.x;
 500     vect[1] = point.y;
 501     vect[2] = 1;
 502
 503     /* Mul matr */
 504     icvMulMatrix_64d(   invMatr,
 505                         3,3,
 506                         vect,
 507                         1,3,
 508                         (double*)direct);
 509
 510     return CV_NO_ERR;
 511 }
 512
 513 /*--------------------------------------------------------------------------------------*/
 514
 515 int icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,
 516                        CvPoint3D64d point21,CvPoint3D64d point22,
 517                        CvPoint3D64d* midPoint)
 518 {
 519     double xM,yM,zM;
 520     double xN,yN,zN;
 521
 522     double xA,yA,zA;
 523     double xB,yB,zB;
 524
 525     double xC,yC,zC;
 526     double xD,yD,zD;
 527
 528     xA = point11.x;
 529     yA = point11.y;
 530     zA = point11.z;
 531
 532     xB = point12.x;
 533     yB = point12.y;
 534     zB = point12.z;
 535
 536     xC = point21.x;
 537     yC = point21.y;
 538     zC = point21.z;
 539
 540     xD = point22.x;
 541     yD = point22.y;
 542     zD = point22.z;
 543
 544     double a11,a12,a21,a22;
 545     double b1,b2;
 546
 547     a11 =  (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA);
 548     a12 = -(xD-xC)*(xB-xA)-(yD-yC)*(yB-yA)-(zD-zC)*(zB-zA);
 549     a21 =  (xB-xA)*(xD-xC)+(yB-yA)*(yD-yC)+(zB-zA)*(zD-zC);
 550     a22 = -(xD-xC)*(xD-xC)-(yD-yC)*(yD-yC)-(zD-zC)*(zD-zC);
 551     b1  = -( (xA-xC)*(xB-xA)+(yA-yC)*(yB-yA)+(zA-zC)*(zB-zA) );
 552     b2  = -( (xA-xC)*(xD-xC)+(yA-yC)*(yD-yC)+(zA-zC)*(zD-zC) );
 553
 554     double delta;
 555     double deltaA,deltaB;
 556     double alpha,betta;
 557
 558     delta  = a11*a22-a12*a21;
 559
 560     if( fabs(delta) < EPS64D )
 561     {
 562         /*return ERROR;*/
 563     }
 564
 565     deltaA = b1*a22-b2*a12;
 566     deltaB = a11*b2-b1*a21;
 567
 568     alpha = deltaA / delta;
 569     betta = deltaB / delta;
 570
 571     xM = xA+alpha*(xB-xA);
 572     yM = yA+alpha*(yB-yA);
 573     zM = zA+alpha*(zB-zA);
 574
 575     xN = xC+betta*(xD-xC);
 576     yN = yC+betta*(yD-yC);
 577     zN = zC+betta*(zD-zC);
 578
 579     /* Compute middle point */
 580     midPoint->x = (xM + xN) * 0.5;
 581     midPoint->y = (yM + yN) * 0.5;
 582     midPoint->z = (zM + zN) * 0.5;
 583
 584     return CV_NO_ERR;
 585 }
 586
 587 /*--------------------------------------------------------------------------------------*/
 588
 589 int icvComputeStereoLineCoeffs(   CvPoint3D64d pointA,
 590                                     CvPoint3D64d pointB,
 591                                     CvPoint3D64d pointCam1,
 592                                     double gamma,
 593                                     CvStereoLineCoeff*    coeffs)
 594 {
 595     double x1,y1,z1;
 596
 597     x1 = pointCam1.x;
 598     y1 = pointCam1.y;
 599     z1 = pointCam1.z;
 600
 601     double xA,yA,zA;
 602     double xB,yB,zB;
 603
 604     xA = pointA.x;
 605     yA = pointA.y;
 606     zA = pointA.z;
 607
 608     xB = pointB.x;
 609     yB = pointB.y;
 610     zB = pointB.z;
 611
 612     if( gamma > 0 )
 613     {
 614         coeffs->Xcoef   = -x1 + xA;
 615         coeffs->XcoefA  =  xB + x1 - xA;
 616         coeffs->XcoefB  = -xA - gamma * x1 + gamma * xA;
 617         coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;
 618
 619         coeffs->Ycoef   = -y1 + yA;
 620         coeffs->YcoefA  =  yB + y1 - yA;
 621         coeffs->YcoefB  = -yA - gamma * y1 + gamma * yA;
 622         coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;
 623
 624         coeffs->Zcoef   = -z1 + zA;
 625         coeffs->ZcoefA  =  zB + z1 - zA;
 626         coeffs->ZcoefB  = -zA - gamma * z1 + gamma * zA;
 627         coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
 628     }
 629     else
 630     {
 631         gamma = - gamma;
 632         coeffs->Xcoef   = -( -x1 + xA);
 633         coeffs->XcoefB  = -(  xB + x1 - xA);
 634         coeffs->XcoefA  = -( -xA - gamma * x1 + gamma * xA);
 635         coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA);
 636
 637         coeffs->Ycoef   = -( -y1 + yA);
 638         coeffs->YcoefB  = -(  yB + y1 - yA);
 639         coeffs->YcoefA  = -( -yA - gamma * y1 + gamma * yA);
 640         coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA);
 641
 642         coeffs->Zcoef   = -( -z1 + zA);
 643         coeffs->ZcoefB  = -(  zB + z1 - zA);
 644         coeffs->ZcoefA  = -( -zA - gamma * z1 + gamma * zA);
 645         coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA);
 646     }
 647
 648
 649
 650     return CV_NO_ERR;
 651 }
 652 /*--------------------------------------------------------------------------------------*/
 653
 654
 655 /*---------------------------------------------------------------------------------------*/
 656
 657 /* This function get minimum angle started at point which contains rect */
 658 int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2)
 659 {
 660     /* Get crosslines with image corners */
 661
 662     /* Find four lines */
 663
 664     CvPoint2D64d pa,pb,pc,pd;
 665
 666     pa.x = 0;
 667     pa.y = 0;
 668
 669     pb.x = imageSize.width-1;
 670     pb.y = 0;
 671
 672     pd.x = imageSize.width-1;
 673     pd.y = imageSize.height-1;
 674
 675     pc.x = 0;
 676     pc.y = imageSize.height-1;
 677
 678     /* We can compute points for angle */
 679     /* Test for place section */
 680
 681     if( startPoint.x < 0 )
 682     {/* 1,4,7 */
 683         if( startPoint.y < 0)
 684         {/* 1 */
 685             *point1 = pb;
 686             *point2 = pc;
 687         }
 688         else if( startPoint.y > imageSize.height-1 )
 689         {/* 7 */
 690             *point1 = pa;
 691             *point2 = pd;
 692         }
 693         else
 694         {/* 4 */
 695             *point1 = pa;
 696             *point2 = pc;
 697         }
 698     }
 699     else if ( startPoint.x > imageSize.width-1 )
 700     {/* 3,6,9 */
 701         if( startPoint.y < 0 )
 702         {/* 3 */
 703             *point1 = pa;
 704             *point2 = pd;
 705         }
 706         else if ( startPoint.y > imageSize.height-1 )
 707         {/* 9 */
 708             *point1 = pb;
 709             *point2 = pc;
 710         }
 711         else
 712         {/* 6 */
 713             *point1 = pb;
 714             *point2 = pd;
 715         }
 716     }
 717     else
 718     {/* 2,5,8 */
 719         if( startPoint.y < 0 )
 720         {/* 2 */
 721             if( startPoint.x < imageSize.width/2 )
 722             {
 723                 *point1 = pb;
 724                 *point2 = pa;
 725             }
 726             else
 727             {
 728                 *point1 = pa;
 729                 *point2 = pb;
 730             }
 731         }
 732         else if( startPoint.y > imageSize.height-1 )
 733         {/* 8 */
 734             if( startPoint.x < imageSize.width/2 )
 735             {
 736                 *point1 = pc;
 737                 *point2 = pd;
 738             }
 739             else
 740             {
 741                 *point1 = pd;
 742                 *point2 = pc;
 743             }
 744         }
 745         else
 746         {/* 5 - point in the image */
 747             return 2;
 748         }
 749     }
 750     return 0;
 751 }/* GetAngleLine */
 752
 753 /*---------------------------------------------------------------------------------------*/
 754
 755 void icvGetCoefForPiece(   CvPoint2D64d p_start,CvPoint2D64d p_end,
 756                         double *a,double *b,double *c,
 757                         int* result)
 758 {
 759     double det;
 760     double detA,detB,detC;
 761
 762     det = p_start.x*p_end.y+p_end.x+p_start.y-p_end.y-p_start.y*p_end.x-p_start.x;
 763     if( fabs(det) < EPS64D)/* Error */
 764     {
 765         *result = 0;
 766         return;
 767     }
 768
 769     detA = p_start.y - p_end.y;
 770     detB = p_end.x - p_start.x;
 771     detC = p_start.x*p_end.y - p_end.x*p_start.y;
 772
 773     double invDet = 1.0 / det;
 774     *a = detA * invDet;
 775     *b = detB * invDet;
 776     *c = detC * invDet;
 777
 778     *result = 1;
 779     return;
 780 }
 781
 782 /*---------------------------------------------------------------------------------------*/
 783
 784 /* Get common area of rectifying */
 785 void icvGetCommonArea( CvSize imageSize,
 786                     CvPoint3D64d epipole1,CvPoint3D64d epipole2,
 787                     CvMatr64d fundMatr,
 788                     CvVect64d coeff11,CvVect64d coeff12,
 789                     CvVect64d coeff21,CvVect64d coeff22,
 790                     int* result)
 791 {
 792     int res = 0;
 793     CvPoint2D64d point11;
 794     CvPoint2D64d point12;
 795     CvPoint2D64d point21;
 796     CvPoint2D64d point22;
 797
 798     double corr11[3];
 799     double corr12[3];
 800     double corr21[3];
 801     double corr22[3];
 802
 803     double pointW11[3];
 804     double pointW12[3];
 805     double pointW21[3];
 806     double pointW22[3];
 807
 808     double transFundMatr[3*3];
 809     /* Compute transpose of fundamental matrix */
 810     icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );
 811
 812     CvPoint2D64d epipole1_2d;
 813     CvPoint2D64d epipole2_2d;
 814
 815     if( fabs(epipole1.z) < 1e-8 )
 816     {/* epipole1 in infinity */
 817         *result = 0;
 818         return;
 819     }
 820     epipole1_2d.x = epipole1.x / epipole1.z;
 821     epipole1_2d.y = epipole1.y / epipole1.z;
 822
 823     if( fabs(epipole2.z) < 1e-8 )
 824     {/* epipole2 in infinity */
 825         *result = 0;
 826         return;
 827     }
 828     epipole2_2d.x = epipole2.x / epipole2.z;
 829     epipole2_2d.y = epipole2.y / epipole2.z;
 830
 831     int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12);
 832     if( stat == 2 )
 833     {
 834         /* No angle */
 835         *result = 0;
 836         return;
 837     }
 838
 839     stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22);
 840     if( stat == 2 )
 841     {
 842         /* No angle */
 843         *result = 0;
 844         return;
 845     }
 846
 847     /* ============= Computation for line 1 ================ */
 848     /* Find correspondence line for angle points11 */
 849     /* corr21 = Fund'*p1 */
 850
 851     pointW11[0] = point11.x;
 852     pointW11[1] = point11.y;
 853     pointW11[2] = 1.0;
 854
 855     icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
 856                             pointW11,
 857                             corr21,
 858                             3,3);
 859
 860     /* Find crossing of line with image 2 */
 861     CvPoint2D64d start;
 862     CvPoint2D64d end;
 863     icvGetCrossRectDirect( imageSize,
 864                         corr21[0],corr21[1],corr21[2],
 865                         &start,&end,
 866                         &res);
 867
 868     if( res == 0 )
 869     {/* We have not cross */
 870         /* We must define new angle */
 871
 872         pointW21[0] = point21.x;
 873         pointW21[1] = point21.y;
 874         pointW21[2] = 1.0;
 875
 876         /* Find correspondence line for this angle points */
 877         /* We know point and try to get corr line */
 878         /* For point21 */
 879         /* corr11 = Fund * p21 */
 880
 881         icvTransformVector_64d( fundMatr, /* !!! Modified */
 882                                 pointW21,
 883                                 corr11,
 884                                 3,3);
 885
 886         /* We have cross. And it's result cross for up line. Set result coefs */
 887
 888         /* Set coefs for line 1 image 1 */
 889         coeff11[0] = corr11[0];
 890         coeff11[1] = corr11[1];
 891         coeff11[2] = corr11[2];
 892
 893         /* Set coefs for line 1 image 2 */
 894         icvGetCoefForPiece(    epipole2_2d,point21,
 895                             &coeff21[0],&coeff21[1],&coeff21[2],
 896                             &res);
 897         if( res == 0 )
 898         {
 899             *result = 0;
 900             return;/* Error */
 901         }
 902     }
 903     else
 904     {/* Line 1 cross image 2 */
 905         /* Set coefs for line 1 image 1 */
 906         icvGetCoefForPiece(    epipole1_2d,point11,
 907                             &coeff11[0],&coeff11[1],&coeff11[2],
 908                             &res);
 909         if( res == 0 )
 910         {
 911             *result = 0;
 912             return;/* Error */
 913         }
 914
 915         /* Set coefs for line 1 image 2 */
 916         coeff21[0] = corr21[0];
 917         coeff21[1] = corr21[1];
 918         coeff21[2] = corr21[2];
 919
 920     }
 921
 922     /* ============= Computation for line 2 ================ */
 923     /* Find correspondence line for angle points11 */
 924     /* corr22 = Fund*p2 */
 925
 926     pointW12[0] = point12.x;
 927     pointW12[1] = point12.y;
 928     pointW12[2] = 1.0;
 929
 930     icvTransformVector_64d( transFundMatr,
 931                             pointW12,
 932                             corr22,
 933                             3,3);
 934
 935     /* Find crossing of line with image 2 */
 936     icvGetCrossRectDirect( imageSize,
 937                         corr22[0],corr22[1],corr22[2],
 938                         &start,&end,
 939                         &res);
 940
 941     if( res == 0 )
 942     {/* We have not cross */
 943         /* We must define new angle */
 944
 945         pointW22[0] = point22.x;
 946         pointW22[1] = point22.y;
 947         pointW22[2] = 1.0;
 948
 949         /* Find correspondence line for this angle points */
 950         /* We know point and try to get corr line */
 951         /* For point21 */
 952         /* corr2 = Fund' * p1 */
 953
 954         icvTransformVector_64d( fundMatr,
 955                                 pointW22,
 956                                 corr12,
 957                                 3,3);
 958
 959
 960         /* We have cross. And it's result cross for down line. Set result coefs */
 961
 962         /* Set coefs for line 2 image 1 */
 963         coeff12[0] = corr12[0];
 964         coeff12[1] = corr12[1];
 965         coeff12[2] = corr12[2];
 966
 967         /* Set coefs for line 1 image 2 */
 968         icvGetCoefForPiece(    epipole2_2d,point22,
 969                             &coeff22[0],&coeff22[1],&coeff22[2],
 970                             &res);
 971         if( res == 0 )
 972         {
 973             *result = 0;
 974             return;/* Error */
 975         }
 976     }
 977     else
 978     {/* Line 2 cross image 2 */
 979         /* Set coefs for line 2 image 1 */
 980         icvGetCoefForPiece(    epipole1_2d,point12,
 981                             &coeff12[0],&coeff12[1],&coeff12[2],
 982                             &res);
 983         if( res == 0 )
 984         {
 985             *result = 0;
 986             return;/* Error */
 987         }
 988
 989         /* Set coefs for line 1 image 2 */
 990         coeff22[0] = corr22[0];
 991         coeff22[1] = corr22[1];
 992         coeff22[2] = corr22[2];
 993
 994     }
 995
 996     /* Now we know common area */
 997
 998     return;
 999
1000 }/* GetCommonArea */
1001
1002 /*---------------------------------------------------------------------------------------*/
1003
1004 /* Get cross for direction1 and direction2 */
1005 /*  Result = 1 - cross */
1006 /*  Result = 2 - parallel and not equal */
1007 /*  Result = 3 - parallel and equal */
1008
1009 void icvGetCrossDirectDirect(  CvVect64d direct1,CvVect64d direct2,
1010                             CvPoint2D64d *cross,int* result)
1011 {
1012     double det  = direct1[0]*direct2[1] - direct2[0]*direct1[1];
1013     double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2];
1014
1015     if( fabs(det) > EPS64D )
1016     {/* Have cross */
1017         cross->x = detx/det;
1018         cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det;
1019         *result = 1;
1020     }
1021     else
1022     {/* may be parallel */
1023         if( fabs(detx) > EPS64D )
1024         {/* parallel and not equal */
1025             *result = 2;
1026         }
1027         else
1028         {/* equals */
1029             *result = 3;
1030         }
1031     }
1032
1033     return;
1034 }
1035
1036 /*---------------------------------------------------------------------------------------*/
1037
1038 /* Get cross for piece p1,p2 and direction a,b,c */
1039 /*  Result = 0 - no cross */
1040 /*  Result = 1 - cross */
1041 /*  Result = 2 - parallel and not equal */
1042 /*  Result = 3 - parallel and equal */
1043
1044 void icvGetCrossPieceDirect(   CvPoint2D64d p_start,CvPoint2D64d p_end,
1045                             double a,double b,double c,
1046                             CvPoint2D64d *cross,int* result)
1047 {
1048
1049     if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 )
1050     {/* Have cross */
1051         double det;
1052         double detxc,detyc;
1053
1054         det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
1055
1056         if( fabs(det) < EPS64D )
1057         {/* lines are parallel and may be equal or line is point */
1058             if(  fabs(a*p_start.x + b*p_start.y + c) < EPS64D )
1059             {/* line is point or not diff */
1060                 *result = 3;
1061                 return;
1062             }
1063             else
1064             {
1065                 *result = 2;
1066             }
1067             return;
1068         }
1069
1070         detxc = b*(p_end.y*p_start.x - p_start.y*p_end.x) + c*(p_start.x - p_end.x);
1071         detyc = a*(p_end.x*p_start.y - p_start.x*p_end.y) + c*(p_start.y - p_end.y);
1072
1073         cross->x = detxc / det;
1074         cross->y = detyc / det;
1075         *result = 1;
1076
1077     }
1078     else
1079     {
1080         *result = 0;
1081     }
1082     return;
1083 }
1084 /*--------------------------------------------------------------------------------------*/
1085
1086 void icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end,
1087                             CvPoint2D64d p2_start,CvPoint2D64d p2_end,
1088                             CvPoint2D64d* cross,
1089                             int* result)
1090 {
1091     double ex1,ey1,ex2,ey2;
1092     double px1,py1,px2,py2;
1093     double del;
1094     double delA,delB,delX,delY;
1095     double alpha,betta;
1096
1097     ex1 = p1_start.x;
1098     ey1 = p1_start.y;
1099     ex2 = p1_end.x;
1100     ey2 = p1_end.y;
1101
1102     px1 = p2_start.x;
1103     py1 = p2_start.y;
1104     px2 = p2_end.x;
1105     py2 = p2_end.y;
1106
1107     del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2);
1108     if( fabs(del) <= EPS64D )
1109     {/* May be they are parallel !!! */
1110         *result = 0;
1111         return;
1112     }
1113
1114     delA =  (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1);
1115     delB =  (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1);
1116
1117     alpha = delA / del;
1118     betta = delB / del;
1119
1120     if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
1121     {
1122         *result = 0;
1123         return;
1124     }
1125
1126     delX =  (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
1127             (ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2));
1128
1129     delY =  (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
1130             (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2));
1131
1132     cross->x = delX / del;
1133     cross->y = delY / del;
1134
1135     *result = 1;
1136     return;
1137 }
1138
1139
1140 /*---------------------------------------------------------------------------------------*/
1141
1142 void icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist)
1143 {
1144     double dx = point2.x - point1.x;
1145     double dy = point2.y - point1.y;
1146     *dist = sqrt( dx*dx + dy*dy );
1147     return;
1148 }
1149
1150 /*---------------------------------------------------------------------------------------*/
1151
1152 void icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist)
1153 {
1154     double dx = point2.x - point1.x;
1155     double dy = point2.y - point1.y;
1156     double dz = point2.z - point1.z;
1157     *dist = sqrt( dx*dx + dy*dy + dz*dz );
1158     return;
1159 }
1160
1161 /*---------------------------------------------------------------------------------------*/
1162
1163 /* Find line from epipole which cross image rect */
1164 /* Find points of cross 0 or 1 or 2. Return number of points in cross */
1165 void icvGetCrossRectDirect(    CvSize imageSize,
1166                             double a,double b,double c,
1167                             CvPoint2D64d *start,CvPoint2D64d *end,
1168                             int* result)
1169 {
1170     CvPoint2D64d frameBeg;
1171     CvPoint2D64d frameEnd;
1172     CvPoint2D64d cross[4];
1173     int     haveCross[4];
1174
1175     haveCross[0] = 0;
1176     haveCross[1] = 0;
1177     haveCross[2] = 0;
1178     haveCross[3] = 0;
1179
1180     frameBeg.x = 0;
1181     frameBeg.y = 0;
1182     frameEnd.x = imageSize.width;
1183     frameEnd.y = 0;
1184
1185     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]);
1186
1187     frameBeg.x = imageSize.width;
1188     frameBeg.y = 0;
1189     frameEnd.x = imageSize.width;
1190     frameEnd.y = imageSize.height;
1191     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]);
1192
1193     frameBeg.x = imageSize.width;
1194     frameBeg.y = imageSize.height;
1195     frameEnd.x = 0;
1196     frameEnd.y = imageSize.height;
1197     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]);
1198
1199     frameBeg.x = 0;
1200     frameBeg.y = imageSize.height;
1201     frameEnd.x = 0;
1202     frameEnd.y = 0;
1203     icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]);
1204
1205     double maxDist;
1206
1207     int maxI=0,maxJ=0;
1208
1209
1210     int i,j;
1211
1212     maxDist = -1.0;
1213
1214     double distance;
1215
1216     for( i = 0; i < 3; i++ )
1217     {
1218         if( haveCross[i] == 1 )
1219         {
1220             for( j = i + 1; j < 4; j++ )
1221             {
1222                 if( haveCross[j] == 1)
1223                 {/* Compute dist */
1224                     icvGetPieceLength(cross[i],cross[j],&distance);
1225                     if( distance > maxDist )
1226                     {
1227                         maxI = i;
1228                         maxJ = j;
1229                         maxDist = distance;
1230                     }
1231                 }
1232             }
1233         }
1234     }
1235
1236     if( maxDist >= 0 )
1237     {/* We have cross */
1238         *start = cross[maxI];
1239         *result = 1;
1240         if( maxDist > 0 )
1241         {
1242             *end   = cross[maxJ];
1243             *result = 2;
1244         }
1245     }
1246     else
1247     {
1248         *result = 0;
1249     }
1250
1251     return;
1252 }/* GetCrossRectDirect */
1253
1254 /*---------------------------------------------------------------------------------------*/
1255 void icvProjectPointToImage(   CvPoint3D64d point,
1256                             CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,
1257                             CvPoint2D64d* projPoint)
1258 {
1259
1260     double tmpVect1[3];
1261     double tmpVect2[3];
1262
1263     icvMulMatrix_64d (  rotMatr,
1264                         3,3,
1265                         (double*)&point,
1266                         1,3,
1267                         tmpVect1);
1268
1269     icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3);
1270
1271     icvMulMatrix_64d (  camMatr,
1272                         3,3,
1273                         tmpVect2,
1274                         1,3,
1275                         tmpVect1);
1276
1277     projPoint->x = tmpVect1[0] / tmpVect1[2];
1278     projPoint->y = tmpVect1[1] / tmpVect1[2];
1279
1280     return;
1281 }
1282
1283 /*---------------------------------------------------------------------------------------*/
1284 /* Get quads for transform images */
1285 void icvGetQuadsTransform(
1286                           CvSize        imageSize,
1287                         CvMatr64d     camMatr1,
1288                         CvMatr64d     rotMatr1,
1289                         CvVect64d     transVect1,
1290                         CvMatr64d     camMatr2,
1291                         CvMatr64d     rotMatr2,
1292                         CvVect64d     transVect2,
1293                         CvSize*       warpSize,
1294                         double quad1[4][2],
1295                         double quad2[4][2],
1296                         CvMatr64d     fundMatr,
1297                         CvPoint3D64d* epipole1,
1298                         CvPoint3D64d* epipole2
1299                         )
1300 {
1301     /* First compute fundamental matrix and epipoles */
1302     int res;
1303
1304
1305     /* Compute epipoles and fundamental matrix using new functions */
1306     {
1307         double convRotMatr[9];
1308         double convTransVect[3];
1309
1310         icvCreateConvertMatrVect( rotMatr1,
1311                                   transVect1,
1312                                   rotMatr2,
1313                                   transVect2,
1314                                   convRotMatr,
1315                                   convTransVect);
1316         float convRotMatr_32f[9];
1317         float convTransVect_32f[3];
1318
1319         icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9);
1320         icvCvt_64d_32f(convTransVect,convTransVect_32f,3);
1321
1322         /* We know R and t */
1323         /* Compute essential matrix */
1324         float essMatr[9];
1325         float fundMatr_32f[9];
1326
1327         float camMatr1_32f[9];
1328         float camMatr2_32f[9];
1329
1330         icvCvt_64d_32f(camMatr1,camMatr1_32f,9);
1331         icvCvt_64d_32f(camMatr2,camMatr2_32f,9);
1332
1333         cvComputeEssentialMatrix(   convRotMatr_32f,
1334                                     convTransVect_32f,
1335                                     essMatr);
1336
1337         cvConvertEssential2Fundamental( essMatr,
1338                                         fundMatr_32f,
1339                                         camMatr1_32f,
1340                                         camMatr2_32f);
1341
1342         CvPoint3D32f epipole1_32f;
1343         CvPoint3D32f epipole2_32f;
1344
1345         cvComputeEpipolesFromFundMatrix( fundMatr_32f,
1346                                          &epipole1_32f,
1347                                          &epipole2_32f);
1348         /* copy to 64d epipoles */
1349         epipole1->x = epipole1_32f.x;
1350         epipole1->y = epipole1_32f.y;
1351         epipole1->z = epipole1_32f.z;
1352
1353         epipole2->x = epipole2_32f.x;
1354         epipole2->y = epipole2_32f.y;
1355         epipole2->z = epipole2_32f.z;
1356
1357         /* Convert fundamental matrix */
1358         icvCvt_32f_64d(fundMatr_32f,fundMatr,9);
1359     }
1360
1361     double coeff11[3];
1362     double coeff12[3];
1363     double coeff21[3];
1364     double coeff22[3];
1365
1366     icvGetCommonArea(   imageSize,
1367                         *epipole1,*epipole2,
1368                         fundMatr,
1369                         coeff11,coeff12,
1370                         coeff21,coeff22,
1371                         &res);
1372
1373     CvPoint2D64d point11, point12,point21, point22;
1374     double width1,width2;
1375     double height1,height2;
1376     double tmpHeight1,tmpHeight2;
1377
1378     CvPoint2D64d epipole1_2d;
1379     CvPoint2D64d epipole2_2d;
1380
1381     /* ----- Image 1 ----- */
1382     if( fabs(epipole1->z) < 1e-8 )
1383     {
1384         return;
1385     }
1386     epipole1_2d.x = epipole1->x / epipole1->z;
1387     epipole1_2d.y = epipole1->y / epipole1->z;
1388
1389     icvGetCutPiece( coeff11,coeff12,
1390                 epipole1_2d,
1391                 imageSize,
1392                 &point11,&point12,
1393                 &point21,&point22,
1394                 &res);
1395
1396     /* Compute distance */
1397     icvGetPieceLength(point11,point21,&width1);
1398     icvGetPieceLength(point11,point12,&tmpHeight1);
1399     icvGetPieceLength(point21,point22,&tmpHeight2);
1400     height1 = MAX(tmpHeight1,tmpHeight2);
1401
1402     quad1[0][0] = point11.x;
1403     quad1[0][1] = point11.y;
1404
1405     quad1[1][0] = point21.x;
1406     quad1[1][1] = point21.y;
1407
1408     quad1[2][0] = point22.x;
1409     quad1[2][1] = point22.y;
1410
1411     quad1[3][0] = point12.x;
1412     quad1[3][1] = point12.y;
1413
1414     /* ----- Image 2 ----- */
1415     if( fabs(epipole2->z) < 1e-8 )
1416     {
1417         return;
1418     }
1419     epipole2_2d.x = epipole2->x / epipole2->z;
1420     epipole2_2d.y = epipole2->y / epipole2->z;
1421
1422     icvGetCutPiece( coeff21,coeff22,
1423                 epipole2_2d,
1424                 imageSize,
1425                 &point11,&point12,
1426                 &point21,&point22,
1427                 &res);
1428
1429     /* Compute distance */
1430     icvGetPieceLength(point11,point21,&width2);
1431     icvGetPieceLength(point11,point12,&tmpHeight1);
1432     icvGetPieceLength(point21,point22,&tmpHeight2);
1433     height2 = MAX(tmpHeight1,tmpHeight2);
1434
1435     quad2[0][0] = point11.x;
1436     quad2[0][1] = point11.y;
1437
1438     quad2[1][0] = point21.x;
1439     quad2[1][1] = point21.y;
1440
1441     quad2[2][0] = point22.x;
1442     quad2[2][1] = point22.y;
1443
1444     quad2[3][0] = point12.x;
1445     quad2[3][1] = point12.y;
1446
1447
1448     /*=======================================================*/
1449     /* This is a new additional way to compute quads. */
1450     /* We must correct quads */
1451     {
1452         double convRotMatr[9];
1453         double convTransVect[3];
1454
1455         double newQuad1[4][2];
1456         double newQuad2[4][2];
1457
1458
1459         icvCreateConvertMatrVect( rotMatr1,
1460                                   transVect1,
1461                                   rotMatr2,
1462                                   transVect2,
1463                                   convRotMatr,
1464                                   convTransVect);
1465
1466         /* -------------Compute for first image-------------- */
1467         CvPoint2D32f pointb1;
1468         CvPoint2D32f pointe1;
1469
1470         CvPoint2D32f pointb2;
1471         CvPoint2D32f pointe2;
1472
1473         pointb1.x = (float)quad1[0][0];
1474         pointb1.y = (float)quad1[0][1];
1475
1476         pointe1.x = (float)quad1[3][0];
1477         pointe1.y = (float)quad1[3][1];
1478
1479         icvComputeeInfiniteProject1(convRotMatr,
1480                                     camMatr1,
1481                                     camMatr2,
1482                                     pointb1,
1483                                     &pointb2);
1484
1485         icvComputeeInfiniteProject1(convRotMatr,
1486                                     camMatr1,
1487                                     camMatr2,
1488                                     pointe1,
1489                                     &pointe2);
1490
1491         /*  JUST TEST FOR POINT */
1492
1493         /* Compute distances */
1494         double dxOld,dyOld;
1495         double dxNew,dyNew;
1496         double distOld,distNew;
1497
1498         dxOld = quad2[1][0] - quad2[0][0];
1499         dyOld = quad2[1][1] - quad2[0][1];
1500         distOld = dxOld*dxOld + dyOld*dyOld;
1501
1502         dxNew = quad2[1][0] - pointb2.x;
1503         dyNew = quad2[1][1] - pointb2.y;
1504         distNew = dxNew*dxNew + dyNew*dyNew;
1505
1506         if( distNew > distOld )
1507         {/* Get new points for second quad */
1508             newQuad2[0][0] = pointb2.x;
1509             newQuad2[0][1] = pointb2.y;
1510             newQuad2[3][0] = pointe2.x;
1511             newQuad2[3][1] = pointe2.y;
1512             newQuad1[0][0] = quad1[0][0];
1513             newQuad1[0][1] = quad1[0][1];
1514             newQuad1[3][0] = quad1[3][0];
1515             newQuad1[3][1] = quad1[3][1];
1516         }
1517         else
1518         {/* Get new points for first quad */
1519
1520             pointb2.x = (float)quad2[0][0];
1521             pointb2.y = (float)quad2[0][1];
1522
1523             pointe2.x = (float)quad2[3][0];
1524             pointe2.y = (float)quad2[3][1];
1525
1526             icvComputeeInfiniteProject2(convRotMatr,
1527                                         camMatr1,
1528                                         camMatr2,
1529                                         &pointb1,
1530                                         pointb2);
1531
1532             icvComputeeInfiniteProject2(convRotMatr,
1533                                         camMatr1,
1534                                         camMatr2,
1535                                         &pointe1,
1536                                         pointe2);
1537
1538
1539             /*  JUST TEST FOR POINT */
1540
1541             newQuad2[0][0] = quad2[0][0];
1542             newQuad2[0][1] = quad2[0][1];
1543             newQuad2[3][0] = quad2[3][0];
1544             newQuad2[3][1] = quad2[3][1];
1545
1546             newQuad1[0][0] = pointb1.x;
1547             newQuad1[0][1] = pointb1.y;
1548             newQuad1[3][0] = pointe1.x;
1549             newQuad1[3][1] = pointe1.y;
1550         }
1551
1552         /* -------------Compute for second image-------------- */
1553         pointb1.x = (float)quad1[1][0];
1554         pointb1.y = (float)quad1[1][1];
1555
1556         pointe1.x = (float)quad1[2][0];
1557         pointe1.y = (float)quad1[2][1];
1558
1559         icvComputeeInfiniteProject1(convRotMatr,
1560                                     camMatr1,
1561                                     camMatr2,
1562                                     pointb1,
1563                                     &pointb2);
1564
1565         icvComputeeInfiniteProject1(convRotMatr,
1566                                     camMatr1,
1567                                     camMatr2,
1568                                     pointe1,
1569                                     &pointe2);
1570
1571         /* Compute distances */
1572
1573         dxOld = quad2[0][0] - quad2[1][0];
1574         dyOld = quad2[0][1] - quad2[1][1];
1575         distOld = dxOld*dxOld + dyOld*dyOld;
1576
1577         dxNew = quad2[0][0] - pointb2.x;
1578         dyNew = quad2[0][1] - pointb2.y;
1579         distNew = dxNew*dxNew + dyNew*dyNew;
1580
1581         if( distNew > distOld )
1582         {/* Get new points for second quad */
1583             newQuad2[1][0] = pointb2.x;
1584             newQuad2[1][1] = pointb2.y;
1585             newQuad2[2][0] = pointe2.x;
1586             newQuad2[2][1] = pointe2.y;
1587             newQuad1[1][0] = quad1[1][0];
1588             newQuad1[1][1] = quad1[1][1];
1589             newQuad1[2][0] = quad1[2][0];
1590             newQuad1[2][1] = quad1[2][1];
1591         }
1592         else
1593         {/* Get new points for first quad */
1594
1595             pointb2.x = (float)quad2[1][0];
1596             pointb2.y = (float)quad2[1][1];
1597
1598             pointe2.x = (float)quad2[2][0];
1599             pointe2.y = (float)quad2[2][1];
1600
1601             icvComputeeInfiniteProject2(convRotMatr,
1602                                         camMatr1,
1603                                         camMatr2,
1604                                         &pointb1,
1605                                         pointb2);
1606
1607             icvComputeeInfiniteProject2(convRotMatr,
1608                                         camMatr1,
1609                                         camMatr2,
1610                                         &pointe1,
1611                                         pointe2);
1612
1613             newQuad2[1][0] = quad2[1][0];
1614             newQuad2[1][1] = quad2[1][1];
1615             newQuad2[2][0] = quad2[2][0];
1616             newQuad2[2][1] = quad2[2][1];
1617
1618             newQuad1[1][0] = pointb1.x;
1619             newQuad1[1][1] = pointb1.y;
1620             newQuad1[2][0] = pointe1.x;
1621             newQuad1[2][1] = pointe1.y;
1622         }
1623
1624
1625
1626 /*-------------------------------------------------------------------------------*/
1627
1628         /* Copy new quads to old quad */
1629         int i;
1630         for( i = 0; i < 4; i++ )
1631         {
1632             {
1633                 quad1[i][0] = newQuad1[i][0];
1634                 quad1[i][1] = newQuad1[i][1];
1635                 quad2[i][0] = newQuad2[i][0];
1636                 quad2[i][1] = newQuad2[i][1];
1637             }
1638         }
1639     }
1640     /*=======================================================*/
1641
1642     double warpWidth,warpHeight;
1643
1644     warpWidth  = MAX(width1,width2);
1645     warpHeight = MAX(height1,height2);
1646
1647     warpSize->width  = (int)warpWidth;
1648     warpSize->height = (int)warpHeight;
1649
1650     warpSize->width  = cvRound(warpWidth-1);
1651     warpSize->height = cvRound(warpHeight-1);
1652
1653 /* !!! by Valery Mosyagin. this lines added just for test no warp */
1654     warpSize->width  = imageSize.width;
1655     warpSize->height = imageSize.height;
1656
1657     return;
1658 }
1659
1660
1661 /*---------------------------------------------------------------------------------------*/
1662
1663 void icvGetQuadsTransformNew(  CvSize        imageSize,
1664                             CvMatr32f     camMatr1,
1665                             CvMatr32f     camMatr2,
1666                             CvMatr32f     rotMatr1,
1667                             CvVect32f     transVect1,
1668                             CvSize*       warpSize,
1669                             double        quad1[4][2],
1670                             double        quad2[4][2],
1671                             CvMatr32f     fundMatr,
1672                             CvPoint3D32f* epipole1,
1673                             CvPoint3D32f* epipole2
1674                         )
1675 {
1676     /* Convert data */
1677     /* Convert camera matrix */
1678     double camMatr1_64d[9];
1679     double camMatr2_64d[9];
1680     double rotMatr1_64d[9];
1681     double transVect1_64d[3];
1682     double rotMatr2_64d[9];
1683     double transVect2_64d[3];
1684     double fundMatr_64d[9];
1685     CvPoint3D64d epipole1_64d;
1686     CvPoint3D64d epipole2_64d;
1687
1688     icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
1689     icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
1690     icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
1691     icvCvt_32f_64d(transVect1,transVect1_64d,3);
1692
1693     /* Create vector and matrix */
1694
1695     rotMatr2_64d[0] = 1;
1696     rotMatr2_64d[1] = 0;
1697     rotMatr2_64d[2] = 0;
1698     rotMatr2_64d[3] = 0;
1699     rotMatr2_64d[4] = 1;
1700     rotMatr2_64d[5] = 0;
1701     rotMatr2_64d[6] = 0;
1702     rotMatr2_64d[7] = 0;
1703     rotMatr2_64d[8] = 1;
1704
1705     transVect2_64d[0] = 0;
1706     transVect2_64d[1] = 0;
1707     transVect2_64d[2] = 0;
1708
1709     icvGetQuadsTransform(   imageSize,
1710                             camMatr1_64d,
1711                             rotMatr1_64d,
1712                             transVect1_64d,
1713                             camMatr2_64d,
1714                             rotMatr2_64d,
1715                             transVect2_64d,
1716                             warpSize,
1717                             quad1,
1718                             quad2,
1719                             fundMatr_64d,
1720                             &epipole1_64d,
1721                             &epipole2_64d
1722                         );
1723
1724     /* Convert epipoles */
1725     epipole1->x = (float)(epipole1_64d.x);
1726     epipole1->y = (float)(epipole1_64d.y);
1727     epipole1->z = (float)(epipole1_64d.z);
1728
1729     epipole2->x = (float)(epipole2_64d.x);
1730     epipole2->y = (float)(epipole2_64d.y);
1731     epipole2->z = (float)(epipole2_64d.z);
1732
1733     /* Convert fundamental matrix */
1734     icvCvt_64d_32f(fundMatr_64d,fundMatr,9);
1735
1736     return;
1737 }
1738
1739 /*---------------------------------------------------------------------------------------*/
1740 void icvGetQuadsTransformStruct(  CvStereoCamera* stereoCamera)
1741 {
1742     /* Wrapper for icvGetQuadsTransformNew */
1743
1744
1745     double  quad1[4][2];
1746     double  quad2[4][2];
1747
1748     icvGetQuadsTransformNew(     cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),cvRound(stereoCamera->camera[0]->imgSize[1])),
1749                             stereoCamera->camera[0]->matrix,
1750                             stereoCamera->camera[1]->matrix,
1751                             stereoCamera->rotMatrix,
1752                             stereoCamera->transVector,
1753                             &(stereoCamera->warpSize),
1754                             quad1,
1755                             quad2,
1756                             stereoCamera->fundMatr,
1757                             &(stereoCamera->epipole[0]),
1758                             &(stereoCamera->epipole[1])
1759                         );
1760
1761     int i;
1762     for( i = 0; i < 4; i++ )
1763     {
1764         stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]);
1765         stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]);
1766     }
1767
1768     return;
1769 }
1770
1771 /*---------------------------------------------------------------------------------------*/
1772 void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
1773 {
1774     /* For given intrinsic and extrinsic parameters computes rest parameters
1775     **   such as fundamental matrix. warping coeffs, epipoles, ...
1776     */
1777
1778
1779     /* compute rotate matrix and translate vector */
1780     double rotMatr1[9];
1781     double rotMatr2[9];
1782
1783     double transVect1[3];
1784     double transVect2[3];
1785
1786     double convRotMatr[9];
1787     double convTransVect[3];
1788
1789     /* fill matrices */
1790     icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9);
1791     icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9);
1792
1793     icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3);
1794     icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3);
1795
1796     icvCreateConvertMatrVect(   rotMatr1,
1797                                 transVect1,
1798                                 rotMatr2,
1799                                 transVect2,
1800                                 convRotMatr,
1801                                 convTransVect);
1802
1803     /* copy to stereo camera params */
1804     icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9);
1805     icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3);
1806
1807
1808     icvGetQuadsTransformStruct(stereoCamera);
1809     icvComputeRestStereoParams(stereoCamera);
1810 }
1811
1812
1813
1814 /*---------------------------------------------------------------------------------------*/
1815
1816 /* Get cut line for one image */
1817 void icvGetCutPiece(   CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,
1818                     CvPoint2D64d epipole,
1819                     CvSize imageSize,
1820                     CvPoint2D64d* point11,CvPoint2D64d* point12,
1821                     CvPoint2D64d* point21,CvPoint2D64d* point22,
1822                     int* result)
1823 {
1824     /* Compute nearest cut line to epipole */
1825     /* Get corners inside sector */
1826     /* Collect all candidate point */
1827
1828     CvPoint2D64d candPoints[8];
1829     CvPoint2D64d midPoint;
1830     int numPoints = 0;
1831     int res;
1832     int i;
1833
1834     double cutLine1[3];
1835     double cutLine2[3];
1836
1837     /* Find middle line of sector */
1838     double midLine[3]={0,0,0};
1839
1840
1841     /* Different way  */
1842     CvPoint2D64d pointOnLine1;  pointOnLine1.x = pointOnLine1.y = 0;
1843     CvPoint2D64d pointOnLine2;  pointOnLine2.x = pointOnLine2.y = 0;
1844
1845     CvPoint2D64d start1,end1;
1846
1847     icvGetCrossRectDirect( imageSize,
1848                         areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
1849                         &start1,&end1,&res);
1850     if( res > 0 )
1851     {
1852         pointOnLine1 = start1;
1853     }
1854
1855     icvGetCrossRectDirect( imageSize,
1856                         areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
1857                         &start1,&end1,&res);
1858     if( res > 0 )
1859     {
1860         pointOnLine2 = start1;
1861     }
1862
1863     icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);
1864
1865     icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);
1866
1867     /* Test corner points */
1868     CvPoint2D64d cornerPoint;
1869     CvPoint2D64d tmpPoints[2];
1870
1871     cornerPoint.x = 0;
1872     cornerPoint.y = 0;
1873     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1874     if( res == 1 )
1875     {/* Add point */
1876         candPoints[numPoints] = cornerPoint;
1877         numPoints++;
1878     }
1879
1880     cornerPoint.x = imageSize.width;
1881     cornerPoint.y = 0;
1882     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1883     if( res == 1 )
1884     {/* Add point */
1885         candPoints[numPoints] = cornerPoint;
1886         numPoints++;
1887     }
1888
1889     cornerPoint.x = imageSize.width;
1890     cornerPoint.y = imageSize.height;
1891     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1892     if( res == 1 )
1893     {/* Add point */
1894         candPoints[numPoints] = cornerPoint;
1895         numPoints++;
1896     }
1897
1898     cornerPoint.x = 0;
1899     cornerPoint.y = imageSize.height;
1900     icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
1901     if( res == 1 )
1902     {/* Add point */
1903         candPoints[numPoints] = cornerPoint;
1904         numPoints++;
1905     }
1906
1907     /* Find cross line 1 with image border */
1908     icvGetCrossRectDirect( imageSize,
1909                         areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
1910                         &tmpPoints[0], &tmpPoints[1],
1911                         &res);
1912     for( i = 0; i < res; i++ )
1913     {
1914         candPoints[numPoints++] = tmpPoints[i];
1915     }
1916
1917     /* Find cross line 2 with image border */
1918     icvGetCrossRectDirect( imageSize,
1919                         areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
1920                         &tmpPoints[0], &tmpPoints[1],
1921                         &res);
1922
1923     for( i = 0; i < res; i++ )
1924     {
1925         candPoints[numPoints++] = tmpPoints[i];
1926     }
1927
1928     if( numPoints < 2 )
1929     {
1930         *result = 0;
1931         return;/* Error. Not enought points */
1932     }
1933     /* Project all points to middle line and get max and min */
1934
1935     CvPoint2D64d projPoint;
1936     CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
1937     CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;
1938
1939
1940     double dist;
1941     double maxDist = 0;
1942     double minDist = 10000000;
1943
1944
1945     for( i = 0; i < numPoints; i++ )
1946     {
1947         icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
1948         icvGetPieceLength(epipole,projPoint,&dist);
1949         if( dist < minDist)
1950         {
1951             minDist = dist;
1952             minPoint = projPoint;
1953         }
1954
1955         if( dist > maxDist)
1956         {
1957             maxDist = dist;
1958             maxPoint = projPoint;
1959         }
1960     }
1961
1962     /* We know maximum and minimum points. Now we can compute cut lines */
1963
1964     icvGetNormalDirect(midLine,minPoint,cutLine1);
1965     icvGetNormalDirect(midLine,maxPoint,cutLine2);
1966
1967     /* Test for begin of line. */
1968     CvPoint2D64d tmpPoint2;
1969
1970     /* Get cross with */
1971     icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
1972     icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);
1973
1974     icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
1975     icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);
1976
1977     if( epipole.x > imageSize.width * 0.5 )
1978     {/* Need to change points */
1979         tmpPoint2 = *point11;
1980         *point11 = *point21;
1981         *point21 = tmpPoint2;
1982
1983         tmpPoint2 = *point12;
1984         *point12 = *point22;
1985         *point22 = tmpPoint2;
1986     }
1987
1988     return;
1989 }
1990 /*---------------------------------------------------------------------------------------*/
1991 /* Get middle angle */
1992 void icvGetMiddleAnglePoint(   CvPoint2D64d basePoint,
1993                             CvPoint2D64d point1,CvPoint2D64d point2,
1994                             CvPoint2D64d* midPoint)
1995 {/* !!! May be need to return error */
1996
1997     double dist1;
1998     double dist2;
1999     icvGetPieceLength(basePoint,point1,&dist1);
2000     icvGetPieceLength(basePoint,point2,&dist2);
2001     CvPoint2D64d pointNew1;
2002     CvPoint2D64d pointNew2;
2003     double alpha = dist2/dist1;
2004
2005     pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
2006     pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );
2007
2008     pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
2009     pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );
2010
2011     int res;
2012     icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);
2013
2014     return;
2015 }
2016
2017 /*---------------------------------------------------------------------------------------*/
2018 /* Get normal direct to direct in line */
2019 void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
2020 {
2021     normDirect[0] =   direct[1];
2022     normDirect[1] = - direct[0];
2023     normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);
2024     return;
2025 }
2026
2027 /*---------------------------------------------------------------------------------------*/
2028 CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
2029 {
2030     return  (point1.x - basePoint.x)*(point2.y - basePoint.y) -
2031             (point2.x - basePoint.x)*(point1.y - basePoint.y);
2032 }
2033 /*---------------------------------------------------------------------------------------*/
2034 /* Test for point in sector           */
2035 /* Return 0 - point not inside sector */
2036 /* Return 1 - point inside sector     */
2037 void icvTestPoint( CvPoint2D64d testPoint,
2038                 CvVect64d line1,CvVect64d line2,
2039                 CvPoint2D64d basePoint,
2040                 int* result)
2041 {
2042     CvPoint2D64d point1,point2;
2043
2044     icvProjectPointToDirect(testPoint,line1,&point1);
2045     icvProjectPointToDirect(testPoint,line2,&point2);
2046
2047     double sign1 = icvGetVect(basePoint,point1,point2);
2048     double sign2 = icvGetVect(basePoint,point1,testPoint);
2049     if( sign1 * sign2 > 0 )
2050     {/* Correct for first line */
2051         sign1 = - sign1;
2052         sign2 = icvGetVect(basePoint,point2,testPoint);
2053         if( sign1 * sign2 > 0 )
2054         {/* Correct for both lines */
2055             *result = 1;
2056         }
2057         else
2058         {
2059             *result = 0;
2060         }
2061     }
2062     else
2063     {
2064         *result = 0;
2065     }
2066
2067     return;
2068 }
2069
2070 /*---------------------------------------------------------------------------------------*/
2071 /* Project point to line */
2072 void icvProjectPointToDirect(  CvPoint2D64d point,CvVect64d lineCoeff,
2073                             CvPoint2D64d* projectPoint)
2074 {
2075     double a = lineCoeff[0];
2076     double b = lineCoeff[1];
2077
2078     double det =  1.0 / ( a*a + b*b );
2079     double delta =  a*point.y - b*point.x;
2080
2081     projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
2082     projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;
2083
2084     return;
2085 }
2086
2087 /*---------------------------------------------------------------------------------------*/
2088 /* Get distance from point to direction */
2089 void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
2090 {
2091     CvPoint2D64d tmpPoint;
2092     icvProjectPointToDirect(point,lineCoef,&tmpPoint);
2093     double dx = point.x - tmpPoint.x;
2094     double dy = point.y - tmpPoint.y;
2095     *dist = sqrt(dx*dx+dy*dy);
2096     return;
2097 }
2098 /*---------------------------------------------------------------------------------------*/
2099
2100 CV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
2101                                        int desired_depth, int desired_num_channels )
2102 {
2103     CvSize src_size ;
2104     src_size.width = src->width;
2105     src_size.height = src->height;
2106
2107     CvSize dst_size = src_size;
2108
2109     if( dst )
2110     {
2111         dst_size.width = dst->width;
2112         dst_size.height = dst->height;
2113     }
2114
2115     if( !dst || dst->depth != desired_depth ||
2116         dst->nChannels != desired_num_channels ||
2117         dst_size.width != src_size.width ||
2118         dst_size.height != dst_size.height )
2119     {
2120         cvReleaseImage( &dst );
2121         dst = cvCreateImage( src_size, desired_depth, desired_num_channels );
2122         CvRect rect = cvRect(0,0,src_size.width,src_size.height);
2123         cvSetImageROI( dst, rect );
2124
2125     }
2126
2127     return dst;
2128 }
2129
2130 int
2131 icvCvt_32f_64d( float *src, double *dst, int size )
2132 {
2133     int t;
2134
2135     if( !src || !dst )
2136         return CV_NULLPTR_ERR;
2137     if( size <= 0 )
2138         return CV_BADRANGE_ERR;
2139
2140     for( t = 0; t < size; t++ )
2141     {
2142         dst[t] = (double) (src[t]);
2143     }
2144
2145     return CV_OK;
2146 }
2147
2148 /*======================================================================================*/
2149 /* Type conversion double -> float */
2150 int
2151 icvCvt_64d_32f( double *src, float *dst, int size )
2152 {
2153     int t;
2154
2155     if( !src || !dst )
2156         return CV_NULLPTR_ERR;
2157     if( size <= 0 )
2158         return CV_BADRANGE_ERR;
2159
2160     for( t = 0; t < size; t++ )
2161     {
2162         dst[t] = (float) (src[t]);
2163     }
2164
2165     return CV_OK;
2166 }
2167
2168 /*----------------------------------------------------------------------------------*/
2169
2170
2171 /* Find line which cross frame by line(a,b,c) */
2172 void FindLineForEpiline(    CvSize imageSize,
2173                             float a,float b,float c,
2174                             CvPoint2D32f *start,CvPoint2D32f *end,
2175                             int*)
2176 {
2177     CvPoint2D32f frameBeg;
2178
2179     CvPoint2D32f frameEnd;
2180     CvPoint2D32f cross[4];
2181     int     haveCross[4];
2182     float   dist;
2183
2184     haveCross[0] = 0;
2185     haveCross[1] = 0;
2186     haveCross[2] = 0;
2187     haveCross[3] = 0;
2188
2189     frameBeg.x = 0;
2190     frameBeg.y = 0;
2191     frameEnd.x = (float)(imageSize.width);
2192     frameEnd.y = 0;
2193     haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
2194
2195     frameBeg.x = (float)(imageSize.width);
2196     frameBeg.y = 0;
2197     frameEnd.x = (float)(imageSize.width);
2198     frameEnd.y = (float)(imageSize.height);
2199     haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
2200
2201     frameBeg.x = (float)(imageSize.width);
2202     frameBeg.y = (float)(imageSize.height);
2203     frameEnd.x = 0;
2204     frameEnd.y = (float)(imageSize.height);
2205     haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
2206
2207     frameBeg.x = 0;
2208     frameBeg.y = (float)(imageSize.height);
2209     frameEnd.x = 0;
2210     frameEnd.y = 0;
2211     haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
2212
2213     int n;
2214     float minDist = (float)(INT_MAX);
2215     float maxDist = (float)(INT_MIN);
2216
2217     int maxN = -1;
2218     int minN = -1;
2219
2220     double midPointX = imageSize.width  / 2.0;
2221     double midPointY = imageSize.height / 2.0;
2222
2223     for( n = 0; n < 4; n++ )
2224     {
2225         if( haveCross[n] > 0 )
2226         {
2227             dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
2228                             (midPointY - cross[n].y)*(midPointY - cross[n].y));
2229
2230             if( dist < minDist )
2231             {
2232                 minDist = dist;
2233                 minN = n;
2234             }
2235
2236             if( dist > maxDist )
2237             {
2238                 maxDist = dist;
2239                 maxN = n;
2240             }
2241         }
2242     }
2243
2244     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
2245     {
2246         *start = cross[minN];
2247         *end   = cross[maxN];
2248     }
2249     else
2250     {
2251         start->x = 0;
2252         start->y = 0;
2253         end->x = 0;
2254         end->y = 0;
2255     }
2256
2257     return;
2258
2259 }
2260
2261
2262 /*----------------------------------------------------------------------------------*/
2263
2264 int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
2265 {
2266     float width  = (float)(imageSize.width);
2267     float height = (float)(imageSize.height);
2268
2269     /* Get crosslines with image corners */
2270
2271     /* Find four lines */
2272
2273     CvPoint2D32f pa,pb,pc,pd;
2274
2275     pa.x = 0;
2276     pa.y = 0;
2277
2278     pb.x = width;
2279     pb.y = 0;
2280
2281     pd.x = width;
2282     pd.y = height;
2283
2284     pc.x = 0;
2285     pc.y = height;
2286
2287     /* We can compute points for angle */
2288     /* Test for place section */
2289
2290     float x,y;
2291     x = epipole.x;
2292     y = epipole.y;
2293
2294     if( x < 0 )
2295     {/* 1,4,7 */
2296         if( y < 0)
2297         {/* 1 */
2298             point1 = pb;
2299             point2 = pc;
2300         }
2301         else if( y > height )
2302         {/* 7 */
2303             point1 = pa;
2304             point2 = pd;
2305         }
2306         else
2307         {/* 4 */
2308             point1 = pa;
2309             point2 = pc;
2310         }
2311     }
2312     else if ( x > width )
2313     {/* 3,6,9 */
2314         if( y < 0 )
2315         {/* 3 */
2316             point1 = pa;
2317             point2 = pd;
2318         }
2319         else if ( y > height )
2320         {/* 9 */
2321             point1 = pc;
2322             point2 = pb;
2323         }
2324         else
2325         {/* 6 */
2326             point1 = pb;
2327             point2 = pd;
2328         }
2329     }
2330     else
2331     {/* 2,5,8 */
2332         if( y < 0 )
2333         {/* 2 */
2334             point1 = pa;
2335             point2 = pb;
2336         }
2337         else if( y > height )
2338         {/* 8 */
2339             point1 = pc;
2340             point2 = pd;
2341         }
2342         else
2343         {/* 5 - point in the image */
2344             return 2;
2345         }
2346
2347
2348     }
2349
2350
2351     return 0;
2352 }
2353
2354 /*--------------------------------------------------------------------------------------*/
2355 void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])
2356 {/* Computes perspective coeffs for transformation from src to dst quad */
2357
2358
2359     CV_FUNCNAME( "icvComputePerspectiveCoeffs" );
2360
2361     __BEGIN__;
2362
2363     double A[64];
2364     double b[8];
2365     double c[8];
2366     CvPoint2D32f pt[4];
2367     int i;
2368
2369     pt[0] = srcQuad[0];
2370     pt[1] = srcQuad[1];
2371     pt[2] = srcQuad[2];
2372     pt[3] = srcQuad[3];
2373
2374     for( i = 0; i < 4; i++ )
2375     {
2376 #if 0
2377         double x = dstQuad[i].x;
2378         double y = dstQuad[i].y;
2379         double X = pt[i].x;
2380         double Y = pt[i].y;
2381 #else
2382         double x = pt[i].x;
2383         double y = pt[i].y;
2384         double X = dstQuad[i].x;
2385         double Y = dstQuad[i].y;
2386 #endif
2387         double* a = A + i*16;
2388
2389         a[0] = x;
2390         a[1] = y;
2391         a[2] = 1;
2392         a[3] = 0;
2393         a[4] = 0;
2394         a[5] = 0;
2395         a[6] = -X*x;
2396         a[7] = -X*y;
2397
2398         a += 8;
2399
2400         a[0] = 0;
2401         a[1] = 0;
2402         a[2] = 0;
2403         a[3] = x;
2404         a[4] = y;
2405         a[5] = 1;
2406         a[6] = -Y*x;
2407         a[7] = -Y*y;
2408
2409         b[i*2] = X;
2410         b[i*2 + 1] = Y;
2411     }
2412
2413     {
2414     double invA[64];
2415     CvMat matA = cvMat( 8, 8, CV_64F, A );
2416     CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
2417     CvMat matB = cvMat( 8, 1, CV_64F, b );
2418     CvMat matX = cvMat( 8, 1, CV_64F, c );
2419
2420     CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2421     CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2422     }
2423
2424     coeffs[0][0] = c[0];
2425     coeffs[0][1] = c[1];
2426     coeffs[0][2] = c[2];
2427     coeffs[1][0] = c[3];
2428     coeffs[1][1] = c[4];
2429     coeffs[1][2] = c[5];
2430     coeffs[2][0] = c[6];
2431     coeffs[2][1] = c[7];
2432     coeffs[2][2] = 1.0;
2433
2434     __END__;
2435
2436     return;
2437 }
2438
2439 /*--------------------------------------------------------------------------------------*/
2440
2441 CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY )
2442 {
2443     CV_FUNCNAME( "cvComputePerspectiveMap" );
2444
2445     __BEGIN__;
2446
2447     CvSize size;
2448     CvMat  stubx, *mapx = (CvMat*)rectMapX;
2449     CvMat  stuby, *mapy = (CvMat*)rectMapY;
2450     int i, j;
2451
2452     CV_CALL( mapx = cvGetMat( mapx, &stubx ));
2453     CV_CALL( mapy = cvGetMat( mapy, &stuby ));
2454
2455     if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 )
2456         CV_ERROR( CV_StsUnsupportedFormat, "" );
2457
2458     size = cvGetMatSize(mapx);
2459     assert( fabs(c[2][2] - 1.) < FLT_EPSILON );
2460
2461     for( i = 0; i < size.height; i++ )
2462     {
2463         float* mx = (float*)(mapx->data.ptr + mapx->step*i);
2464         float* my = (float*)(mapy->data.ptr + mapy->step*i);
2465
2466         for( j = 0; j < size.width; j++ )
2467         {
2468             double w = 1./(c[2][0]*j + c[2][1]*i + 1.);
2469             double x = (c[0][0]*j + c[0][1]*i + c[0][2])*w;
2470             double y = (c[1][0]*j + c[1][1]*i + c[1][2])*w;
2471
2472             mx[j] = (float)x;
2473             my[j] = (float)y;
2474         }
2475     }
2476
2477     __END__;
2478 }
2479
2480 /*--------------------------------------------------------------------------------------*/
2481
2482 CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
2483                                               CvArr* rectMap )
2484 {
2485     /* Computes Perspective Transform coeffs and map if need
2486         for given image size and given result quad */
2487     CV_FUNCNAME( "cvInitPerspectiveTransform" );
2488
2489     __BEGIN__;
2490
2491     double A[64];
2492     double b[8];
2493     double c[8];
2494     CvPoint2D32f pt[4];
2495     CvMat  mapstub, *map = (CvMat*)rectMap;
2496     int i, j;
2497
2498     if( map )
2499     {
2500         CV_CALL( map = cvGetMat( map, &mapstub ));
2501
2502         if( CV_MAT_TYPE( map->type ) != CV_32FC2 )
2503             CV_ERROR( CV_StsUnsupportedFormat, "" );
2504
2505         if( map->width != size.width || map->height != size.height )
2506             CV_ERROR( CV_StsUnmatchedSizes, "" );
2507     }
2508
2509     pt[0] = cvPoint2D32f( 0, 0 );
2510     pt[1] = cvPoint2D32f( size.width, 0 );
2511     pt[2] = cvPoint2D32f( size.width, size.height );
2512     pt[3] = cvPoint2D32f( 0, size.height );
2513
2514     for( i = 0; i < 4; i++ )
2515     {
2516 #if 0
2517         double x = quad[i].x;
2518         double y = quad[i].y;
2519         double X = pt[i].x;
2520         double Y = pt[i].y;
2521 #else
2522         double x = pt[i].x;
2523         double y = pt[i].y;
2524         double X = quad[i].x;
2525         double Y = quad[i].y;
2526 #endif
2527         double* a = A + i*16;
2528
2529         a[0] = x;
2530         a[1] = y;
2531         a[2] = 1;
2532         a[3] = 0;
2533         a[4] = 0;
2534         a[5] = 0;
2535         a[6] = -X*x;
2536         a[7] = -X*y;
2537
2538         a += 8;
2539
2540         a[0] = 0;
2541         a[1] = 0;
2542         a[2] = 0;
2543         a[3] = x;
2544         a[4] = y;
2545         a[5] = 1;
2546         a[6] = -Y*x;
2547         a[7] = -Y*y;
2548
2549         b[i*2] = X;
2550         b[i*2 + 1] = Y;
2551     }
2552
2553     {
2554     double invA[64];
2555     CvMat matA = cvMat( 8, 8, CV_64F, A );
2556     CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
2557     CvMat matB = cvMat( 8, 1, CV_64F, b );
2558     CvMat matX = cvMat( 8, 1, CV_64F, c );
2559
2560     CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2561     CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2562     }
2563
2564     matrix[0][0] = c[0];
2565     matrix[0][1] = c[1];
2566     matrix[0][2] = c[2];
2567     matrix[1][0] = c[3];
2568     matrix[1][1] = c[4];
2569     matrix[1][2] = c[5];
2570     matrix[2][0] = c[6];
2571     matrix[2][1] = c[7];
2572     matrix[2][2] = 1.0;
2573
2574     if( map )
2575     {
2576         for( i = 0; i < size.height; i++ )
2577         {
2578             CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i);
2579             for( j = 0; j < size.width; j++ )
2580             {
2581                 double w = 1./(c[6]*j + c[7]*i + 1.);
2582                 double x = (c[0]*j + c[1]*i + c[2])*w;
2583                 double y = (c[3]*j + c[4]*i + c[5])*w;
2584
2585                 maprow[j].x = (float)x;
2586                 maprow[j].y = (float)y;
2587             }
2588         }
2589     }
2590
2591     __END__;
2592
2593     return;
2594 }
2595
2596
2597 /*-----------------------------------------------------------------------*/
2598 /* Compute projected infinite point for second image if first image point is known */
2599 void icvComputeeInfiniteProject1(   CvMatr64d     rotMatr,
2600                                     CvMatr64d     camMatr1,
2601                                     CvMatr64d     camMatr2,
2602                                     CvPoint2D32f  point1,
2603                                     CvPoint2D32f* point2)
2604 {
2605     double invMatr1[9];
2606     icvInvertMatrix_64d(camMatr1,3,invMatr1);
2607     double P1[3];
2608     double p1[3];
2609     p1[0] = (double)(point1.x);
2610     p1[1] = (double)(point1.y);
2611     p1[2] = 1;
2612
2613     icvMulMatrix_64d(   invMatr1,
2614                         3,3,
2615                         p1,
2616                         1,3,
2617                         P1);
2618
2619     double invR[9];
2620     icvTransposeMatrix_64d( rotMatr, 3, 3, invR );
2621
2622     /* Change system 1 to system 2 */
2623     double P2[3];
2624     icvMulMatrix_64d(   invR,
2625                         3,3,
2626                         P1,
2627                         1,3,
2628                         P2);
2629
2630     /* Now we can project this point to image 2 */
2631     double projP[3];
2632
2633     icvMulMatrix_64d(   camMatr2,
2634                         3,3,
2635                         P2,
2636                         1,3,
2637                         projP);
2638
2639     point2->x = (float)(projP[0] / projP[2]);
2640     point2->y = (float)(projP[1] / projP[2]);
2641
2642     return;
2643 }
2644
2645 /*-----------------------------------------------------------------------*/
2646 /* Compute projected infinite point for second image if first image point is known */
2647 void icvComputeeInfiniteProject2(   CvMatr64d     rotMatr,
2648                                     CvMatr64d     camMatr1,
2649                                     CvMatr64d     camMatr2,
2650                                     CvPoint2D32f*  point1,
2651                                     CvPoint2D32f point2)
2652 {
2653     double invMatr2[9];
2654     icvInvertMatrix_64d(camMatr2,3,invMatr2);
2655     double P2[3];
2656     double p2[3];
2657     p2[0] = (double)(point2.x);
2658     p2[1] = (double)(point2.y);
2659     p2[2] = 1;
2660
2661     icvMulMatrix_64d(   invMatr2,
2662                         3,3,
2663                         p2,
2664                         1,3,
2665                         P2);
2666
2667     /* Change system 1 to system 2 */
2668
2669     double P1[3];
2670     icvMulMatrix_64d(   rotMatr,
2671                         3,3,
2672                         P2,
2673                         1,3,
2674                         P1);
2675
2676     /* Now we can project this point to image 2 */
2677     double projP[3];
2678
2679     icvMulMatrix_64d(   camMatr1,
2680                         3,3,
2681                         P1,
2682                         1,3,
2683                         projP);
2684
2685     point1->x = (float)(projP[0] / projP[2]);
2686     point1->y = (float)(projP[1] / projP[2]);
2687
2688     return;
2689 }
2690
2691 /* Select best R and t for given cameras, points, ... */
2692 /* For both cameras */
2693 int icvSelectBestRt(           int           numImages,
2694                                     int*          numPoints,
2695                                     CvPoint2D32f* imagePoints1,
2696                                     CvPoint2D32f* imagePoints2,
2697                                     CvPoint3D32f* objectPoints,
2698
2699                                     CvMatr32f     cameraMatrix1,
2700                                     CvVect32f     distortion1,
2701                                     CvMatr32f     rotMatrs1,
2702                                     CvVect32f     transVects1,
2703
2704                                     CvMatr32f     cameraMatrix2,
2705                                     CvVect32f     distortion2,
2706                                     CvMatr32f     rotMatrs2,
2707                                     CvVect32f     transVects2,
2708
2709                                     CvMatr32f     bestRotMatr,
2710                                     CvVect32f     bestTransVect
2711                                     )
2712 {
2713
2714     /* Need to convert input data 32 -> 64 */
2715     CvPoint3D64d* objectPoints_64d;
2716
2717     double* rotMatrs1_64d;
2718     double* rotMatrs2_64d;
2719
2720     double* transVects1_64d;
2721     double* transVects2_64d;
2722
2723     double cameraMatrix1_64d[9];
2724     double cameraMatrix2_64d[9];
2725
2726     double distortion1_64d[4];
2727     double distortion2_64d[4];
2728
2729     /* allocate memory for 64d data */
2730     int totalNum = 0;
2731
2732     int i;
2733     for( i = 0; i < numImages; i++ )
2734     {
2735         totalNum += numPoints[i];
2736     }
2737
2738     objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));
2739
2740     rotMatrs1_64d    = (double*)calloc(numImages,sizeof(double)*9);
2741     rotMatrs2_64d    = (double*)calloc(numImages,sizeof(double)*9);
2742
2743     transVects1_64d  = (double*)calloc(numImages,sizeof(double)*3);
2744     transVects2_64d  = (double*)calloc(numImages,sizeof(double)*3);
2745
2746     /* Convert input data to 64d */
2747
2748     icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d,  totalNum*3);
2749
2750     icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d,  numImages*9);
2751     icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d,  numImages*9);
2752
2753     icvCvt_32f_64d(transVects1, transVects1_64d,  numImages*3);
2754     icvCvt_32f_64d(transVects2, transVects2_64d,  numImages*3);
2755
2756     /* Convert to arrays */
2757     icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
2758     icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
2759
2760     icvCvt_32f_64d(distortion1, distortion1_64d, 4);
2761     icvCvt_32f_64d(distortion2, distortion2_64d, 4);
2762
2763
2764     /* for each R and t compute error for image pair */
2765     float* errors;
2766
2767     errors = (float*)calloc(numImages*numImages,sizeof(float));
2768     if( errors == 0 )
2769     {
2770         return CV_OUTOFMEM_ERR;
2771     }
2772
2773     int currImagePair;
2774     int currRt;
2775     for( currRt = 0; currRt < numImages; currRt++ )
2776     {
2777         int begPoint = 0;
2778         for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2779         {
2780             /* For current R,t R,t compute relative position of cameras */
2781
2782             double convRotMatr[9];
2783             double convTransVect[3];
2784
2785             icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
2786                                       transVects1_64d + currRt*3,
2787                                       rotMatrs2_64d + currRt*9,
2788                                       transVects2_64d + currRt*3,
2789                                       convRotMatr,
2790                                       convTransVect);
2791
2792             /* Project points using relative position of cameras */
2793
2794             double convRotMatr2[9];
2795             double convTransVect2[3];
2796
2797             convRotMatr2[0] = 1;
2798             convRotMatr2[1] = 0;
2799             convRotMatr2[2] = 0;
2800
2801             convRotMatr2[3] = 0;
2802             convRotMatr2[4] = 1;
2803             convRotMatr2[5] = 0;
2804
2805             convRotMatr2[6] = 0;
2806             convRotMatr2[7] = 0;
2807             convRotMatr2[8] = 1;
2808
2809             convTransVect2[0] = 0;
2810             convTransVect2[1] = 0;
2811             convTransVect2[2] = 0;
2812
2813             /* Compute error for given pair and Rt */
2814             /* We must project points to image and compute error */
2815
2816             CvPoint2D64d* projImagePoints1;
2817             CvPoint2D64d* projImagePoints2;
2818
2819             CvPoint3D64d* points1;
2820             CvPoint3D64d* points2;
2821
2822             int numberPnt;
2823             numberPnt = numPoints[currImagePair];
2824             projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2825             projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2826
2827             points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2828             points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2829
2830             /* Transform object points to first camera position */
2831             int i;
2832             for( i = 0; i < numberPnt; i++ )
2833             {
2834                 /* Create second camera point */
2835                 CvPoint3D64d tmpPoint;
2836                 tmpPoint.x = (double)(objectPoints[i].x);
2837                 tmpPoint.y = (double)(objectPoints[i].y);
2838                 tmpPoint.z = (double)(objectPoints[i].z);
2839
2840                 icvConvertPointSystem(  tmpPoint,
2841                                         points2+i,
2842                                         rotMatrs2_64d + currImagePair*9,
2843                                         transVects2_64d + currImagePair*3);
2844
2845                 /* Create first camera point using R, t */
2846                 icvConvertPointSystem(  points2[i],
2847                                         points1+i,
2848                                         convRotMatr,
2849                                         convTransVect);
2850
2851                 CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
2852                 icvConvertPointSystem(  tmpPoint,
2853                                         &tmpPoint2,
2854                                         rotMatrs1_64d + currImagePair*9,
2855                                         transVects1_64d + currImagePair*3);
2856                 /*double err;
2857                 double dx,dy,dz;
2858                 dx = tmpPoint2.x - points1[i].x;
2859                 dy = tmpPoint2.y - points1[i].y;
2860                 dz = tmpPoint2.z - points1[i].z;
2861                 err = sqrt(dx*dx + dy*dy + dz*dz);*/
2862
2863
2864             }
2865
2866 #if 0
2867             cvProjectPointsSimple(  numPoints[currImagePair],
2868                                     objectPoints_64d + begPoint,
2869                                     rotMatrs1_64d + currRt*9,
2870                                     transVects1_64d + currRt*3,
2871                                     cameraMatrix1_64d,
2872                                     distortion1_64d,
2873                                     projImagePoints1);
2874
2875             cvProjectPointsSimple(  numPoints[currImagePair],
2876                                     objectPoints_64d + begPoint,
2877                                     rotMatrs2_64d + currRt*9,
2878                                     transVects2_64d + currRt*3,
2879                                     cameraMatrix2_64d,
2880                                     distortion2_64d,
2881                                     projImagePoints2);
2882 #endif
2883
2884             /* Project with no translate and no rotation */
2885
2886 #if 0
2887             {
2888                 double nodist[4] = {0,0,0,0};
2889                 cvProjectPointsSimple(  numPoints[currImagePair],
2890                                         points1,
2891                                         convRotMatr2,
2892                                         convTransVect2,
2893                                         cameraMatrix1_64d,
2894                                         nodist,
2895                                         projImagePoints1);
2896
2897                 cvProjectPointsSimple(  numPoints[currImagePair],
2898                                         points2,
2899                                         convRotMatr2,
2900                                         convTransVect2,
2901                                         cameraMatrix2_64d,
2902                                         nodist,
2903                                         projImagePoints2);
2904
2905             }
2906 #endif
2907
2908             cvProjectPointsSimple(  numPoints[currImagePair],
2909                                     points1,
2910                                     convRotMatr2,
2911                                     convTransVect2,
2912                                     cameraMatrix1_64d,
2913                                     distortion1_64d,
2914                                     projImagePoints1);
2915
2916             cvProjectPointsSimple(  numPoints[currImagePair],
2917                                     points2,
2918                                     convRotMatr2,
2919                                     convTransVect2,
2920                                     cameraMatrix2_64d,
2921                                     distortion2_64d,
2922                                     projImagePoints2);
2923
2924             /* points are projected. Compute error */
2925
2926             int currPoint;
2927             double err1 = 0;
2928             double err2 = 0;
2929             double err;
2930             for( currPoint = 0; currPoint < numberPnt; currPoint++ )
2931             {
2932                 double len1,len2;
2933                 double dx1,dy1;
2934                 dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
2935                 dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
2936                 len1 = sqrt(dx1*dx1 + dy1*dy1);
2937                 err1 += len1;
2938
2939                 double dx2,dy2;
2940                 dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
2941                 dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
2942                 len2 = sqrt(dx2*dx2 + dy2*dy2);
2943                 err2 += len2;
2944             }
2945
2946             err1 /= (float)(numberPnt);
2947             err2 /= (float)(numberPnt);
2948
2949             err = (err1+err2) * 0.5;
2950             begPoint += numberPnt;
2951
2952             /* Set this error to */
2953             errors[numImages*currImagePair+currRt] = (float)err;
2954
2955             free(points1);
2956             free(points2);
2957             free(projImagePoints1);
2958             free(projImagePoints2);
2959         }
2960     }
2961
2962     /* Just select R and t with minimal average error */
2963
2964     int bestnumRt = 0;
2965     float minError = 0;/* Just for no warnings. Uses 'first' flag. */
2966     int first = 1;
2967     for( currRt = 0; currRt < numImages; currRt++ )
2968     {
2969         float avErr = 0;
2970         for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2971         {
2972             avErr += errors[numImages*currImagePair+currRt];
2973         }
2974         avErr /= (float)(numImages);
2975
2976         if( first )
2977         {
2978             bestnumRt = 0;
2979             minError = avErr;
2980             first = 0;
2981         }
2982         else
2983         {
2984             if( avErr < minError )
2985             {
2986                 bestnumRt = currRt;
2987                 minError = avErr;
2988             }
2989         }
2990
2991     }
2992
2993     double bestRotMatr_64d[9];
2994     double bestTransVect_64d[3];
2995
2996     icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
2997                               transVects1_64d + bestnumRt * 3,
2998                               rotMatrs2_64d + bestnumRt * 9,
2999                               transVects2_64d + bestnumRt * 3,
3000                               bestRotMatr_64d,
3001                               bestTransVect_64d);
3002
3003     icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
3004     icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);
3005
3006
3007     free(errors);
3008
3009     return CV_OK;
3010 }
3011
3012
3013 /* ----------------- Stereo calibration functions --------------------- */
3014
3015 float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
3016 {
3017     float ax = point2.x - point1.x;
3018     float ay = point2.y - point1.y;
3019
3020     float bx = point.x - point1.x;
3021     float by = point.y - point1.y;
3022
3023     return (ax*by - ay*bx);
3024 }
3025
3026 /* Convert function for stereo warping */
3027 int icvConvertWarpCoordinates(double coeffs[3][3],
3028                                 CvPoint2D32f* cameraPoint,
3029                                 CvPoint2D32f* warpPoint,
3030                                 int direction)
3031 {
3032     double x,y;
3033     double det;
3034     if( direction == CV_WARP_TO_CAMERA )
3035     {/* convert from camera image to warped image coordinates */
3036         x = warpPoint->x;
3037         y = warpPoint->y;
3038
3039         det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
3040         if( fabs(det) > 1e-8 )
3041         {
3042             cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
3043             cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
3044             return CV_OK;
3045         }
3046     }
3047     else if( direction == CV_CAMERA_TO_WARP )
3048     {/* convert from warped image to camera image coordinates */
3049         x = cameraPoint->x;
3050         y = cameraPoint->y;
3051
3052         det = (coeffs[2][0]*x-coeffs[0][0])*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[2][0]*y-coeffs[1][0]);
3053
3054         if( fabs(det) > 1e-8 )
3055         {
3056             warpPoint->x = (float)(((coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[1][2]-coeffs[2][2]*y))/det);
3057             warpPoint->y = (float)(((coeffs[2][0]*x-coeffs[0][0])*(coeffs[1][2]-coeffs[2][2]*y)-(coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][0]*y-coeffs[1][0]))/det);
3058             return CV_OK;
3059         }
3060     }
3061
3062     return CV_BADFACTOR_ERR;
3063 }
3064
3065 /* Compute stereo params using some camera params */
3066 /* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
3067 int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
3068 {
3069
3070
3071     icvGetQuadsTransformStruct(stereoparams);
3072
3073     cvInitPerspectiveTransform( stereoparams->warpSize,
3074                                 stereoparams->quad[0],
3075                                 stereoparams->coeffs[0],
3076                                 0);
3077
3078     cvInitPerspectiveTransform( stereoparams->warpSize,
3079                                 stereoparams->quad[1],
3080                                 stereoparams->coeffs[1],
3081                                 0);
3082
3083     /* Create border for warped images */
3084     CvPoint2D32f corns[4];
3085     corns[0].x = 0;
3086     corns[0].y = 0;
3087
3088     corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3089     corns[1].y = 0;
3090
3091     corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3092     corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3093
3094     corns[3].x = 0;
3095     corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3096
3097     int i;
3098     for( i = 0; i < 4; i++ )
3099     {
3100         /* For first camera */
3101         icvConvertWarpCoordinates( stereoparams->coeffs[0],
3102                                 corns+i,
3103                                 stereoparams->border[0]+i,
3104                                 CV_CAMERA_TO_WARP);
3105
3106         /* For second camera */
3107         icvConvertWarpCoordinates( stereoparams->coeffs[1],
3108                                 corns+i,
3109                                 stereoparams->border[1]+i,
3110                                 CV_CAMERA_TO_WARP);
3111     }
3112
3113     /* Test compute  */
3114     {
3115         CvPoint2D32f warpPoints[4];
3116         warpPoints[0] = cvPoint2D32f(0,0);
3117         warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0);
3118         warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1);
3119         warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1);
3120
3121         CvPoint2D32f camPoints1[4];
3122         CvPoint2D32f camPoints2[4];
3123
3124         for( int i = 0; i < 4; i++ )
3125         {
3126             icvConvertWarpCoordinates(stereoparams->coeffs[0],
3127                                 camPoints1+i,
3128                                 warpPoints+i,
3129                                 CV_WARP_TO_CAMERA);
3130
3131             icvConvertWarpCoordinates(stereoparams->coeffs[1],
3132                                 camPoints2+i,
3133                                 warpPoints+i,
3134                                 CV_WARP_TO_CAMERA);
3135         }
3136     }
3137
3138
3139     /* Allocate memory for scanlines coeffs */
3140
3141     stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));
3142
3143     /* Compute coeffs for epilines  */
3144
3145     icvComputeCoeffForStereo( stereoparams);
3146
3147     /* all coeffs are known */
3148     return CV_OK;
3149 }
3150
3151 /*-------------------------------------------------------------------------------------------*/
3152
3153 int icvStereoCalibration( int numImages,
3154                             int* nums,
3155                             CvSize imageSize,
3156                             CvPoint2D32f* imagePoints1,
3157                             CvPoint2D32f* imagePoints2,
3158                             CvPoint3D32f* objectPoints,
3159                             CvStereoCamera* stereoparams
3160                            )
3161 {
3162     /* Firstly we must calibrate both cameras */
3163     /*  Alocate memory for data */
3164     /* Allocate for translate vectors */
3165     float* transVects1;
3166     float* transVects2;
3167     float* rotMatrs1;
3168     float* rotMatrs2;
3169
3170     transVects1 = (float*)calloc(numImages,sizeof(float)*3);
3171     transVects2 = (float*)calloc(numImages,sizeof(float)*3);
3172
3173     rotMatrs1   = (float*)calloc(numImages,sizeof(float)*9);
3174     rotMatrs2   = (float*)calloc(numImages,sizeof(float)*9);
3175
3176     /* Calibrate first camera */
3177     cvCalibrateCamera(  numImages,
3178                         nums,
3179                         imageSize,
3180                         imagePoints1,
3181                         objectPoints,
3182                         stereoparams->camera[0]->distortion,
3183                         stereoparams->camera[0]->matrix,
3184                         transVects1,
3185                         rotMatrs1,
3186                         1);
3187
3188     /* Calibrate second camera */
3189     cvCalibrateCamera(  numImages,
3190                         nums,
3191                         imageSize,
3192                         imagePoints2,
3193                         objectPoints,
3194                         stereoparams->camera[1]->distortion,
3195                         stereoparams->camera[1]->matrix,
3196                         transVects2,
3197                         rotMatrs2,
3198                         1);
3199
3200     /* Cameras are calibrated */
3201
3202     stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
3203     stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
3204
3205     stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
3206     stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
3207
3208     icvSelectBestRt(    numImages,
3209                         nums,
3210                         imagePoints1,
3211                         imagePoints2,
3212                         objectPoints,
3213                         stereoparams->camera[0]->matrix,
3214                         stereoparams->camera[0]->distortion,
3215                         rotMatrs1,
3216                         transVects1,
3217                         stereoparams->camera[1]->matrix,
3218                         stereoparams->camera[1]->distortion,
3219                         rotMatrs2,
3220                         transVects2,
3221                         stereoparams->rotMatrix,
3222                         stereoparams->transVector
3223                         );
3224
3225     /* Free memory */
3226     free(transVects1);
3227     free(transVects2);
3228     free(rotMatrs1);
3229     free(rotMatrs2);
3230
3231     icvComputeRestStereoParams(stereoparams);
3232
3233     return CV_NO_ERR;
3234 }
3235
3236 /* Find line from epipole */
3237 void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
3238 {
3239     CvPoint2D32f frameBeg;
3240     CvPoint2D32f frameEnd;
3241     CvPoint2D32f cross[4];
3242     int     haveCross[4];
3243     float   dist;
3244
3245     haveCross[0] = 0;
3246     haveCross[1] = 0;
3247     haveCross[2] = 0;
3248     haveCross[3] = 0;
3249
3250     frameBeg.x = 0;
3251     frameBeg.y = 0;
3252     frameEnd.x = (float)(imageSize.width);
3253     frameEnd.y = 0;
3254     haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);
3255
3256     frameBeg.x = (float)(imageSize.width);
3257     frameBeg.y = 0;
3258     frameEnd.x = (float)(imageSize.width);
3259     frameEnd.y = (float)(imageSize.height);
3260     haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);
3261
3262     frameBeg.x = (float)(imageSize.width);
3263     frameBeg.y = (float)(imageSize.height);
3264     frameEnd.x = 0;
3265     frameEnd.y = (float)(imageSize.height);
3266     haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);
3267
3268     frameBeg.x = 0;
3269     frameBeg.y = (float)(imageSize.height);
3270     frameEnd.x = 0;
3271     frameEnd.y = 0;
3272     haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);
3273
3274     int n;
3275     float minDist = (float)(INT_MAX);
3276     float maxDist = (float)(INT_MIN);
3277
3278     int maxN = -1;
3279     int minN = -1;
3280
3281     for( n = 0; n < 4; n++ )
3282     {
3283         if( haveCross[n] > 0 )
3284         {
3285             dist =  (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
3286                     (epipole.y - cross[n].y)*(epipole.y - cross[n].y);
3287
3288             if( dist < minDist )
3289             {
3290                 minDist = dist;
3291                 minN = n;
3292             }
3293
3294             if( dist > maxDist )
3295             {
3296                 maxDist = dist;
3297                 maxN = n;
3298             }
3299         }
3300     }
3301
3302     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3303     {
3304         *start = cross[minN];
3305         *end   = cross[maxN];
3306     }
3307     else
3308     {
3309         start->x = 0;
3310         start->y = 0;
3311         end->x = 0;
3312         end->y = 0;
3313     }
3314
3315     return;
3316 }
3317
3318
3319 /* Find line which cross frame by line(a,b,c) */
3320 void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
3321 {
3322     CvPoint2D32f frameBeg;
3323     CvPoint2D32f frameEnd;
3324     CvPoint2D32f cross[4];
3325     int     haveCross[4];
3326     float   dist;
3327
3328     haveCross[0] = 0;
3329     haveCross[1] = 0;
3330     haveCross[2] = 0;
3331     haveCross[3] = 0;
3332
3333     frameBeg.x = 0;
3334     frameBeg.y = 0;
3335     frameEnd.x = (float)(imageSize.width);
3336     frameEnd.y = 0;
3337     haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
3338
3339     frameBeg.x = (float)(imageSize.width);
3340     frameBeg.y = 0;
3341     frameEnd.x = (float)(imageSize.width);
3342     frameEnd.y = (float)(imageSize.height);
3343     haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
3344
3345     frameBeg.x = (float)(imageSize.width);
3346     frameBeg.y = (float)(imageSize.height);
3347     frameEnd.x = 0;
3348     frameEnd.y = (float)(imageSize.height);
3349     haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
3350
3351     frameBeg.x = 0;
3352     frameBeg.y = (float)(imageSize.height);
3353     frameEnd.x = 0;
3354     frameEnd.y = 0;
3355     haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
3356
3357     int n;
3358     float minDist = (float)(INT_MAX);
3359     float maxDist = (float)(INT_MIN);
3360
3361     int maxN = -1;
3362     int minN = -1;
3363
3364     double midPointX = imageSize.width  / 2.0;
3365     double midPointY = imageSize.height / 2.0;
3366
3367     for( n = 0; n < 4; n++ )
3368     {
3369         if( haveCross[n] > 0 )
3370         {
3371             dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
3372                             (midPointY - cross[n].y)*(midPointY - cross[n].y));
3373
3374             if( dist < minDist )
3375             {
3376                 minDist = dist;
3377                 minN = n;
3378             }
3379
3380             if( dist > maxDist )
3381             {
3382                 maxDist = dist;
3383                 maxN = n;
3384             }
3385         }
3386     }
3387
3388     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3389     {
3390         *start = cross[minN];
3391         *end   = cross[maxN];
3392     }
3393     else
3394     {
3395         start->x = 0;
3396         start->y = 0;
3397         end->x = 0;
3398         end->y = 0;
3399     }
3400
3401     return;
3402
3403 }
3404
3405 /* Cross lines */
3406 int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
3407 {
3408     double ex1,ey1,ex2,ey2;
3409     double px1,py1,px2,py2;
3410     double del;
3411     double delA,delB,delX,delY;
3412     double alpha,betta;
3413
3414     ex1 = p1_start.x;
3415     ey1 = p1_start.y;
3416     ex2 = p1_end.x;
3417     ey2 = p1_end.y;
3418
3419     px1 = p2_start.x;
3420     py1 = p2_start.y;
3421     px2 = p2_end.x;
3422     py2 = p2_end.y;
3423
3424     del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3425     if( del == 0)
3426     {
3427         return -1;
3428     }
3429
3430     delA =  (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3431     delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3432
3433     alpha =  delA / del;
3434     betta = -delB / del;
3435
3436     if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
3437     {
3438         return -1;
3439     }
3440
3441     delX =  (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3442             (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3443
3444     delY =  (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3445             (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3446
3447     cross->x = (float)( delX / del);
3448     cross->y = (float)(-delY / del);
3449     return 1;
3450 }
3451
3452
3453 int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
3454 {
3455     double ex1 = p1_start.x;
3456     double ey1 = p1_start.y;
3457     double ex2 = p1_end.x;
3458     double ey2 = p1_end.y;
3459
3460     double px1 = v2_start.x;
3461     double py1 = v2_start.y;
3462     double px2 = v2_end.x;
3463     double py2 = v2_end.y;
3464
3465     double del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3466     if( del == 0)
3467     {
3468         return -1;
3469     }
3470
3471     double delA =  (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3472     //double delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3473
3474     double alpha =  delA / del;
3475     //double betta = -delB / del;
3476
3477     if( alpha < 0 || alpha > 1.0 )
3478     {
3479         return -1;
3480     }
3481
3482     double delX =  (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3483             (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3484
3485     double delY =  (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3486             (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3487
3488     cross->x = (float)( delX / del);
3489     cross->y = (float)(-delY / del);
3490     return 1;
3491 }
3492
3493
3494 int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
3495 {
3496     double del;
3497     double delX,delY,delA;
3498
3499     double px1,px2,py1,py2;
3500     double X,Y,alpha;
3501
3502     px1 = p1.x;
3503     py1 = p1.y;
3504
3505     px2 = p2.x;
3506     py2 = p2.y;
3507
3508     del = a * (px2 - px1) + b * (py2-py1);
3509     if( del == 0 )
3510     {
3511         return -1;
3512     }
3513
3514     delA = - c - a*px1 - b*py1;
3515     alpha = delA / del;
3516
3517     if( alpha < 0 || alpha > 1.0 )
3518     {
3519         return -1;/* no cross */
3520     }
3521
3522     delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
3523     delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);
3524
3525     X = delX / del;
3526     Y = delY / del;
3527
3528     cross->x = (float)X;
3529     cross->y = (float)Y;
3530
3531     return 1;
3532 }
3533
3534 int cvComputeEpipoles( CvMatr32f camMatr1,  CvMatr32f camMatr2,
3535                             CvMatr32f rotMatr1,  CvMatr32f rotMatr2,
3536                             CvVect32f transVect1,CvVect32f transVect2,
3537                             CvVect32f epipole1,
3538                             CvVect32f epipole2)
3539 {
3540
3541     /* Copy matrix */
3542
3543     CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1);
3544     CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2);
3545     CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1);
3546     CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2);
3547     CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1);
3548     CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2);
3549     CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1);
3550     CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2);
3551
3552
3553     CvMat cmatrP1   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1);
3554     CvMat cmatrP2   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2);
3555     CvMat cvectp1   = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1);
3556     CvMat cvectp2   = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2);
3557     CvMat ctmpF1    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1);
3558     CvMat ctmpM1    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1);
3559     CvMat ctmpM2    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2);
3560     CvMat cinvP1    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1);
3561     CvMat cinvP2    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2);
3562     CvMat ctmpMatr  = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr);
3563     CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1);
3564     CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2);
3565     CvMat cmatrF1   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1);
3566     CvMat ctmpF     = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF);
3567     CvMat ctmpE1    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1);
3568     CvMat ctmpE2    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2);
3569
3570     /* Compute first */
3571     cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
3572     cvmInvert( &cmatrP1,&cinvP1 );
3573     cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );
3574
3575     /* Compute second */
3576     cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
3577     cvmInvert( &cmatrP2,&cinvP2 );
3578     cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );
3579
3580     cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
3581     cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
3582     cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);
3583
3584     cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
3585     cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
3586     cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);
3587
3588
3589     /* Need scale */
3590
3591     cvmScale(&ctmpE1,&cepipole1,1.0);
3592     cvmScale(&ctmpE2,&cepipole2,1.0);
3593
3594     cvmFree(&cmatrP1);
3595     cvmFree(&cmatrP1);
3596     cvmFree(&cvectp1);
3597     cvmFree(&cvectp2);
3598     cvmFree(&ctmpF1);
3599     cvmFree(&ctmpM1);
3600     cvmFree(&ctmpM2);
3601     cvmFree(&cinvP1);
3602     cvmFree(&cinvP2);
3603     cvmFree(&ctmpMatr);
3604     cvmFree(&ctmpVect1);
3605     cvmFree(&ctmpVect2);
3606     cvmFree(&cmatrF1);
3607     cvmFree(&ctmpF);
3608     cvmFree(&ctmpE1);
3609     cvmFree(&ctmpE2);
3610
3611     return CV_NO_ERR;
3612 }/* cvComputeEpipoles */
3613
3614
3615 /* Compute epipoles for fundamental matrix */
3616 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
3617                                          CvPoint3D32f* epipole1,
3618                                          CvPoint3D32f* epipole2)
3619 {
3620     /* Decompose fundamental matrix using SVD ( A = U W V') */
3621     CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
3622
3623     CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
3624     CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
3625     CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);
3626
3627     /* From svd we need just last vector of U and V or last row from U' and V' */
3628     /* We get transposed matrixes U and V */
3629     cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T);
3630
3631     /* Get last row from U' and compute epipole1 */
3632     epipole1->x = matrU->data.fl[6];
3633     epipole1->y = matrU->data.fl[7];
3634     epipole1->z = matrU->data.fl[8];
3635
3636     /* Get last row from V' and compute epipole2 */
3637     epipole2->x = matrV->data.fl[6];
3638     epipole2->y = matrV->data.fl[7];
3639     epipole2->z = matrV->data.fl[8];
3640
3641     cvReleaseMat(&matrW);
3642     cvReleaseMat(&matrU);
3643     cvReleaseMat(&matrV);
3644     return CV_OK;
3645 }
3646
3647 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
3648                                          CvMatr32f fundMatr,
3649                                          CvMatr32f cameraMatr1,
3650                                          CvMatr32f cameraMatr2)
3651 {/* Fund = inv(CM1') * Ess * inv(CM2) */
3652
3653     CvMat essMatrC     = cvMat(3,3,CV_MAT32F,essMatr);
3654     CvMat fundMatrC    = cvMat(3,3,CV_MAT32F,fundMatr);
3655     CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1);
3656     CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2);
3657
3658     CvMat* invCM2  = cvCreateMat(3,3,CV_MAT32F);
3659     CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
3660     CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);
3661
3662     cvTranspose(&cameraMatr1C,tmpMatr);
3663     cvInvert(tmpMatr,invCM1T);
3664     cvmMul(invCM1T,&essMatrC,tmpMatr);
3665     cvInvert(&cameraMatr2C,invCM2);
3666     cvmMul(tmpMatr,invCM2,&fundMatrC);
3667
3668     /* Scale fundamental matrix */
3669     double scale;
3670     scale = 1.0/fundMatrC.data.fl[8];
3671     cvConvertScale(&fundMatrC,&fundMatrC,scale);
3672
3673     cvReleaseMat(&invCM2);
3674     cvReleaseMat(&tmpMatr);
3675     cvReleaseMat(&invCM1T);
3676
3677     return CV_OK;
3678 }
3679
3680 /* Compute essential matrix */
3681
3682 int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
3683                                     CvMatr32f transVect,
3684                                     CvMatr32f essMatr)
3685 {
3686     float transMatr[9];
3687
3688     /* Make antisymmetric matrix from transpose vector */
3689     transMatr[0] =   0;
3690     transMatr[1] = - transVect[2];
3691     transMatr[2] =   transVect[1];
3692
3693     transMatr[3] =   transVect[2];
3694     transMatr[4] =   0;
3695     transMatr[5] = - transVect[0];
3696
3697     transMatr[6] = - transVect[1];
3698     transMatr[7] =   transVect[0];
3699     transMatr[8] =   0;
3700
3701     icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);
3702
3703     return CV_OK;
3704 }
3705
3706