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