git.maemo.org Git - opencv/blob - cvaux/src/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     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 }