vcs.maemo.org Git - opencv/blob - src/cvaux/cvcalibfilter.cpp

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