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