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 for( i = 0; i < cameraCount; i++ ) 522 { 523 if( dstarr[i] && latestCounts[i] ) 524 { 525 CvMat dst_stub, *dst; 526 int count = 0; 527 bool found = false; 528 CvPoint2D32f* pts = 0; 529 530 GetLatestPoints( i, &pts, &count, &found ); 531 532 dst = cvGetMat( dstarr[i], &dst_stub ); 533 534 static const CvScalar line_colors[] = 535 { 536 {{0,0,255}}, 537 {{0,128,255}}, 538 {{0,200,200}}, 539 {{0,255,0}}, 540 {{200,200,0}}, 541 {{255,0,0}}, 542 {{255,0,255}} 543 }; 544 545 const int colorCount = sizeof(line_colors)/sizeof(line_colors[0]); 546 const int r = 4; 547 CvScalar color = line_colors[0]; 548 CvPoint prev_pt = { 0, 0}; 549 550 for( j = 0; j < count; j++ ) 551 { 552 CvPoint pt; 553 pt.x = cvRound(pts[j].x); 554 pt.y = cvRound(pts[j].y); 555 556 if( found ) 557 { 558 if( etalonType == CV_CALIB_ETALON_CHESSBOARD ) 559 color = line_colors[(j/cvRound(etalonParams[0]))%colorCount]; 560 else 561 color = CV_RGB(0,255,0); 562 563 if( j != 0 ) 564 cvLine( dst, prev_pt, pt, color, 1, CV_AA ); 565 } 566 567 cvLine( dst, cvPoint( pt.x - r, pt.y - r ), 568 cvPoint( pt.x + r, pt.y + r ), color, 1, CV_AA ); 569 570 cvLine( dst, cvPoint( pt.x - r, pt.y + r), 571 cvPoint( pt.x + r, pt.y - r), color, 1, CV_AA ); 572 573 cvCircle( dst, pt, r+1, color, 1, CV_AA ); 574 575 prev_pt = pt; 576 } 577 } 578 } 579 } 580 581 582 /* Get total number of frames and already accepted pair of frames */ 583 int CvCalibFilter::GetFrameCount( int* total ) const 584 { 585 if( total ) 586 *total = framesTotal; 587 588 return framesAccepted; 589 } 590 591 592 /* Get camera parameters for specified camera. If camera is not calibrated 593 the function returns 0 */ 594 const CvCamera* CvCalibFilter::GetCameraParams( int idx ) const 595 { 596 if( (unsigned)idx >= (unsigned)cameraCount ) 597 { 598 assert(0); 599 return 0; 600 } 601 602 return isCalibrated ? cameraParams + idx : 0; 603 } 604 605 606 /* Get camera parameters for specified camera. If camera is not calibrated 607 the function returns 0 */ 608 const CvStereoCamera* CvCalibFilter::GetStereoParams() const 609 { 610 if( !(isCalibrated && cameraCount == 2) ) 611 { 612 assert(0); 613 return 0; 614 } 615 616 return &stereo; 617 } 618 619 620 /* Sets camera parameters for all cameras */ 621 bool CvCalibFilter::SetCameraParams( CvCamera* params ) 622 { 623 CvMat mat; 624 int arrSize; 625 626 Stop(); 627 628 if( !params ) 629 { 630 assert(0); 631 return false; 632 } 633 634 arrSize = cameraCount * sizeof(params[0]); 635 636 cvInitMatHeader( &mat, 1, cameraCount * (arrSize/sizeof(float)), 637 CV_32FC1, params ); 638 cvCheckArr( &mat, CV_CHECK_RANGE, -10000, 10000 ); 639 640 memcpy( cameraParams, params, arrSize ); 641 isCalibrated = true; 642 643 return true; 644 } 645 646 647 bool CvCalibFilter::SaveCameraParams( const char* filename ) 648 { 649 if( isCalibrated ) 650 { 651 int i, j; 652 653 FILE* f = fopen( filename, "w" ); 654 655 if( !f ) return false; 656 657 fprintf( f, "%d\n\n", cameraCount ); 658 659 for( i = 0; i < cameraCount; i++ ) 660 { 661 for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ ) 662 { 663 fprintf( f, "%15.10f ", ((float*)(cameraParams + i))[j] ); 664 } 665 fprintf( f, "\n\n" ); 666 } 667 668 /* Save stereo params */ 669 670 /* Save quad */ 671 for( i = 0; i < 2; i++ ) 672 { 673 for( j = 0; j < 4; j++ ) 674 { 675 fprintf(f, "%15.10f ", stereo.quad[i][j].x ); 676 fprintf(f, "%15.10f ", stereo.quad[i][j].y ); 677 } 678 fprintf(f, "\n"); 679 } 680 681 /* Save coeffs */ 682 for( i = 0; i < 2; i++ ) 683 { 684 for( j = 0; j < 9; j++ ) 685 { 686 fprintf(f, "%15.10lf ", stereo.coeffs[i][j/3][j%3] ); 687 } 688 fprintf(f, "\n"); 689 } 690 691 692 fclose(f); 693 return true; 694 } 695 696 return true; 697 } 698 699 700 bool CvCalibFilter::LoadCameraParams( const char* filename ) 701 { 702 int i, j; 703 int d = 0; 704 FILE* f = fopen( filename, "r" ); 705 706 isCalibrated = false; 707 708 if( !f ) return false; 709 710 if( fscanf( f, "%d", &d ) != 1 || d <= 0 || d > 10 ) 711 return false; 712 713 SetCameraCount( d ); 714 715 for( i = 0; i < cameraCount; i++ ) 716 { 717 for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ ) 718 { 719 fscanf( f, "%f", &((float*)(cameraParams + i))[j] ); 720 } 721 } 722 723 724 /* Load stereo params */ 725 726 /* load quad */ 727 for( i = 0; i < 2; i++ ) 728 { 729 for( j = 0; j < 4; j++ ) 730 { 731 fscanf(f, "%f ", &(stereo.quad[i][j].x) ); 732 fscanf(f, "%f ", &(stereo.quad[i][j].y) ); 733 } 734 } 735 736 /* Load coeffs */ 737 for( i = 0; i < 2; i++ ) 738 { 739 for( j = 0; j < 9; j++ ) 740 { 741 fscanf(f, "%lf ", &(stereo.coeffs[i][j/3][j%3]) ); 742 } 743 } 744 745 746 747 748 fclose(f); 749 750 stereo.warpSize = cvSize( cvRound(cameraParams[0].imgSize[0]), cvRound(cameraParams[0].imgSize[1])); 751 752 isCalibrated = true; 753 754 return true; 755 } 756 757 758 bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr ) 759 { 760 return Rectify( (CvMat**)srcarr, (CvMat**)dstarr ); 761 } 762 763 bool CvCalibFilter::Rectify( CvMat** srcarr, CvMat** dstarr ) 764 { 765 int i; 766 767 if( !srcarr || !dstarr ) 768 { 769 assert(0); 770 return false; 771 } 772 773 if( isCalibrated && cameraCount == 2 ) 774 { 775 for( i = 0; i < cameraCount; i++ ) 776 { 777 if( srcarr[i] && dstarr[i] ) 778 { 779 IplImage src_stub, *src; 780 IplImage dst_stub, *dst; 781 782 src = cvGetImage( srcarr[i], &src_stub ); 783 dst = cvGetImage( dstarr[i], &dst_stub ); 784 785 if( src->imageData == dst->imageData ) 786 { 787 if( !undistImg || 788 undistImg->width != src->width || 789 undistImg->height != src->height || 790 CV_MAT_CN(undistImg->type) != src->nChannels ) 791 { 792 cvReleaseMat( &undistImg ); 793 undistImg = cvCreateMat( src->height, src->width, 794 CV_8U + (src->nChannels-1)*8 ); 795 } 796 cvCopy( src, undistImg ); 797 src = cvGetImage( undistImg, &src_stub ); 798 } 799 800 cvZero( dst ); 801 802 if( !rectMap[i][0] || rectMap[i][0]->width != src->width || 803 rectMap[i][0]->height != src->height ) 804 { 805 cvReleaseMat( &rectMap[i][0] ); 806 cvReleaseMat( &rectMap[i][1] ); 807 rectMap[i][0] = cvCreateMat(stereo.warpSize.height,stereo.warpSize.width,CV_32FC1); 808 rectMap[i][1] = cvCreateMat(stereo.warpSize.height,stereo.warpSize.width,CV_32FC1); 809 cvComputePerspectiveMap(stereo.coeffs[i], rectMap[i][0], rectMap[i][1]); 810 } 811 cvRemap( src, dst, rectMap[i][0], rectMap[i][1] ); 812 } 813 } 814 } 815 else 816 { 817 for( i = 0; i < cameraCount; i++ ) 818 { 819 if( srcarr[i] != dstarr[i] ) 820 cvCopy( srcarr[i], dstarr[i] ); 821 } 822 } 823 824 return true; 825 } 826 827 bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr ) 828 { 829 return Undistort( (CvMat**)srcarr, (CvMat**)dstarr ); 830 } 831 832 833 bool CvCalibFilter::Undistort( CvMat** srcarr, CvMat** dstarr ) 834 { 835 int i; 836 837 if( !srcarr || !dstarr ) 838 { 839 assert(0); 840 return false; 841 } 842 843 if( isCalibrated ) 844 { 845 for( i = 0; i < cameraCount; i++ ) 846 { 847 if( srcarr[i] && dstarr[i] ) 848 { 849 CvMat src_stub, *src; 850 CvMat dst_stub, *dst; 851 852 src = cvGetMat( srcarr[i], &src_stub ); 853 dst = cvGetMat( dstarr[i], &dst_stub ); 854 855 if( src->data.ptr == dst->data.ptr ) 856 { 857 if( !undistImg || undistImg->width != src->width || 858 undistImg->height != src->height || 859 CV_ARE_TYPES_EQ( undistImg, src )) 860 { 861 cvReleaseMat( &undistImg ); 862 undistImg = cvCreateMat( src->height, src->width, src->type ); 863 } 864 865 cvCopy( src, undistImg ); 866 src = undistImg; 867 } 868 869 #if 1 870 { 871 CvMat A = cvMat( 3, 3, CV_32FC1, cameraParams[i].matrix ); 872 CvMat k = cvMat( 1, 4, CV_32FC1, cameraParams[i].distortion ); 873 874 if( !undistMap[i][0] || undistMap[i][0]->width != src->width || 875 undistMap[i][0]->height != src->height ) 876 { 877 cvReleaseMat( &undistMap[i][0] ); 878 cvReleaseMat( &undistMap[i][1] ); 879 undistMap[i][0] = cvCreateMat( src->height, src->width, CV_32FC1 ); 880 undistMap[i][1] = cvCreateMat( src->height, src->width, CV_32FC1 ); 881 cvInitUndistortMap( &A, &k, undistMap[i][0], undistMap[i][1] ); 882 } 883 884 cvRemap( src, dst, undistMap[i][0], undistMap[i][1] ); 885 #else 886 cvUndistort2( src, dst, &A, &k ); 887 #endif 888 } 889 } 890 } 891 } 892 else 893 { 894 for( i = 0; i < cameraCount; i++ ) 895 { 896 if( srcarr[i] != dstarr[i] ) 897 cvCopy( srcarr[i], dstarr[i] ); 898 } 899 } 900 901 902 return true; 903 } 904
