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