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 static 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 static 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 static 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 static 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 != src_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 static 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 static 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 #if 0
2171 /* Find line which cross frame by line(a,b,c) */
2172 static 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 static int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
2264 {
2265     float width  = (float)(imageSize.width);
2266     float height = (float)(imageSize.height);
2267
2268     /* Get crosslines with image corners */
2269
2270     /* Find four lines */
2271
2272     CvPoint2D32f pa,pb,pc,pd;
2273
2274     pa.x = 0;
2275     pa.y = 0;
2276
2277     pb.x = width;
2278     pb.y = 0;
2279
2280     pd.x = width;
2281     pd.y = height;
2282
2283     pc.x = 0;
2284     pc.y = height;
2285
2286     /* We can compute points for angle */
2287     /* Test for place section */
2288
2289     float x,y;
2290     x = epipole.x;
2291     y = epipole.y;
2292
2293     if( x < 0 )
2294     {/* 1,4,7 */
2295         if( y < 0)
2296         {/* 1 */
2297             point1 = pb;
2298             point2 = pc;
2299         }
2300         else if( y > height )
2301         {/* 7 */
2302             point1 = pa;
2303             point2 = pd;
2304         }
2305         else
2306         {/* 4 */
2307             point1 = pa;
2308             point2 = pc;
2309         }
2310     }
2311     else if ( x > width )
2312     {/* 3,6,9 */
2313         if( y < 0 )
2314         {/* 3 */
2315             point1 = pa;
2316             point2 = pd;
2317         }
2318         else if ( y > height )
2319         {/* 9 */
2320             point1 = pc;
2321             point2 = pb;
2322         }
2323         else
2324         {/* 6 */
2325             point1 = pb;
2326             point2 = pd;
2327         }
2328     }
2329     else
2330     {/* 2,5,8 */
2331         if( y < 0 )
2332         {/* 2 */
2333             point1 = pa;
2334             point2 = pb;
2335         }
2336         else if( y > height )
2337         {/* 8 */
2338             point1 = pc;
2339             point2 = pd;
2340         }
2341         else
2342         {/* 5 - point in the image */
2343             return 2;
2344         }
2345
2346
2347     }
2348
2349
2350     return 0;
2351 }
2352
2353 /*--------------------------------------------------------------------------------------*/
2354 static void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])
2355 {/* Computes perspective coeffs for transformation from src to dst quad */
2356
2357
2358     CV_FUNCNAME( "icvComputePerspectiveCoeffs" );
2359
2360     __BEGIN__;
2361
2362     double A[64];
2363     double b[8];
2364     double c[8];
2365     CvPoint2D32f pt[4];
2366     int i;
2367
2368     pt[0] = srcQuad[0];
2369     pt[1] = srcQuad[1];
2370     pt[2] = srcQuad[2];
2371     pt[3] = srcQuad[3];
2372
2373     for( i = 0; i < 4; i++ )
2374     {
2375 #if 0
2376         double x = dstQuad[i].x;
2377         double y = dstQuad[i].y;
2378         double X = pt[i].x;
2379         double Y = pt[i].y;
2380 #else
2381         double x = pt[i].x;
2382         double y = pt[i].y;
2383         double X = dstQuad[i].x;
2384         double Y = dstQuad[i].y;
2385 #endif
2386         double* a = A + i*16;
2387
2388         a[0] = x;
2389         a[1] = y;
2390         a[2] = 1;
2391         a[3] = 0;
2392         a[4] = 0;
2393         a[5] = 0;
2394         a[6] = -X*x;
2395         a[7] = -X*y;
2396
2397         a += 8;
2398
2399         a[0] = 0;
2400         a[1] = 0;
2401         a[2] = 0;
2402         a[3] = x;
2403         a[4] = y;
2404         a[5] = 1;
2405         a[6] = -Y*x;
2406         a[7] = -Y*y;
2407
2408         b[i*2] = X;
2409         b[i*2 + 1] = Y;
2410     }
2411
2412     {
2413     double invA[64];
2414     CvMat matA = cvMat( 8, 8, CV_64F, A );
2415     CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
2416     CvMat matB = cvMat( 8, 1, CV_64F, b );
2417     CvMat matX = cvMat( 8, 1, CV_64F, c );
2418
2419     CV_CALL( cvPseudoInverse( &matA, &matInvA ));
2420     CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
2421     }
2422
2423     coeffs[0][0] = c[0];
2424     coeffs[0][1] = c[1];
2425     coeffs[0][2] = c[2];
2426     coeffs[1][0] = c[3];
2427     coeffs[1][1] = c[4];
2428     coeffs[1][2] = c[5];
2429     coeffs[2][0] = c[6];
2430     coeffs[2][1] = c[7];
2431     coeffs[2][2] = 1.0;
2432
2433     __END__;
2434
2435     return;
2436 }
2437 #endif
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 static 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     for(int i = 0; i < numImages; i++ )
2733     {
2734         totalNum += numPoints[i];
2735     }
2736
2737     objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));
2738
2739     rotMatrs1_64d    = (double*)calloc(numImages,sizeof(double)*9);
2740     rotMatrs2_64d    = (double*)calloc(numImages,sizeof(double)*9);
2741
2742     transVects1_64d  = (double*)calloc(numImages,sizeof(double)*3);
2743     transVects2_64d  = (double*)calloc(numImages,sizeof(double)*3);
2744
2745     /* Convert input data to 64d */
2746
2747     icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d,  totalNum*3);
2748
2749     icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d,  numImages*9);
2750     icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d,  numImages*9);
2751
2752     icvCvt_32f_64d(transVects1, transVects1_64d,  numImages*3);
2753     icvCvt_32f_64d(transVects2, transVects2_64d,  numImages*3);
2754
2755     /* Convert to arrays */
2756     icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
2757     icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
2758
2759     icvCvt_32f_64d(distortion1, distortion1_64d, 4);
2760     icvCvt_32f_64d(distortion2, distortion2_64d, 4);
2761
2762
2763     /* for each R and t compute error for image pair */
2764     float* errors;
2765
2766     errors = (float*)calloc(numImages*numImages,sizeof(float));
2767     if( errors == 0 )
2768     {
2769         return CV_OUTOFMEM_ERR;
2770     }
2771
2772     int currImagePair;
2773     int currRt;
2774     for( currRt = 0; currRt < numImages; currRt++ )
2775     {
2776         int begPoint = 0;
2777         for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2778         {
2779             /* For current R,t R,t compute relative position of cameras */
2780
2781             double convRotMatr[9];
2782             double convTransVect[3];
2783
2784             icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
2785                                       transVects1_64d + currRt*3,
2786                                       rotMatrs2_64d + currRt*9,
2787                                       transVects2_64d + currRt*3,
2788                                       convRotMatr,
2789                                       convTransVect);
2790
2791             /* Project points using relative position of cameras */
2792
2793             double convRotMatr2[9];
2794             double convTransVect2[3];
2795
2796             convRotMatr2[0] = 1;
2797             convRotMatr2[1] = 0;
2798             convRotMatr2[2] = 0;
2799
2800             convRotMatr2[3] = 0;
2801             convRotMatr2[4] = 1;
2802             convRotMatr2[5] = 0;
2803
2804             convRotMatr2[6] = 0;
2805             convRotMatr2[7] = 0;
2806             convRotMatr2[8] = 1;
2807
2808             convTransVect2[0] = 0;
2809             convTransVect2[1] = 0;
2810             convTransVect2[2] = 0;
2811
2812             /* Compute error for given pair and Rt */
2813             /* We must project points to image and compute error */
2814
2815             CvPoint2D64d* projImagePoints1;
2816             CvPoint2D64d* projImagePoints2;
2817
2818             CvPoint3D64d* points1;
2819             CvPoint3D64d* points2;
2820
2821             int numberPnt;
2822             numberPnt = numPoints[currImagePair];
2823             projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2824             projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
2825
2826             points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2827             points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
2828
2829             /* Transform object points to first camera position */
2830             for(int i = 0; i < numberPnt; i++ )
2831             {
2832                 /* Create second camera point */
2833                 CvPoint3D64d tmpPoint;
2834                 tmpPoint.x = (double)(objectPoints[i].x);
2835                 tmpPoint.y = (double)(objectPoints[i].y);
2836                 tmpPoint.z = (double)(objectPoints[i].z);
2837
2838                 icvConvertPointSystem(  tmpPoint,
2839                                         points2+i,
2840                                         rotMatrs2_64d + currImagePair*9,
2841                                         transVects2_64d + currImagePair*3);
2842
2843                 /* Create first camera point using R, t */
2844                 icvConvertPointSystem(  points2[i],
2845                                         points1+i,
2846                                         convRotMatr,
2847                                         convTransVect);
2848
2849                 CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
2850                 icvConvertPointSystem(  tmpPoint,
2851                                         &tmpPoint2,
2852                                         rotMatrs1_64d + currImagePair*9,
2853                                         transVects1_64d + currImagePair*3);
2854                 /*double err;
2855                 double dx,dy,dz;
2856                 dx = tmpPoint2.x - points1[i].x;
2857                 dy = tmpPoint2.y - points1[i].y;
2858                 dz = tmpPoint2.z - points1[i].z;
2859                 err = sqrt(dx*dx + dy*dy + dz*dz);*/
2860             }
2861
2862 #if 0
2863             cvProjectPointsSimple(  numPoints[currImagePair],
2864                                     objectPoints_64d + begPoint,
2865                                     rotMatrs1_64d + currRt*9,
2866                                     transVects1_64d + currRt*3,
2867                                     cameraMatrix1_64d,
2868                                     distortion1_64d,
2869                                     projImagePoints1);
2870
2871             cvProjectPointsSimple(  numPoints[currImagePair],
2872                                     objectPoints_64d + begPoint,
2873                                     rotMatrs2_64d + currRt*9,
2874                                     transVects2_64d + currRt*3,
2875                                     cameraMatrix2_64d,
2876                                     distortion2_64d,
2877                                     projImagePoints2);
2878 #endif
2879
2880             /* Project with no translate and no rotation */
2881
2882 #if 0
2883             {
2884                 double nodist[4] = {0,0,0,0};
2885                 cvProjectPointsSimple(  numPoints[currImagePair],
2886                                         points1,
2887                                         convRotMatr2,
2888                                         convTransVect2,
2889                                         cameraMatrix1_64d,
2890                                         nodist,
2891                                         projImagePoints1);
2892
2893                 cvProjectPointsSimple(  numPoints[currImagePair],
2894                                         points2,
2895                                         convRotMatr2,
2896                                         convTransVect2,
2897                                         cameraMatrix2_64d,
2898                                         nodist,
2899                                         projImagePoints2);
2900
2901             }
2902 #endif
2903
2904             cvProjectPointsSimple(  numPoints[currImagePair],
2905                                     points1,
2906                                     convRotMatr2,
2907                                     convTransVect2,
2908                                     cameraMatrix1_64d,
2909                                     distortion1_64d,
2910                                     projImagePoints1);
2911
2912             cvProjectPointsSimple(  numPoints[currImagePair],
2913                                     points2,
2914                                     convRotMatr2,
2915                                     convTransVect2,
2916                                     cameraMatrix2_64d,
2917                                     distortion2_64d,
2918                                     projImagePoints2);
2919
2920             /* points are projected. Compute error */
2921
2922             int currPoint;
2923             double err1 = 0;
2924             double err2 = 0;
2925             double err;
2926             for( currPoint = 0; currPoint < numberPnt; currPoint++ )
2927             {
2928                 double len1,len2;
2929                 double dx1,dy1;
2930                 dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
2931                 dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
2932                 len1 = sqrt(dx1*dx1 + dy1*dy1);
2933                 err1 += len1;
2934
2935                 double dx2,dy2;
2936                 dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
2937                 dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
2938                 len2 = sqrt(dx2*dx2 + dy2*dy2);
2939                 err2 += len2;
2940             }
2941
2942             err1 /= (float)(numberPnt);
2943             err2 /= (float)(numberPnt);
2944
2945             err = (err1+err2) * 0.5;
2946             begPoint += numberPnt;
2947
2948             /* Set this error to */
2949             errors[numImages*currImagePair+currRt] = (float)err;
2950
2951             free(points1);
2952             free(points2);
2953             free(projImagePoints1);
2954             free(projImagePoints2);
2955         }
2956     }
2957
2958     /* Just select R and t with minimal average error */
2959
2960     int bestnumRt = 0;
2961     float minError = 0;/* Just for no warnings. Uses 'first' flag. */
2962     int first = 1;
2963     for( currRt = 0; currRt < numImages; currRt++ )
2964     {
2965         float avErr = 0;
2966         for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
2967         {
2968             avErr += errors[numImages*currImagePair+currRt];
2969         }
2970         avErr /= (float)(numImages);
2971
2972         if( first )
2973         {
2974             bestnumRt = 0;
2975             minError = avErr;
2976             first = 0;
2977         }
2978         else
2979         {
2980             if( avErr < minError )
2981             {
2982                 bestnumRt = currRt;
2983                 minError = avErr;
2984             }
2985         }
2986
2987     }
2988
2989     double bestRotMatr_64d[9];
2990     double bestTransVect_64d[3];
2991
2992     icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
2993                               transVects1_64d + bestnumRt * 3,
2994                               rotMatrs2_64d + bestnumRt * 9,
2995                               transVects2_64d + bestnumRt * 3,
2996                               bestRotMatr_64d,
2997                               bestTransVect_64d);
2998
2999     icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
3000     icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);
3001
3002
3003     free(errors);
3004
3005     return CV_OK;
3006 }
3007
3008
3009 /* ----------------- Stereo calibration functions --------------------- */
3010
3011 float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
3012 {
3013     float ax = point2.x - point1.x;
3014     float ay = point2.y - point1.y;
3015
3016     float bx = point.x - point1.x;
3017     float by = point.y - point1.y;
3018
3019     return (ax*by - ay*bx);
3020 }
3021
3022 /* Convert function for stereo warping */
3023 int icvConvertWarpCoordinates(double coeffs[3][3],
3024                                 CvPoint2D32f* cameraPoint,
3025                                 CvPoint2D32f* warpPoint,
3026                                 int direction)
3027 {
3028     double x,y;
3029     double det;
3030     if( direction == CV_WARP_TO_CAMERA )
3031     {/* convert from camera image to warped image coordinates */
3032         x = warpPoint->x;
3033         y = warpPoint->y;
3034
3035         det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
3036         if( fabs(det) > 1e-8 )
3037         {
3038             cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
3039             cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
3040             return CV_OK;
3041         }
3042     }
3043     else if( direction == CV_CAMERA_TO_WARP )
3044     {/* convert from warped image to camera image coordinates */
3045         x = cameraPoint->x;
3046         y = cameraPoint->y;
3047
3048         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]);
3049
3050         if( fabs(det) > 1e-8 )
3051         {
3052             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);
3053             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);
3054             return CV_OK;
3055         }
3056     }
3057
3058     return CV_BADFACTOR_ERR;
3059 }
3060
3061 /* Compute stereo params using some camera params */
3062 /* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
3063 int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
3064 {
3065
3066
3067     icvGetQuadsTransformStruct(stereoparams);
3068
3069     cvInitPerspectiveTransform( stereoparams->warpSize,
3070                                 stereoparams->quad[0],
3071                                 stereoparams->coeffs[0],
3072                                 0);
3073
3074     cvInitPerspectiveTransform( stereoparams->warpSize,
3075                                 stereoparams->quad[1],
3076                                 stereoparams->coeffs[1],
3077                                 0);
3078
3079     /* Create border for warped images */
3080     CvPoint2D32f corns[4];
3081     corns[0].x = 0;
3082     corns[0].y = 0;
3083
3084     corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3085     corns[1].y = 0;
3086
3087     corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
3088     corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3089
3090     corns[3].x = 0;
3091     corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
3092
3093     for(int i = 0; i < 4; i++ )
3094     {
3095         /* For first camera */
3096         icvConvertWarpCoordinates( stereoparams->coeffs[0],
3097                                 corns+i,
3098                                 stereoparams->border[0]+i,
3099                                 CV_CAMERA_TO_WARP);
3100
3101         /* For second camera */
3102         icvConvertWarpCoordinates( stereoparams->coeffs[1],
3103                                 corns+i,
3104                                 stereoparams->border[1]+i,
3105                                 CV_CAMERA_TO_WARP);
3106     }
3107
3108     /* Test compute  */
3109     {
3110         CvPoint2D32f warpPoints[4];
3111         warpPoints[0] = cvPoint2D32f(0,0);
3112         warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0);
3113         warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1);
3114         warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1);
3115
3116         CvPoint2D32f camPoints1[4];
3117         CvPoint2D32f camPoints2[4];
3118
3119         for( int i = 0; i < 4; i++ )
3120         {
3121             icvConvertWarpCoordinates(stereoparams->coeffs[0],
3122                                 camPoints1+i,
3123                                 warpPoints+i,
3124                                 CV_WARP_TO_CAMERA);
3125
3126             icvConvertWarpCoordinates(stereoparams->coeffs[1],
3127                                 camPoints2+i,
3128                                 warpPoints+i,
3129                                 CV_WARP_TO_CAMERA);
3130         }
3131     }
3132
3133
3134     /* Allocate memory for scanlines coeffs */
3135
3136     stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));
3137
3138     /* Compute coeffs for epilines  */
3139
3140     icvComputeCoeffForStereo( stereoparams);
3141
3142     /* all coeffs are known */
3143     return CV_OK;
3144 }
3145
3146 /*-------------------------------------------------------------------------------------------*/
3147
3148 int icvStereoCalibration( int numImages,
3149                             int* nums,
3150                             CvSize imageSize,
3151                             CvPoint2D32f* imagePoints1,
3152                             CvPoint2D32f* imagePoints2,
3153                             CvPoint3D32f* objectPoints,
3154                             CvStereoCamera* stereoparams
3155                            )
3156 {
3157     /* Firstly we must calibrate both cameras */
3158     /*  Alocate memory for data */
3159     /* Allocate for translate vectors */
3160     float* transVects1;
3161     float* transVects2;
3162     float* rotMatrs1;
3163     float* rotMatrs2;
3164
3165     transVects1 = (float*)calloc(numImages,sizeof(float)*3);
3166     transVects2 = (float*)calloc(numImages,sizeof(float)*3);
3167
3168     rotMatrs1   = (float*)calloc(numImages,sizeof(float)*9);
3169     rotMatrs2   = (float*)calloc(numImages,sizeof(float)*9);
3170
3171     /* Calibrate first camera */
3172     cvCalibrateCamera(  numImages,
3173                         nums,
3174                         imageSize,
3175                         imagePoints1,
3176                         objectPoints,
3177                         stereoparams->camera[0]->distortion,
3178                         stereoparams->camera[0]->matrix,
3179                         transVects1,
3180                         rotMatrs1,
3181                         1);
3182
3183     /* Calibrate second camera */
3184     cvCalibrateCamera(  numImages,
3185                         nums,
3186                         imageSize,
3187                         imagePoints2,
3188                         objectPoints,
3189                         stereoparams->camera[1]->distortion,
3190                         stereoparams->camera[1]->matrix,
3191                         transVects2,
3192                         rotMatrs2,
3193                         1);
3194
3195     /* Cameras are calibrated */
3196
3197     stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
3198     stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
3199
3200     stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
3201     stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
3202
3203     icvSelectBestRt(    numImages,
3204                         nums,
3205                         imagePoints1,
3206                         imagePoints2,
3207                         objectPoints,
3208                         stereoparams->camera[0]->matrix,
3209                         stereoparams->camera[0]->distortion,
3210                         rotMatrs1,
3211                         transVects1,
3212                         stereoparams->camera[1]->matrix,
3213                         stereoparams->camera[1]->distortion,
3214                         rotMatrs2,
3215                         transVects2,
3216                         stereoparams->rotMatrix,
3217                         stereoparams->transVector
3218                         );
3219
3220     /* Free memory */
3221     free(transVects1);
3222     free(transVects2);
3223     free(rotMatrs1);
3224     free(rotMatrs2);
3225
3226     icvComputeRestStereoParams(stereoparams);
3227
3228     return CV_NO_ERR;
3229 }
3230
3231 #if 0
3232 /* Find line from epipole */
3233 static void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
3234 {
3235     CvPoint2D32f frameBeg;
3236     CvPoint2D32f frameEnd;
3237     CvPoint2D32f cross[4];
3238     int     haveCross[4];
3239     float   dist;
3240
3241     haveCross[0] = 0;
3242     haveCross[1] = 0;
3243     haveCross[2] = 0;
3244     haveCross[3] = 0;
3245
3246     frameBeg.x = 0;
3247     frameBeg.y = 0;
3248     frameEnd.x = (float)(imageSize.width);
3249     frameEnd.y = 0;
3250     haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);
3251
3252     frameBeg.x = (float)(imageSize.width);
3253     frameBeg.y = 0;
3254     frameEnd.x = (float)(imageSize.width);
3255     frameEnd.y = (float)(imageSize.height);
3256     haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);
3257
3258     frameBeg.x = (float)(imageSize.width);
3259     frameBeg.y = (float)(imageSize.height);
3260     frameEnd.x = 0;
3261     frameEnd.y = (float)(imageSize.height);
3262     haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);
3263
3264     frameBeg.x = 0;
3265     frameBeg.y = (float)(imageSize.height);
3266     frameEnd.x = 0;
3267     frameEnd.y = 0;
3268     haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);
3269
3270     int n;
3271     float minDist = (float)(INT_MAX);
3272     float maxDist = (float)(INT_MIN);
3273
3274     int maxN = -1;
3275     int minN = -1;
3276
3277     for( n = 0; n < 4; n++ )
3278     {
3279         if( haveCross[n] > 0 )
3280         {
3281             dist =  (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
3282                     (epipole.y - cross[n].y)*(epipole.y - cross[n].y);
3283
3284             if( dist < minDist )
3285             {
3286                 minDist = dist;
3287                 minN = n;
3288             }
3289
3290             if( dist > maxDist )
3291             {
3292                 maxDist = dist;
3293                 maxN = n;
3294             }
3295         }
3296     }
3297
3298     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3299     {
3300         *start = cross[minN];
3301         *end   = cross[maxN];
3302     }
3303     else
3304     {
3305         start->x = 0;
3306         start->y = 0;
3307         end->x = 0;
3308         end->y = 0;
3309     }
3310
3311     return;
3312 }
3313
3314 /* Find line which cross frame by line(a,b,c) */
3315 static void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
3316 {
3317     CvPoint2D32f frameBeg;
3318     CvPoint2D32f frameEnd;
3319     CvPoint2D32f cross[4];
3320     int     haveCross[4];
3321     float   dist;
3322
3323     haveCross[0] = 0;
3324     haveCross[1] = 0;
3325     haveCross[2] = 0;
3326     haveCross[3] = 0;
3327
3328     frameBeg.x = 0;
3329     frameBeg.y = 0;
3330     frameEnd.x = (float)(imageSize.width);
3331     frameEnd.y = 0;
3332     haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
3333
3334     frameBeg.x = (float)(imageSize.width);
3335     frameBeg.y = 0;
3336     frameEnd.x = (float)(imageSize.width);
3337     frameEnd.y = (float)(imageSize.height);
3338     haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
3339
3340     frameBeg.x = (float)(imageSize.width);
3341     frameBeg.y = (float)(imageSize.height);
3342     frameEnd.x = 0;
3343     frameEnd.y = (float)(imageSize.height);
3344     haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
3345
3346     frameBeg.x = 0;
3347     frameBeg.y = (float)(imageSize.height);
3348     frameEnd.x = 0;
3349     frameEnd.y = 0;
3350     haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
3351
3352     int n;
3353     float minDist = (float)(INT_MAX);
3354     float maxDist = (float)(INT_MIN);
3355
3356     int maxN = -1;
3357     int minN = -1;
3358
3359     double midPointX = imageSize.width  / 2.0;
3360     double midPointY = imageSize.height / 2.0;
3361
3362     for( n = 0; n < 4; n++ )
3363     {
3364         if( haveCross[n] > 0 )
3365         {
3366             dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
3367                             (midPointY - cross[n].y)*(midPointY - cross[n].y));
3368
3369             if( dist < minDist )
3370             {
3371                 minDist = dist;
3372                 minN = n;
3373             }
3374
3375             if( dist > maxDist )
3376             {
3377                 maxDist = dist;
3378                 maxN = n;
3379             }
3380         }
3381     }
3382
3383     if( minN >= 0 && maxN >= 0 && (minN != maxN) )
3384     {
3385         *start = cross[minN];
3386         *end   = cross[maxN];
3387     }
3388     else
3389     {
3390         start->x = 0;
3391         start->y = 0;
3392         end->x = 0;
3393         end->y = 0;
3394     }
3395
3396     return;
3397
3398 }
3399
3400 /* Cross lines */
3401 static int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
3402 {
3403     double ex1,ey1,ex2,ey2;
3404     double px1,py1,px2,py2;
3405     double del;
3406     double delA,delB,delX,delY;
3407     double alpha,betta;
3408
3409     ex1 = p1_start.x;
3410     ey1 = p1_start.y;
3411     ex2 = p1_end.x;
3412     ey2 = p1_end.y;
3413
3414     px1 = p2_start.x;
3415     py1 = p2_start.y;
3416     px2 = p2_end.x;
3417     py2 = p2_end.y;
3418
3419     del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3420     if( del == 0)
3421     {
3422         return -1;
3423     }
3424
3425     delA =  (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3426     delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3427
3428     alpha =  delA / del;
3429     betta = -delB / del;
3430
3431     if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
3432     {
3433         return -1;
3434     }
3435
3436     delX =  (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3437             (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3438
3439     delY =  (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3440             (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3441
3442     cross->x = (float)( delX / del);
3443     cross->y = (float)(-delY / del);
3444     return 1;
3445 }
3446 #endif
3447
3448 int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
3449 {
3450     double ex1 = p1_start.x;
3451     double ey1 = p1_start.y;
3452     double ex2 = p1_end.x;
3453     double ey2 = p1_end.y;
3454
3455     double px1 = v2_start.x;
3456     double py1 = v2_start.y;
3457     double px2 = v2_end.x;
3458     double py2 = v2_end.y;
3459
3460     double del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
3461     if( del == 0)
3462     {
3463         return -1;
3464     }
3465
3466     double delA =  (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
3467     //double delB =  (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
3468
3469     double alpha =  delA / del;
3470     //double betta = -delB / del;
3471
3472     if( alpha < 0 || alpha > 1.0 )
3473     {
3474         return -1;
3475     }
3476
3477     double delX =  (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
3478             (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
3479
3480     double delY =  (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
3481             (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
3482
3483     cross->x = (float)( delX / del);
3484     cross->y = (float)(-delY / del);
3485     return 1;
3486 }
3487
3488
3489 int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
3490 {
3491     double del;
3492     double delX,delY,delA;
3493
3494     double px1,px2,py1,py2;
3495     double X,Y,alpha;
3496
3497     px1 = p1.x;
3498     py1 = p1.y;
3499
3500     px2 = p2.x;
3501     py2 = p2.y;
3502
3503     del = a * (px2 - px1) + b * (py2-py1);
3504     if( del == 0 )
3505     {
3506         return -1;
3507     }
3508
3509     delA = - c - a*px1 - b*py1;
3510     alpha = delA / del;
3511
3512     if( alpha < 0 || alpha > 1.0 )
3513     {
3514         return -1;/* no cross */
3515     }
3516
3517     delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
3518     delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);
3519
3520     X = delX / del;
3521     Y = delY / del;
3522
3523     cross->x = (float)X;
3524     cross->y = (float)Y;
3525
3526     return 1;
3527 }
3528
3529 #if 0
3530 static int cvComputeEpipoles( CvMatr32f camMatr1,  CvMatr32f camMatr2,
3531                             CvMatr32f rotMatr1,  CvMatr32f rotMatr2,
3532                             CvVect32f transVect1,CvVect32f transVect2,
3533                             CvVect32f epipole1,
3534                             CvVect32f epipole2)
3535 {
3536
3537     /* Copy matrix */
3538
3539     CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1);
3540     CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2);
3541     CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1);
3542     CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2);
3543     CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1);
3544     CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2);
3545     CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1);
3546     CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2);
3547
3548
3549     CvMat cmatrP1   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1);
3550     CvMat cmatrP2   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2);
3551     CvMat cvectp1   = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1);
3552     CvMat cvectp2   = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2);
3553     CvMat ctmpF1    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1);
3554     CvMat ctmpM1    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1);
3555     CvMat ctmpM2    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2);
3556     CvMat cinvP1    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1);
3557     CvMat cinvP2    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2);
3558     CvMat ctmpMatr  = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr);
3559     CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1);
3560     CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2);
3561     CvMat cmatrF1   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1);
3562     CvMat ctmpF     = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF);
3563     CvMat ctmpE1    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1);
3564     CvMat ctmpE2    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2);
3565
3566     /* Compute first */
3567     cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
3568     cvmInvert( &cmatrP1,&cinvP1 );
3569     cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );
3570
3571     /* Compute second */
3572     cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
3573     cvmInvert( &cmatrP2,&cinvP2 );
3574     cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );
3575
3576     cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
3577     cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
3578     cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);
3579
3580     cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
3581     cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
3582     cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);
3583
3584
3585     /* Need scale */
3586
3587     cvmScale(&ctmpE1,&cepipole1,1.0);
3588     cvmScale(&ctmpE2,&cepipole2,1.0);
3589
3590     cvmFree(&cmatrP1);
3591     cvmFree(&cmatrP1);
3592     cvmFree(&cvectp1);
3593     cvmFree(&cvectp2);
3594     cvmFree(&ctmpF1);
3595     cvmFree(&ctmpM1);
3596     cvmFree(&ctmpM2);
3597     cvmFree(&cinvP1);
3598     cvmFree(&cinvP2);
3599     cvmFree(&ctmpMatr);
3600     cvmFree(&ctmpVect1);
3601     cvmFree(&ctmpVect2);
3602     cvmFree(&cmatrF1);
3603     cvmFree(&ctmpF);
3604     cvmFree(&ctmpE1);
3605     cvmFree(&ctmpE2);
3606
3607     return CV_NO_ERR;
3608 }/* cvComputeEpipoles */
3609 #endif
3610
3611 /* Compute epipoles for fundamental matrix */
3612 int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
3613                                          CvPoint3D32f* epipole1,
3614                                          CvPoint3D32f* epipole2)
3615 {
3616     /* Decompose fundamental matrix using SVD ( A = U W V') */
3617     CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
3618
3619     CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
3620     CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
3621     CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);
3622
3623     /* From svd we need just last vector of U and V or last row from U' and V' */
3624     /* We get transposed matrices U and V */
3625     cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T);
3626
3627     /* Get last row from U' and compute epipole1 */
3628     epipole1->x = matrU->data.fl[6];
3629     epipole1->y = matrU->data.fl[7];
3630     epipole1->z = matrU->data.fl[8];
3631
3632     /* Get last row from V' and compute epipole2 */
3633     epipole2->x = matrV->data.fl[6];
3634     epipole2->y = matrV->data.fl[7];
3635     epipole2->z = matrV->data.fl[8];
3636
3637     cvReleaseMat(&matrW);
3638     cvReleaseMat(&matrU);
3639     cvReleaseMat(&matrV);
3640     return CV_OK;
3641 }
3642
3643 int cvConvertEssential2Fundamental( CvMatr32f essMatr,
3644                                          CvMatr32f fundMatr,
3645                                          CvMatr32f cameraMatr1,
3646                                          CvMatr32f cameraMatr2)
3647 {/* Fund = inv(CM1') * Ess * inv(CM2) */
3648
3649     CvMat essMatrC     = cvMat(3,3,CV_MAT32F,essMatr);
3650     CvMat fundMatrC    = cvMat(3,3,CV_MAT32F,fundMatr);
3651     CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1);
3652     CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2);
3653
3654     CvMat* invCM2  = cvCreateMat(3,3,CV_MAT32F);
3655     CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
3656     CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);
3657
3658     cvTranspose(&cameraMatr1C,tmpMatr);
3659     cvInvert(tmpMatr,invCM1T);
3660     cvmMul(invCM1T,&essMatrC,tmpMatr);
3661     cvInvert(&cameraMatr2C,invCM2);
3662     cvmMul(tmpMatr,invCM2,&fundMatrC);
3663
3664     /* Scale fundamental matrix */
3665     double scale;
3666     scale = 1.0/fundMatrC.data.fl[8];
3667     cvConvertScale(&fundMatrC,&fundMatrC,scale);
3668
3669     cvReleaseMat(&invCM2);
3670     cvReleaseMat(&tmpMatr);
3671     cvReleaseMat(&invCM1T);
3672
3673     return CV_OK;
3674 }
3675
3676 /* Compute essential matrix */
3677
3678 int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
3679                                     CvMatr32f transVect,
3680                                     CvMatr32f essMatr)
3681 {
3682     float transMatr[9];
3683
3684     /* Make antisymmetric matrix from transpose vector */
3685     transMatr[0] =   0;
3686     transMatr[1] = - transVect[2];
3687     transMatr[2] =   transVect[1];
3688
3689     transMatr[3] =   transVect[2];
3690     transMatr[4] =   0;
3691     transMatr[5] = - transVect[0];
3692
3693     transMatr[6] = - transVect[1];
3694     transMatr[7] =   transVect[0];
3695     transMatr[8] =   0;
3696
3697     icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);
3698
3699     return CV_OK;
3700 }