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
CvCalibFilter()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
~CvCalibFilter()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
SetEtalon(CvCalibEtalonType type,double * params,int pointCount,CvPoint2D32f * points)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
GetEtalon(int * paramCount,const double ** params,int * pointCount,const CvPoint2D32f ** points) const192 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
SetCameraCount(int count)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
SetFrames(int frames)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
Stop(bool calibrate)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
FindEtalon(IplImage ** imgs)345 bool CvCalibFilter::FindEtalon( IplImage** imgs )
346 {
347 return FindEtalon( (CvMat**)imgs );
348 }
349
350
FindEtalon(CvMat ** mats)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
Push(const CvPoint2D32f ** pts)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
GetLatestPoints(int idx,CvPoint2D32f ** pts,int * count,bool * found)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
DrawPoints(IplImage ** dst)505 void CvCalibFilter::DrawPoints( IplImage** dst )
506 {
507 DrawPoints( (CvMat**)dst );
508 }
509
510
DrawPoints(CvMat ** dstarr)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 */
GetFrameCount(int * total) const586 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 */
GetCameraParams(int idx) const597 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 */
GetStereoParams() const611 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 */
SetCameraParams(CvCamera * params)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
SaveCameraParams(const char * filename)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
LoadCameraParams(const char * filename)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
Rectify(IplImage ** srcarr,IplImage ** dstarr)761 bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr )
762 {
763 return Rectify( (CvMat**)srcarr, (CvMat**)dstarr );
764 }
765
Rectify(CvMat ** srcarr,CvMat ** dstarr)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
Undistort(IplImage ** srcarr,IplImage ** dstarr)830 bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr )
831 {
832 return Undistort( (CvMat**)srcarr, (CvMat**)dstarr );
833 }
834
835
Undistort(CvMat ** srcarr,CvMat ** dstarr)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