1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
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.
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.
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.
48 #pragma warning( disable: 4701 )
51 CvCalibFilter::CvCalibFilter()
54 etalonType = CV_CALIB_ETALON_USER;
63 memset( points, 0, sizeof(points));
64 memset( undistMap, 0, sizeof(undistMap));
66 memset( latestCounts, 0, sizeof(latestCounts));
67 memset( latestPoints, 0, sizeof(latestPoints));
68 memset( &stereo, 0, sizeof(stereo) );
74 imgSize = cvSize(0,0);
79 memset( rectMap, 0, sizeof(rectMap));
83 CvCalibFilter::~CvCalibFilter()
86 cvFree( &etalonParams );
87 cvFree( &etalonPoints );
88 cvReleaseMat( &grayImg );
89 cvReleaseMat( &tempImg );
90 cvReleaseMat( &undistImg );
91 cvReleaseMemStorage( &storage );
95 bool CvCalibFilter::SetEtalon( CvCalibEtalonType type, double* params,
96 int pointCount, CvPoint2D32f* points )
102 for( i = 0; i < MAX_CAMERAS; i++ )
103 cvFree( latestPoints + i );
105 if( type == CV_CALIB_ETALON_USER || type != etalonType )
107 cvFree( &etalonParams );
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 )
123 pointCount = cvRound((params[0] - 1)*(params[1] - 1));
126 case CV_CALIB_ETALON_USER:
127 etalonParamCount = 0;
129 if( !points || pointCount < 4 )
141 if( etalonParamCount > 0 )
143 arrSize = etalonParamCount * sizeof(etalonParams[0]);
144 etalonParams = (double*)cvAlloc( arrSize );
147 arrSize = pointCount * sizeof(etalonPoints[0]);
149 if( etalonPointCount != pointCount )
151 cvFree( &etalonPoints );
152 etalonPointCount = pointCount;
153 etalonPoints = (CvPoint2D32f*)cvAlloc( arrSize );
158 case CV_CALIB_ETALON_CHESSBOARD:
160 int etalonWidth = cvRound( params[0] ) - 1;
161 int etalonHeight = cvRound( params[1] ) - 1;
164 etalonParams[0] = etalonWidth;
165 etalonParams[1] = etalonHeight;
166 etalonParams[2] = params[2];
168 for( y = 0; y < etalonHeight; y++ )
169 for( x = 0; x < etalonWidth; x++ )
171 etalonPoints[k++] = cvPoint2D32f( (etalonWidth - 1 - x)*params[2],
177 case CV_CALIB_ETALON_USER:
178 memcpy( etalonParams, params, arrSize );
179 memcpy( etalonPoints, points, arrSize );
192 CvCalibFilter::GetEtalon( int* paramCount, const double** params,
193 int* pointCount, const CvPoint2D32f** points ) const
196 *paramCount = etalonParamCount;
199 *params = etalonParams;
202 *pointCount = etalonPointCount;
205 *points = etalonPoints;
211 void CvCalibFilter::SetCameraCount( int count )
215 if( count != cameraCount )
217 for( int i = 0; i < cameraCount; i++ )
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] );
227 memset( latestCounts, 0, sizeof(latestPoints) );
234 bool CvCalibFilter::SetFrames( int frames )
242 framesTotal = frames;
247 void CvCalibFilter::Stop( bool calibrate )
250 isCalibrated = false;
252 // deallocate undistortion maps
253 for( i = 0; i < cameraCount; i++ )
255 cvReleaseMat( &undistMap[i][0] );
256 cvReleaseMat( &undistMap[i][1] );
257 cvReleaseMat( &rectMap[i][0] );
258 cvReleaseMat( &rectMap[i][1] );
261 if( calibrate && framesAccepted > 0 )
263 int n = framesAccepted;
264 CvPoint3D32f* buffer =
265 (CvPoint3D32f*)cvAlloc( n * etalonPointCount * sizeof(buffer[0]));
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]));
271 cvInitMatHeader( &mat, 1, sizeof(CvCamera)/sizeof(float), CV_32FC1, 0 );
272 memset( cameraParams, 0, cameraCount * sizeof(cameraParams[0]));
274 for( i = 0; i < framesAccepted; i++ )
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 );
282 for( i = 0; i < cameraCount; i++ )
284 cvCalibrateCamera( framesAccepted, counts,
285 imgSize, points[i], buffer,
286 cameraParams[i].distortion,
287 cameraParams[i].matrix,
288 transVect, rotMatr, 0 );
290 cameraParams[i].imgSize[0] = (float)imgSize.width;
291 cameraParams[i].imgSize[1] = (float)imgSize.height;
293 // cameraParams[i].focalLength[0] = cameraParams[i].matrix[0];
294 // cameraParams[i].focalLength[1] = cameraParams[i].matrix[4];
296 // cameraParams[i].principalPoint[0] = cameraParams[i].matrix[2];
297 // cameraParams[i].principalPoint[1] = cameraParams[i].matrix[5];
299 memcpy( cameraParams[i].rotMatr, rotMatr, 9 * sizeof(rotMatr[0]));
300 memcpy( cameraParams[i].transVect, transVect, 3 * sizeof(transVect[0]));
302 mat.data.ptr = (uchar*)(cameraParams + i);
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 ))
312 isCalibrated = i == cameraCount;
314 {/* calibrate stereo cameras */
315 if( cameraCount == 2 )
317 stereo.camera[0] = &cameraParams[0];
318 stereo.camera[1] = &cameraParams[1];
320 icvStereoCalibration( framesAccepted, counts,
326 for( i = 0; i < 9; i++ )
328 stereo.fundMatr[i] = stereo.fundMatr[i];
338 cvFree( &transVect );
345 bool CvCalibFilter::FindEtalon( IplImage** imgs )
347 return FindEtalon( (CvMat**)imgs );
351 bool CvCalibFilter::FindEtalon( CvMat** mats )
355 if( !mats || etalonPointCount == 0 )
363 int i, tempPointCount0 = etalonPointCount*2;
365 for( i = 0; i < cameraCount; i++ )
367 if( !latestPoints[i] )
368 latestPoints[i] = (CvPoint2D32f*)
369 cvAlloc( tempPointCount0*2*sizeof(latestPoints[0]));
372 for( i = 0; i < cameraCount; i++ )
375 int tempPointCount = tempPointCount0;
378 if( !CV_IS_MAT(mats[i]) && !CV_IS_IMAGE(mats[i]))
384 size = cvGetSize(mats[i]);
386 if( size.width != imgSize.width || size.height != imgSize.height )
391 if( !grayImg || grayImg->width != imgSize.width ||
392 grayImg->height != imgSize.height )
394 cvReleaseMat( &grayImg );
395 cvReleaseMat( &tempImg );
396 grayImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
397 tempImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
401 storage = cvCreateMemStorage();
405 case CV_CALIB_ETALON_CHESSBOARD:
406 if( CV_MAT_CN(cvGetElemType(mats[i])) == 1 )
407 cvCopy( mats[i], grayImg );
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;
415 cvFindCornerSubPix( grayImg, latestPoints[i], tempPointCount,
416 cvSize(5,5), cvSize(-1,-1),
417 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,10,0.1));
425 latestCounts[i] = found ? tempPointCount : -tempPointCount;
426 result = result && found;
431 cvClearMemStorage( storage );
437 bool CvCalibFilter::Push( const CvPoint2D32f** pts )
440 int i, newMaxPoints = etalonPointCount*(MAX(framesAccepted,framesTotal) + 1);
442 isCalibrated = false;
446 for( i = 0; i < cameraCount; i++ )
447 if( latestCounts[i] <= 0 )
449 pts = (const CvPoint2D32f**)latestPoints;
452 for( i = 0; i < cameraCount; i++ )
460 if( maxPoints < newMaxPoints )
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]));
468 memcpy( points[i] + framesAccepted*etalonPointCount, pts[i],
469 etalonPointCount*sizeof(points[0][0]));
472 if( maxPoints < newMaxPoints )
473 maxPoints = newMaxPoints;
475 result = i == cameraCount;
477 if( ++framesAccepted >= framesTotal )
483 bool CvCalibFilter::GetLatestPoints( int idx, CvPoint2D32f** pts,
484 int* count, bool* found )
488 if( (unsigned)idx >= (unsigned)cameraCount ||
489 !pts || !count || !found )
495 n = latestCounts[idx];
499 *pts = latestPoints[idx];
505 void CvCalibFilter::DrawPoints( IplImage** dst )
507 DrawPoints( (CvMat**)dst );
511 void CvCalibFilter::DrawPoints( CvMat** dstarr )
523 for( i = 0; i < cameraCount; i++ )
525 if( dstarr[i] && latestCounts[i] )
527 CvMat dst_stub, *dst;
530 CvPoint2D32f* pts = 0;
532 GetLatestPoints( i, &pts, &count, &found );
534 dst = cvGetMat( dstarr[i], &dst_stub );
536 static const CvScalar line_colors[] =
547 const int colorCount = sizeof(line_colors)/sizeof(line_colors[0]);
549 CvScalar color = line_colors[0];
550 CvPoint prev_pt = { 0, 0};
552 for( j = 0; j < count; j++ )
555 pt.x = cvRound(pts[j].x);
556 pt.y = cvRound(pts[j].y);
560 if( etalonType == CV_CALIB_ETALON_CHESSBOARD )
561 color = line_colors[(j/cvRound(etalonParams[0]))%colorCount];
563 color = CV_RGB(0,255,0);
566 cvLine( dst, prev_pt, pt, color, 1, CV_AA );
569 cvLine( dst, cvPoint( pt.x - r, pt.y - r ),
570 cvPoint( pt.x + r, pt.y + r ), color, 1, CV_AA );
572 cvLine( dst, cvPoint( pt.x - r, pt.y + r),
573 cvPoint( pt.x + r, pt.y - r), color, 1, CV_AA );
575 cvCircle( dst, pt, r+1, color, 1, CV_AA );
585 /* Get total number of frames and already accepted pair of frames */
586 int CvCalibFilter::GetFrameCount( int* total ) const
589 *total = framesTotal;
591 return framesAccepted;
595 /* Get camera parameters for specified camera. If camera is not calibrated
596 the function returns 0 */
597 const CvCamera* CvCalibFilter::GetCameraParams( int idx ) const
599 if( (unsigned)idx >= (unsigned)cameraCount )
605 return isCalibrated ? cameraParams + idx : 0;
609 /* Get camera parameters for specified camera. If camera is not calibrated
610 the function returns 0 */
611 const CvStereoCamera* CvCalibFilter::GetStereoParams() const
613 if( !(isCalibrated && cameraCount == 2) )
623 /* Sets camera parameters for all cameras */
624 bool CvCalibFilter::SetCameraParams( CvCamera* params )
637 arrSize = cameraCount * sizeof(params[0]);
639 cvInitMatHeader( &mat, 1, cameraCount * (arrSize/sizeof(float)),
641 cvCheckArr( &mat, CV_CHECK_RANGE, -10000, 10000 );
643 memcpy( cameraParams, params, arrSize );
650 bool CvCalibFilter::SaveCameraParams( const char* filename )
656 FILE* f = fopen( filename, "w" );
658 if( !f ) return false;
660 fprintf( f, "%d\n\n", cameraCount );
662 for( i = 0; i < cameraCount; i++ )
664 for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ )
666 fprintf( f, "%15.10f ", ((float*)(cameraParams + i))[j] );
668 fprintf( f, "\n\n" );
671 /* Save stereo params */
674 for( i = 0; i < 2; i++ )
676 for( j = 0; j < 4; j++ )
678 fprintf(f, "%15.10f ", stereo.quad[i][j].x );
679 fprintf(f, "%15.10f ", stereo.quad[i][j].y );
685 for( i = 0; i < 2; i++ )
687 for( j = 0; j < 9; j++ )
689 fprintf(f, "%15.10lf ", stereo.coeffs[i][j/3][j%3] );
703 bool CvCalibFilter::LoadCameraParams( const char* filename )
707 FILE* f = fopen( filename, "r" );
709 isCalibrated = false;
711 if( !f ) return false;
713 if( fscanf( f, "%d", &d ) != 1 || d <= 0 || d > 10 )
718 for( i = 0; i < cameraCount; i++ )
720 for( j = 0; j < (int)(sizeof(cameraParams[i])/sizeof(float)); j++ )
722 fscanf( f, "%f", &((float*)(cameraParams + i))[j] );
727 /* Load stereo params */
730 for( i = 0; i < 2; i++ )
732 for( j = 0; j < 4; j++ )
734 fscanf(f, "%f ", &(stereo.quad[i][j].x) );
735 fscanf(f, "%f ", &(stereo.quad[i][j].y) );
740 for( i = 0; i < 2; i++ )
742 for( j = 0; j < 9; j++ )
744 fscanf(f, "%lf ", &(stereo.coeffs[i][j/3][j%3]) );
753 stereo.warpSize = cvSize( cvRound(cameraParams[0].imgSize[0]), cvRound(cameraParams[0].imgSize[1]));
761 bool CvCalibFilter::Rectify( IplImage** srcarr, IplImage** dstarr )
763 return Rectify( (CvMat**)srcarr, (CvMat**)dstarr );
766 bool CvCalibFilter::Rectify( CvMat** srcarr, CvMat** dstarr )
770 if( !srcarr || !dstarr )
776 if( isCalibrated && cameraCount == 2 )
778 for( i = 0; i < cameraCount; i++ )
780 if( srcarr[i] && dstarr[i] )
782 IplImage src_stub, *src;
783 IplImage dst_stub, *dst;
785 src = cvGetImage( srcarr[i], &src_stub );
786 dst = cvGetImage( dstarr[i], &dst_stub );
788 if( src->imageData == dst->imageData )
791 undistImg->width != src->width ||
792 undistImg->height != src->height ||
793 CV_MAT_CN(undistImg->type) != src->nChannels )
795 cvReleaseMat( &undistImg );
796 undistImg = cvCreateMat( src->height, src->width,
797 CV_8U + (src->nChannels-1)*8 );
799 cvCopy( src, undistImg );
800 src = cvGetImage( undistImg, &src_stub );
805 if( !rectMap[i][0] || rectMap[i][0]->width != src->width ||
806 rectMap[i][0]->height != src->height )
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]);
814 cvRemap( src, dst, rectMap[i][0], rectMap[i][1] );
820 for( i = 0; i < cameraCount; i++ )
822 if( srcarr[i] != dstarr[i] )
823 cvCopy( srcarr[i], dstarr[i] );
830 bool CvCalibFilter::Undistort( IplImage** srcarr, IplImage** dstarr )
832 return Undistort( (CvMat**)srcarr, (CvMat**)dstarr );
836 bool CvCalibFilter::Undistort( CvMat** srcarr, CvMat** dstarr )
840 if( !srcarr || !dstarr )
848 for( i = 0; i < cameraCount; i++ )
850 if( srcarr[i] && dstarr[i] )
852 CvMat src_stub, *src;
853 CvMat dst_stub, *dst;
855 src = cvGetMat( srcarr[i], &src_stub );
856 dst = cvGetMat( dstarr[i], &dst_stub );
858 if( src->data.ptr == dst->data.ptr )
860 if( !undistImg || undistImg->width != src->width ||
861 undistImg->height != src->height ||
862 CV_ARE_TYPES_EQ( undistImg, src ))
864 cvReleaseMat( &undistImg );
865 undistImg = cvCreateMat( src->height, src->width, src->type );
868 cvCopy( src, undistImg );
874 CvMat A = cvMat( 3, 3, CV_32FC1, cameraParams[i].matrix );
875 CvMat k = cvMat( 1, 4, CV_32FC1, cameraParams[i].distortion );
877 if( !undistMap[i][0] || undistMap[i][0]->width != src->width ||
878 undistMap[i][0]->height != src->height )
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] );
887 cvRemap( src, dst, undistMap[i][0], undistMap[i][1] );
889 cvUndistort2( src, dst, &A, &k );
897 for( i = 0; i < cameraCount; i++ )
899 if( srcarr[i] != dstarr[i] )
900 cvCopy( srcarr[i], dstarr[i] );