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