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 #include "_cvaux.h" 43 #include <stdio.h> 44 45 #undef quad 46 47 #if _MSC_VER >= 1200 48 #pragma warning( disable: 4701 ) 49 #endif 50 51 CvCalibFilter::CvCalibFilter() 52 { 53 /* etalon data */ 54 etalonType = CV_CALIB_ETALON_USER; 55 etalonParamCount = 0; 56 etalonParams = 0; 57 etalonPointCount = 0; 58 etalonPoints = 0; 59 60 /* camera data */ 61 cameraCount = 1; 62 63 memset( points, 0, sizeof(points)); 64 memset( undistMap, 0, sizeof(undistMap)); 65 undistImg = 0; 66 memset( latestCounts, 0, sizeof(latestCounts)); 67 memset( latestPoints, 0, sizeof(latestPoints)); 68 memset( &stereo, 0, sizeof(stereo) ); 69 maxPoints = 0; 70 framesTotal = 15; 71 framesAccepted = 0; 72 isCalibrated = false; 73 74 imgSize = cvSize(0,0); 75 grayImg = 0; 76 tempImg = 0; 77 storage = 0; 78 79 memset( rectMap, 0, sizeof(rectMap)); 80 } 81 82 83 CvCalibFilter::~CvCalibFilter() 84 { 85 SetCameraCount(0); 86 cvFree( &etalonParams ); 87 cvFree( &etalonPoints ); 88 cvReleaseMat( &grayImg ); 89 cvReleaseMat( &tempImg ); 90 cvReleaseMat( &undistImg ); 91 cvReleaseMemStorage( &storage ); 92 } 93 94 95 bool CvCalibFilter::SetEtalon( CvCalibEtalonType type, double* params, 96 int pointCount, CvPoint2D32f* points ) 97 { 98 int i, arrSize; 99 100 Stop(); 101 102 for( i = 0; i < MAX_CAMERAS; i++ ) 103 cvFree( latestPoints + i ); 104 105 if( type == CV_CALIB_ETALON_USER || type != etalonType ) 106 { 107 cvFree( &etalonParams ); 108 } 109 110 etalonType = type; 111 112 switch( etalonType ) 113 { 114 case CV_CALIB_ETALON_CHESSBOARD: 115 etalonParamCount = 3; 116 if( !params || cvRound(params[0]) != params[0] || params[0] < 3 || 117 cvRound(params[1]) != params[1] || params[1] < 3 || params[2] <= 0 ) 118 { 119 assert(0); 120 return false; 121 } 122 123 pointCount = cvRound((params[0] - 1)*(params[1] - 1)); 124 break; 125 126 case CV_CALIB_ETALON_USER: 127 etalonParamCount = 0; 128 129 if( !points || pointCount < 4 ) 130 { 131 assert(0); 132 return false; 133 } 134 break; 135 136 default: 137 assert(0); 138 return false; 139 } 140 141 if( etalonParamCount > 0 ) 142 { 143 arrSize = etalonParamCount * sizeof(etalonParams[0]); 144 etalonParams = (double*)cvAlloc( arrSize ); 145 } 146 147 arrSize = pointCount * sizeof(etalonPoints[0]); 148 149 if( etalonPointCount != pointCount ) 150 { 151 cvFree( &etalonPoints ); 152 etalonPointCount = pointCount; 153 etalonPoints = (CvPoint2D32f*)cvAlloc( arrSize ); 154 } 155 156 switch( etalonType ) 157 { 158 case CV_CALIB_ETALON_CHESSBOARD: 159 { 160 int etalonWidth = cvRound( params[0] ) - 1; 161 int etalonHeight = cvRound( params[1] ) - 1; 162 int x, y, k = 0; 163 164 etalonParams[0] = etalonWidth; 165 etalonParams[1] = etalonHeight; 166 etalonParams[2] = params[2]; 167 168 for( y = 0; y < etalonHeight; y++ ) 169 for( x = 0; x < etalonWidth; x++ ) 170 { 171 etalonPoints[k++] = cvPoint2D32f( (etalonWidth - 1 - x)*params[2], 172 y*params[2] ); 173 } 174 } 175 break; 176 177 case CV_CALIB_ETALON_USER: 178 memcpy( etalonParams, params, arrSize ); 179 memcpy( etalonPoints, points, arrSize ); 180 break; 181 182 default: 183 assert(0); 184 return false; 185 } 186 187 return true; 188 } 189 190 191 CvCalibEtalonType 192 CvCalibFilter::GetEtalon( int* paramCount, const double** params, 193 int* pointCount, const CvPoint2D32f** points ) const 194 { 195 if( paramCount ) 196 *paramCount = etalonParamCount; 197 198 if( params ) 199 *params = etalonParams; 200 201 if( pointCount ) 202 *pointCount = etalonPointCount; 203 204 if( points ) 205 *points = etalonPoints; 206 207 return etalonType; 208 } 209 210 211 void CvCalibFilter::SetCameraCount( int count ) 212 { 213 Stop(); 214 215 if( count != cameraCount ) 216 { 217 for( int i = 0; i < cameraCount; i++ ) 218 { 219 cvFree( points + i ); 220 cvFree( latestPoints + i ); 221 cvReleaseMat( &undistMap[i][0] ); 222 cvReleaseMat( &undistMap[i][1] ); 223 cvReleaseMat( &rectMap[i][0] ); 224 cvReleaseMat( &rectMap[i][1] ); 225 } 226 227 memset( latestCounts, 0, sizeof(latestPoints) ); 228 maxPoints = 0; 229 cameraCount = count; 230 } 231 } 232 233 234 bool CvCalibFilter::SetFrames( int frames ) 235 { 236 if( frames < 5 ) 237 { 238 assert(0); 239 return false; 240 } 241 242 framesTotal = frames; 243 return true; 244 } 245 246 247 void CvCalibFilter::Stop( bool calibrate ) 248 { 249 int i, j; 250 isCalibrated = false; 251 252 // deallocate undistortion maps 253 for( i = 0; i < cameraCount; i++ ) 254 { 255 cvReleaseMat( &undistMap[i][0] ); 256 cvReleaseMat( &undistMap[i][1] ); 257 cvReleaseMat( &rectMap[i][0] ); 258 cvReleaseMat( &rectMap[i][1] ); 259 } 260 261 if( calibrate && framesAccepted > 0 ) 262 { 263 int n = framesAccepted; 264 CvPoint3D32f* buffer = 265 (CvPoint3D32f*)cvAlloc( n * etalonPointCount * sizeof(buffer[0])); 266 CvMat mat; 267 float* rotMatr = (float*)cvAlloc( n * 9 * sizeof(rotMatr[0])); 268 float* transVect = (float*)cvAlloc( n * 3 * sizeof(transVect[0])); 269 int* counts = (int*)cvAlloc( n * sizeof(counts[0])); 270 271 cvInitMatHeader( &mat, 1, sizeof(CvCamera)/sizeof(float), CV_32FC1, 0 ); 272 memset( cameraParams, 0, cameraCount * sizeof(cameraParams[0])); 273 274 for( i = 0; i < framesAccepted; i++ ) 275 { 276 counts[i] = etalonPointCount; 277 for( j = 0; j < etalonPointCount; j++ ) 278 buffer[i * etalonPointCount + j] = cvPoint3D32f( etalonPoints[j].x, 279 etalonPoints[j].y, 0 ); 280 } 281 282 for( i = 0; i < cameraCount; i++ ) 283 { 284 cvCalibrateCamera( framesAccepted, counts, 285 imgSize, points[i], buffer, 286 cameraParams[i].distortion, 287 cameraParams[i].matrix, 288 transVect, rotMatr, 0 ); 289 290 cameraParams[i].imgSize[0] = (float)imgSize.width; 291 cameraParams[i].imgSize[1] = (float)imgSize.height; 292 293 // cameraParams[i].focalLength[0] = cameraParams[i].matrix[0]; 294 // cameraParams[i].focalLength[1] = cameraParams[i].matrix[4]; 295 296 // cameraParams[i].principalPoint[0] = cameraParams[i].matrix[2]; 297 // cameraParams[i].principalPoint[1] = cameraParams[i].matrix[5]; 298 299 memcpy( cameraParams[i].rotMatr, rotMatr, 9 * sizeof(rotMatr[0])); 300 memcpy( cameraParams[i].transVect, transVect, 3 * sizeof(transVect[0])); 301 302 mat.data.ptr = (uchar*)(cameraParams + i); 303 304 /* check resultant camera parameters: if there are some INF's or NAN's, 305 stop and reset results */ 306 if( !cvCheckArr( &mat, CV_CHECK_RANGE | CV_CHECK_QUIET, -10000, 10000 )) 307 break; 308 } 309 310 311 312 isCalibrated = i == cameraCount; 313 314 {/* calibrate stereo cameras */ 315 if( cameraCount == 2 ) 316 { 317 stereo.camera[0] = &cameraParams[0]; 318 stereo.camera[1] = &cameraParams[1]; 319 320 icvStereoCalibration( framesAccepted, counts, 321 imgSize, 322 points[0],points[1], 323 buffer, 324 &stereo); 325 326 for( i = 0; i < 9; i++ ) 327 { 328 stereo.fundMatr[i] = stereo.fundMatr[i]; 329 } 330 331 } 332 333 } 334 335 cvFree( &buffer ); 336 cvFree( &counts ); 337 cvFree( &rotMatr ); 338 cvFree( &transVect ); 339 } 340 341 framesAccepted = 0; 342 } 343 344 345 bool CvCalibFilter::FindEtalon( IplImage** imgs ) 346 { 347 return FindEtalon( (CvMat**)imgs ); 348 } 349 350 351 bool CvCalibFilter::FindEtalon( CvMat** mats ) 352 { 353 bool result = true; 354 355 if( !mats || etalonPointCount == 0 ) 356 { 357 assert(0); 358 result = false; 359 } 360 361 if( result ) 362 { 363 int i, tempPointCount0 = etalonPointCount*2; 364 365 for( i = 0; i < cameraCount; i++ ) 366 { 367 if( !latestPoints[i] ) 368 latestPoints[i] = (CvPoint2D32f*) 369 cvAlloc( tempPointCount0*2*sizeof(latestPoints[0])); 370 } 371 372 for( i = 0; i < cameraCount; i++ ) 373 { 374 CvSize size; 375 int tempPointCount = tempPointCount0; 376 bool found = false; 377 378 if( !CV_IS_MAT(mats[i]) && !CV_IS_IMAGE(mats[i])) 379 { 380 assert(0); 381 break; 382 } 383 384 size = cvGetSize(mats[i]); 385 386 if( size.width != imgSize.width || size.height != imgSize.height ) 387 { 388 imgSize = size; 389 } 390 391 if( !grayImg || grayImg->width != imgSize.width || 392 grayImg->height != imgSize.height ) 393 { 394 cvReleaseMat( &grayImg ); 395 cvReleaseMat( &tempImg ); 396 grayImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 ); 397 tempImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 ); 398 } 399 400 if( !storage ) 401 storage = cvCreateMemStorage(); 402 403 switch( etalonType ) 404 { 405 case CV_CALIB_ETALON_CHESSBOARD: 406 if( CV_MAT_CN(cvGetElemType(mats[i])) == 1 ) 407 cvCopy( mats[i], grayImg ); 408 else 409 cvCvtColor( mats[i], grayImg, CV_BGR2GRAY ); 410 found = cvFindChessBoardCornerGuesses( grayImg, tempImg, storage, 411 cvSize( cvRound(etalonParams[0]), 412 cvRound(etalonParams[1])), 413 latestPoints[i], &tempPointCount ) != 0; 414 if( found ) 415 cvFindCornerSubPix( grayImg, latestPoints[i], tempPointCount, 416 cvSize(5,5), cvSize(-1,-1), 417 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,10,0.1)); 418 break; 419 default: 420 assert(0); 421 result = false; 422 break; 423 } 424 425 latestCounts[i] = found ? tempPointCount : -tempPointCount; 426 result = result && found; 427 } 428 } 429 430 if( storage ) 431 cvClearMemStorage( storage ); 432 433 return result; 434 } 435 436 437 bool CvCalibFilter::Push( const CvPoint2D32f** pts ) 438 { 439 bool result = true; 440 int i, newMaxPoints = etalonPointCount*(MAX(framesAccepted,framesTotal) + 1); 441 442 isCalibrated = false; 443 444 if( !pts ) 445 { 446 for( i = 0; i < cameraCount; i++ ) 447 if( latestCounts[i] <= 0 ) 448 return false; 449 pts = (const CvPoint2D32f**)latestPoints; 450 } 451 452 for( i = 0; i < cameraCount; i++ ) 453 { 454 if( !pts[i] ) 455 { 456 assert(0); 457 break; 458 } 459 460 if( maxPoints < newMaxPoints ) 461 { 462 CvPoint2D32f* prev = points[i]; 463 cvFree( points + i ); 464 points[i] = (CvPoint2D32f*)cvAlloc( newMaxPoints * sizeof(prev[0])); 465 memcpy( points[i], prev, maxPoints * sizeof(prev[0])); 466 } 467 468 memcpy( points[i] + framesAccepted*etalonPointCount, pts[i], 469 etalonPointCount*sizeof(points[0][0])); 470 } 471 472 if( maxPoints < newMaxPoints ) 473 maxPoints = newMaxPoints; 474 475 result = i == cameraCount; 476 477 if( ++framesAccepted >= framesTotal ) 478 Stop( true ); 479 return result; 480 } 481 482 483 bool CvCalibFilter::GetLatestPoints( int idx, CvPoint2D32f** pts, 484 int* count, bool* found ) 485 { 486 int n; 487 488 if( (unsigned)idx >= (unsigned)cameraCount || 489 !pts || !count || !found ) 490 { 491 assert(0); 492 return false; 493 } 494 495 n = latestCounts[idx]; 496 497 *found = n > 0; 498 *count = abs(n); 499 *pts = latestPoints[idx]; 500 501 return true; 502 } 503 504 505 void CvCalibFilter::DrawPoints( IplImage** dst ) 506 { 507 DrawPoints( (CvMat**)dst ); 508 } 509 510 511 void CvCalibFilter::DrawPoints( CvMat** dstarr ) 512 { 513 int i, j; 514 515 if( !dstarr ) 516 { 517 assert(0); 518 return; 519 } 520 521 if( latestCounts ) 522 { 523 for( i = 0; i < cameraCount; i++ ) 524 { 525 if( dstarr[i] && latestCounts[i] ) 526 { 527 CvMat dst_stub, *dst; 528 int count = 0; 529 bool found = false; 530 CvPoint2D32f* pts = 0; 531 532 GetLatestPoints( i, &pts, &count, &found ); 533 534 dst = cvGetMat( dstarr[i], &dst_stub ); 535 536 static const CvScalar line_colors[] = 537 { 538 {{0,0,255}}, 539 {{0,128,255}}, 540 {{0,200,200}}, 541 {{0,255,0}}, 542 {{200,200,0}}, 543 {{255,0,0}}, 544 {{255,0,255}} 545 }; 546 547 const int colorCount = sizeof(line_colors)/sizeof(line_colors[0]); 548 const int r = 4; 549 CvScalar color = line_colors[0]; 550 CvPoint prev_pt = { 0, 0}; 551 552 for( j = 0; j < count; j++ ) 553 { 554 CvPoint pt; 555 pt.x = cvRound(pts[j].x); 556 pt.y = cvRound(pts[j].y); 557 558 if( found ) 559 { 560 if( etalonType == CV_CALIB_ETALON_CHESSBOARD ) 561 color = line_colors[(j/cvRound(etalonParams[0]))%colorCount]; 562 else 563 color = CV_RGB(0,255,0); 564 565 if( j != 0 ) 566 cvLine( dst, prev_pt, pt, color, 1, CV_AA ); 567 } 568 569 cvLine( dst, cvPoint( pt.x - r, pt.y - r ), 570 cvPoint( pt.x + r, pt.y + r ), color, 1, CV_AA ); 571 572 cvLine( dst, cvPoint( pt.x - r, pt.y + r), 573 cvPoint( pt.x + r, pt.y - r), color, 1, CV_AA ); 574 575 cvCircle( dst, pt, r+1, color, 1, CV_AA ); 576 577 prev_pt = pt; 578 } 579 } 580 } 581 } 582 } 583 584 585 /* Get total number of frames and already accepted pair of frames */ 586 int CvCalibFilter::GetFrameCount( int* total ) const 587 { 588 if( total ) 589 *total = framesTotal; 590 591 return framesAccepted; 592 } 593 594 595 /* Get camera parameters for specified camera. If camera is not calibrated 596 the function returns 0 */ 597 const CvCamera* CvCalibFilter::GetCameraParams( int idx ) const 598 { 599 if( (unsigned)idx >= (unsigned)cameraCount ) 600 { 601 assert(0); 602 return 0; 603 } 604 605 return isCalibrated ? cameraParams + idx : 0; 606 } 607 608 609 /* Get camera parameters for specified camera. If camera is not calibrated 610 the function returns 0 */ 611 const CvStereoCamera* CvCalibFilter::GetStereoParams() const 612 { 613 if( !(isCalibrated && cameraCount == 2) ) 614 { 615 assert(0); 616 return 0; 617 } 618 619 return &stereo; 620 } 621 622 623 /* Sets camera parameters for all cameras */ 624 bool CvCalibFilter::SetCameraParams( CvCamera* params ) 625 { 626 CvMat mat; 627 int arrSize; 628 629 Stop(); 630 631 if( !params ) 632 { 633 assert(0); 634 return false; 635 } 636 637 arrSize = cameraCount * sizeof(params[0]); 638 639 cvInitMatHeader( &mat, 1, cameraCount * (arrSize/sizeof(float)), 640 CV_32FC1, params ); 641 cvCheckArr( &mat, CV_CHECK_RANGE, -10000, 10000 ); 642 643 memcpy( cameraParams, params, arrSize ); 644 isCalibrated = true; 645 646 return true; 647 } 648 649 650 bool CvCalibFilter::SaveCameraParams( const char* filename ) 651 { 652 if( isCalibrated ) 653 { 654 int i, j; 655 656 FILE* f = fopen( filename, "w" ); 657 658 if( !f ) return false; 659 660 fprintf( f, "%d\n\n", cameraCount ); 661 662 for( i = 0; i < cameraCount; i++ ) 663 { 664 for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ ) 665 { 666 fprintf( f, "%15.10f ", ((float*)(cameraParams + i))[j] ); 667 } 668 fprintf( f, "\n\n" ); 669 } 670 671 /* Save stereo params */ 672 673 /* Save quad */ 674 for( i = 0; i < 2; i++ ) 675 { 676 for( j = 0; j < 4; j++ ) 677 { 678 fprintf(f, "%15.10f ", stereo.quad[i][j].x ); 679 fprintf(f, "%15.10f ", stereo.quad[i][j].y ); 680 } 681 fprintf(f, "\n"); 682 } 683 684 /* Save coeffs */ 685 for( i = 0; i < 2; i++ ) 686 { 687 for( j = 0; j < 9; j++ ) 688 { 689 fprintf(f, "%15.10lf ", stereo.coeffs[i][j/3][j%3] ); 690 } 691 fprintf(f, "\n"); 692 } 693 694 695 fclose(f); 696 return true; 697 } 698 699 return true; 700 } 701 702 703 bool CvCalibFilter::LoadCameraParams( const char* filename ) 704 { 705 int i, j; 706 int d = 0; 707 FILE* f = fopen( filename, "r" ); 708 709 isCalibrated = false; 710 711 if( !f ) return false; 712 713 if( fscanf( f, "%d", &d ) != 1 || d <= 0 || d > 10 ) 714 return false; 715 716 SetCameraCount( d ); 717 718 for( i = 0; i < cameraCount; i++ ) 719 { 720 for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ ) 721 { 722 fscanf( f, "%f", &((float*)(cameraParams + i))[j] ); 723 } 724 } 725 726 727 /* Load stereo params */ 728 729 /* load quad */ 730 for( i = 0; i < 2; i++ ) 731 { 732 for( j = 0; j < 4; j++ ) 733 { 734 fscanf(f, "%f ", &(stereo.quad[i][j].x) ); 735 fscanf(f, "%f ", &(stereo.quad[i][j].y) ); 736 } 737 } 738 739 /* Load coeffs */ 740 for( i = 0; i < 2; i++ ) 741 { 742 for( j = 0; j < 9; j++ ) 743 { 744 fscanf(f, "%lf ", &(stereo.coeffs[i][j/3][j%3]) ); 745 } 746 } 747 748 749 750 751 fclose(f); 752 753 stereo.warpSize = cvSize( cvRound(cameraParams[0].imgSize[0]), cvRound(cameraParams[0].imgSize[1])); 754 755 isCalibrated = true; 756 757 return true; 758 } 759 760 761 bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr ) 762 { 763 return Rectify( (CvMat**)srcarr, (CvMat**)dstarr ); 764 } 765 766 bool CvCalibFilter::Rectify( CvMat** srcarr, CvMat** dstarr ) 767 { 768 int i; 769 770 if( !srcarr || !dstarr ) 771 { 772 assert(0); 773 return false; 774 } 775 776 if( isCalibrated && cameraCount == 2 ) 777 { 778 for( i = 0; i < cameraCount; i++ ) 779 { 780 if( srcarr[i] && dstarr[i] ) 781 { 782 IplImage src_stub, *src; 783 IplImage dst_stub, *dst; 784 785 src = cvGetImage( srcarr[i], &src_stub ); 786 dst = cvGetImage( dstarr[i], &dst_stub ); 787 788 if( src->imageData == dst->imageData ) 789 { 790 if( !undistImg || 791 undistImg->width != src->width || 792 undistImg->height != src->height || 793 CV_MAT_CN(undistImg->type) != src->nChannels ) 794 { 795 cvReleaseMat( &undistImg ); 796 undistImg = cvCreateMat( src->height, src->width, 797 CV_8U + (src->nChannels-1)*8 ); 798 } 799 cvCopy( src, undistImg ); 800 src = cvGetImage( undistImg, &src_stub ); 801 } 802 803 cvZero( dst ); 804 805 if( !rectMap[i][0] || rectMap[i][0]->width != src->width || 806 rectMap[i][0]->height != src->height ) 807 { 808 cvReleaseMat( &rectMap[i][0] ); 809 cvReleaseMat( &rectMap[i][1] ); 810 rectMap[i][0] = cvCreateMat(stereo.warpSize.height,stereo.warpSize.width,CV_32FC1); 811 rectMap[i][1] = cvCreateMat(stereo.warpSize.height,stereo.warpSize.width,CV_32FC1); 812 cvComputePerspectiveMap(stereo.coeffs[i], rectMap[i][0], rectMap[i][1]); 813 } 814 cvRemap( src, dst, rectMap[i][0], rectMap[i][1] ); 815 } 816 } 817 } 818 else 819 { 820 for( i = 0; i < cameraCount; i++ ) 821 { 822 if( srcarr[i] != dstarr[i] ) 823 cvCopy( srcarr[i], dstarr[i] ); 824 } 825 } 826 827 return true; 828 } 829 830 bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr ) 831 { 832 return Undistort( (CvMat**)srcarr, (CvMat**)dstarr ); 833 } 834 835 836 bool CvCalibFilter::Undistort( CvMat** srcarr, CvMat** dstarr ) 837 { 838 int i; 839 840 if( !srcarr || !dstarr ) 841 { 842 assert(0); 843 return false; 844 } 845 846 if( isCalibrated ) 847 { 848 for( i = 0; i < cameraCount; i++ ) 849 { 850 if( srcarr[i] && dstarr[i] ) 851 { 852 CvMat src_stub, *src; 853 CvMat dst_stub, *dst; 854 855 src = cvGetMat( srcarr[i], &src_stub ); 856 dst = cvGetMat( dstarr[i], &dst_stub ); 857 858 if( src->data.ptr == dst->data.ptr ) 859 { 860 if( !undistImg || undistImg->width != src->width || 861 undistImg->height != src->height || 862 CV_ARE_TYPES_EQ( undistImg, src )) 863 { 864 cvReleaseMat( &undistImg ); 865 undistImg = cvCreateMat( src->height, src->width, src->type ); 866 } 867 868 cvCopy( src, undistImg ); 869 src = undistImg; 870 } 871 872 #if 1 873 { 874 CvMat A = cvMat( 3, 3, CV_32FC1, cameraParams[i].matrix ); 875 CvMat k = cvMat( 1, 4, CV_32FC1, cameraParams[i].distortion ); 876 877 if( !undistMap[i][0] || undistMap[i][0]->width != src->width || 878 undistMap[i][0]->height != src->height ) 879 { 880 cvReleaseMat( &undistMap[i][0] ); 881 cvReleaseMat( &undistMap[i][1] ); 882 undistMap[i][0] = cvCreateMat( src->height, src->width, CV_32FC1 ); 883 undistMap[i][1] = cvCreateMat( src->height, src->width, CV_32FC1 ); 884 cvInitUndistortMap( &A, &k, undistMap[i][0], undistMap[i][1] ); 885 } 886 887 cvRemap( src, dst, undistMap[i][0], undistMap[i][1] ); 888 #else 889 cvUndistort2( src, dst, &A, &k ); 890 #endif 891 } 892 } 893 } 894 } 895 else 896 { 897 for( i = 0; i < cameraCount; i++ ) 898 { 899 if( srcarr[i] != dstarr[i] ) 900 cvCopy( srcarr[i], dstarr[i] ); 901 } 902 } 903 904 905 return true; 906 } 907
