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    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 */
586int 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 */
597const 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 */
611const 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 */
624bool 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
650bool 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
703bool 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
761bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr )
762{
763    return Rectify( (CvMat**)srcarr, (CvMat**)dstarr );
764}
765
766bool 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
830bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr )
831{
832    return Undistort( (CvMat**)srcarr, (CvMat**)dstarr );
833}
834
835
836bool 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