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 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 */
GetFrameCount(int * total) const583 int 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 */
GetCameraParams(int idx) const594 const 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 */
GetStereoParams() const608 const 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 */
SetCameraParams(CvCamera * params)621 bool 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
SaveCameraParams(const char * filename)647 bool 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
LoadCameraParams(const char * filename)700 bool 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
Rectify(IplImage ** srcarr,IplImage ** dstarr)758 bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr )
759 {
760 return Rectify( (CvMat**)srcarr, (CvMat**)dstarr );
761 }
762
Rectify(CvMat ** srcarr,CvMat ** dstarr)763 bool 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
Undistort(IplImage ** srcarr,IplImage ** dstarr)827 bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr )
828 {
829 return Undistort( (CvMat**)srcarr, (CvMat**)dstarr );
830 }
831
832
Undistort(CvMat ** srcarr,CvMat ** dstarr)833 bool 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