5 #include "opencv/cv.h"
\r
6 #include "opencv/highgui.h"
\r
11 #include "tracker_calibration.h"
\r
12 // example command line (for copy-n-paste):
\r
13 // calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>]
\r
15 /* The list of views may look as following (discard the starting and ending ------ separators):
\r
24 that is, the file will contain 6 lines, view002.png will not be used for calibration,
\r
25 other ones will be (those, in which the chessboard pattern will be found)
\r
30 double compute_reprojection_error( const CvMat* object_points,
\r
31 const CvMat* rot_vects, const CvMat* trans_vects,
\r
32 const CvMat* camera_matrix, const CvMat* dist_coeffs,
\r
33 const CvMat* image_points, const CvMat* point_counts,
\r
34 CvMat* per_view_errors )
\r
36 CvMat* image_points2 = cvCreateMat( image_points->rows,
\r
37 image_points->cols, image_points->type );
\r
38 int i, image_count = rot_vects->rows, points_so_far = 0;
\r
39 double total_err = 0, err;
\r
41 for( i = 0; i < image_count; i++ )
\r
43 CvMat object_points_i, image_points_i, image_points2_i;
\r
44 int point_count = point_counts->data.i[i];
\r
45 CvMat rot_vect, trans_vect;
\r
47 cvGetCols( object_points, &object_points_i,
\r
48 points_so_far, points_so_far + point_count );
\r
49 cvGetCols( image_points, &image_points_i,
\r
50 points_so_far, points_so_far + point_count );
\r
51 cvGetCols( image_points2, &image_points2_i,
\r
52 points_so_far, points_so_far + point_count );
\r
53 points_so_far += point_count;
\r
55 cvGetRow( rot_vects, &rot_vect, i );
\r
56 cvGetRow( trans_vects, &trans_vect, i );
\r
58 cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,
\r
59 camera_matrix, dist_coeffs, &image_points2_i,
\r
61 err = cvNorm( &image_points_i, &image_points2_i, CV_L1 );
\r
62 if( per_view_errors )
\r
63 per_view_errors->data.db[i] = err/point_count;
\r
67 cvReleaseMat( &image_points2 );
\r
68 return total_err/points_so_far;
\r
72 int run_calibration( CvSeq* image_points_seq, CvSize img_size, CvSize board_size,
\r
73 float square_size, float aspect_ratio, int flags,
\r
74 CvMat* camera_matrix, CvMat* dist_coeffs, CvMat** extr_params,
\r
75 CvMat** reproj_errs, double* avg_reproj_err )
\r
78 int image_count = image_points_seq->total;
\r
79 int point_count = board_size.width*board_size.height;
\r
80 CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2 );
\r
81 CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3 );
\r
82 CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1 );
\r
83 CvMat rot_vects, trans_vects;
\r
86 cvStartReadSeq( image_points_seq, &reader );
\r
88 // initialize arrays of points
\r
89 for( i = 0; i < image_count; i++ )
\r
91 CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;
\r
92 CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i*point_count;
\r
93 CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i*point_count;
\r
95 for( j = 0; j < board_size.height; j++ )
\r
96 for( k = 0; k < board_size.width; k++ )
\r
98 *obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);
\r
99 *dst_img_pt++ = *src_img_pt++;
\r
101 CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader );
\r
104 cvSet( point_counts, cvScalar(point_count) );
\r
106 *extr_params = cvCreateMat( image_count, 6, CV_32FC1 );
\r
107 cvGetCols( *extr_params, &rot_vects, 0, 3 );
\r
108 cvGetCols( *extr_params, &trans_vects, 3, 6 );
\r
110 cvZero( camera_matrix );
\r
111 cvZero( dist_coeffs );
\r
113 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
\r
115 camera_matrix->data.db[0] = aspect_ratio;
\r
116 camera_matrix->data.db[4] = 1.;
\r
119 cvCalibrateCamera2( object_points, image_points, point_counts,
\r
120 img_size, camera_matrix, dist_coeffs,
\r
121 &rot_vects, &trans_vects, flags );
\r
123 code = cvCheckArr( camera_matrix, CV_CHECK_QUIET ) &&
\r
124 cvCheckArr( dist_coeffs, CV_CHECK_QUIET ) &&
\r
125 cvCheckArr( *extr_params, CV_CHECK_QUIET );
\r
127 *reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 );
\r
129 compute_reprojection_error( object_points, &rot_vects, &trans_vects,
\r
130 camera_matrix, dist_coeffs, image_points, point_counts, *reproj_errs );
\r
132 cvReleaseMat( &object_points );
\r
133 cvReleaseMat( &image_points );
\r
134 cvReleaseMat( &point_counts );
\r
140 void save_camera_params( const char* out_filename, int image_count, CvSize img_size,
\r
141 CvSize board_size, float square_size,
\r
142 float aspect_ratio, int flags,
\r
143 const CvMat* camera_matrix, CvMat* dist_coeffs,
\r
144 const CvMat* extr_params, const CvSeq* image_points_seq,
\r
145 const CvMat* reproj_errs, double avg_reproj_err )
\r
147 CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE );
\r
151 struct tm *t2 = localtime( &t );
\r
153 strftime( buf, sizeof(buf)-1, "%c", t2 );
\r
155 cvWriteString( fs, "calibration_time", buf );
\r
157 cvWriteInt( fs, "image_count", image_count );
\r
158 cvWriteInt( fs, "image_width", img_size.width );
\r
159 cvWriteInt( fs, "image_height", img_size.height );
\r
160 cvWriteInt( fs, "board_width", board_size.width );
\r
161 cvWriteInt( fs, "board_height", board_size.height );
\r
162 cvWriteReal( fs, "square_size", square_size );
\r
164 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
\r
165 cvWriteReal( fs, "aspect_ratio", aspect_ratio );
\r
169 sprintf( buf, "flags: %s%s%s%s",
\r
170 flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
\r
171 flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspect_ratio" : "",
\r
172 flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
\r
173 flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" );
\r
174 cvWriteComment( fs, buf, 0 );
\r
177 cvWriteInt( fs, "flags", flags );
\r
179 cvWrite( fs, "camera_matrix", camera_matrix );
\r
180 cvWrite( fs, "distortion_coefficients", dist_coeffs );
\r
182 cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err );
\r
184 cvWrite( fs, "per_view_reprojection_errors", reproj_errs );
\r
188 cvWriteComment( fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
\r
189 cvWrite( fs, "extrinsic_parameters", extr_params );
\r
192 if( image_points_seq )
\r
194 cvWriteComment( fs, "the array of board corners projections used for calibration", 0 );
\r
195 assert( image_points_seq->total == image_count );
\r
196 CvMat* image_points = cvCreateMat( 1, image_count*board_size.width*board_size.height, CV_32FC2 );
\r
197 cvCvtSeqToArray( image_points_seq, image_points->data.fl );
\r
199 cvWrite( fs, "image_points", image_points );
\r
200 cvReleaseMat( &image_points );
\r
203 cvReleaseFileStorage( &fs );
\r
207 int calibrate( int argc, char** argv )
\r
209 CvSize board_size = {0,0};
\r
210 float square_size = 1.f, aspect_ratio = 1.f;
\r
211 const char* out_filename = "out_camera_data.yml";
\r
212 const char* input_filename = 0;
\r
213 int i, image_count = 10;
\r
214 int write_extrinsics = 0, write_points = 0;
\r
216 CvCapture* capture = 0;
\r
218 char imagename[1024];
\r
219 CvMemStorage* storage;
\r
220 CvSeq* image_points_seq = 0;
\r
221 int elem_size, flip_vertical = 0;
\r
223 clock_t prev_timestamp = 0;
\r
224 CvPoint2D32f* image_points_buf = 0;
\r
225 CvFont font = cvFont( 1, 1 );
\r
226 double _camera[9], _dist_coeffs[4];
\r
227 CvMat camera = cvMat( 3, 3, CV_64F, _camera );
\r
228 CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs );
\r
229 CvMat *extr_params = 0, *reproj_errs = 0;
\r
230 double avg_reproj_err = 0;
\r
231 int mode = DETECTION;
\r
232 int undistort_image = 0;
\r
233 CvSize img_size = {0,0};
\r
234 //const char* live_capture_help =
\r
235 // "When the live video from camera is used as input, the following hot-keys may be used:\n"
\r
236 // " <ESC>, 'q' - quit the calibration\n"
\r
237 // " 'g' - start capturing images\n"
\r
238 // " 'u' - switch undistortion on/off\n";
\r
239 const char* live_capture_help =
\r
240 "Press g to start calibrating the camera\n";
\r
244 printf( "This is a camera calibration sample.\n"
\r
245 "Usage: calibration\n"
\r
246 " -w <board_width> # the number of inner corners per one of board dimension\n"
\r
247 " -h <board_height> # the number of inner corners per another board dimension\n"
\r
248 " [-n <number_of_frames>] # the number of frames to use for calibration\n"
\r
249 " # (if not specified, it will be set to the number\n"
\r
250 " # of board views actually available)\n"
\r
251 " [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n"
\r
252 " # (used only for video capturing)\n"
\r
253 " [-s <square_size>] # square size in some user-defined units (1 by default)\n"
\r
254 " [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
\r
255 " [-op] # write detected feature points\n"
\r
256 " [-oe] # write extrinsic parameters\n"
\r
257 " [-zt] # assume zero tangential distortion\n"
\r
258 " [-a <aspect_ratio>] # fix aspect ratio (fx/fy)\n"
\r
259 " [-p] # fix the principal point at the center\n"
\r
260 " [-v] # flip the captured images around the horizontal axis\n"
\r
261 " [input_data] # input data, one of the following:\n"
\r
262 " # - text file with a list of the images of the board\n"
\r
263 " # - name of video file with a video of the board\n"
\r
264 " # if input_data not specified, a live view from the camera is used\n"
\r
266 printf( "%s", live_capture_help );
\r
270 for( i = 1; i < argc; i++ )
\r
272 const char* s = argv[i];
\r
273 if( strcmp( s, "-w" ) == 0 )
\r
275 if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 )
\r
276 return fprintf( stderr, "Invalid board width\n" ), -1;
\r
278 else if( strcmp( s, "-h" ) == 0 )
\r
280 if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 )
\r
281 return fprintf( stderr, "Invalid board height\n" ), -1;
\r
283 else if( strcmp( s, "-s" ) == 0 )
\r
285 if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 )
\r
286 return fprintf( stderr, "Invalid board square width\n" ), -1;
\r
288 else if( strcmp( s, "-n" ) == 0 )
\r
290 if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )
\r
291 return printf("Invalid number of images\n" ), -1;
\r
293 else if( strcmp( s, "-a" ) == 0 )
\r
295 if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )
\r
296 return printf("Invalid aspect ratio\n" ), -1;
\r
298 else if( strcmp( s, "-d" ) == 0 )
\r
300 if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )
\r
301 return printf("Invalid delay\n" ), -1;
\r
303 else if( strcmp( s, "-op" ) == 0 )
\r
307 else if( strcmp( s, "-oe" ) == 0 )
\r
309 write_extrinsics = 1;
\r
311 else if( strcmp( s, "-zt" ) == 0 )
\r
313 flags |= CV_CALIB_ZERO_TANGENT_DIST;
\r
315 else if( strcmp( s, "-p" ) == 0 )
\r
317 flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
\r
319 else if( strcmp( s, "-v" ) == 0 )
\r
323 else if( strcmp( s, "-o" ) == 0 )
\r
325 out_filename = argv[++i];
\r
327 else if( s[0] != '-' )
\r
328 input_filename = s;
\r
330 return fprintf( stderr, "Unknown option %s", s ), -1;
\r
333 if( input_filename )
\r
335 capture = cvCreateFileCapture( input_filename );
\r
338 f = fopen( input_filename, "rt" );
\r
340 return fprintf( stderr, "The input file could not be opened\n" ), -1;
\r
347 capture = cvCreateCameraCapture(0);
\r
348 cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,640);
\r
349 cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,480);
\r
353 if( !capture && !f )
\r
354 return fprintf( stderr, "Could not initialize video capture\n" ), -2;
\r
356 if( capture && !input_filename)
\r
357 printf( "%s", live_capture_help );
\r
359 elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]);
\r
360 storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 ));
\r
361 image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size );
\r
362 image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
\r
364 cvNamedWindow( "Image View", 1 );
\r
368 IplImage *view = 0, *view_gray = 0;
\r
369 int count = 0, found, blink = 0;
\r
370 CvPoint text_origin;
\r
371 CvSize text_size = {0,0};
\r
376 if( f && fgets( imagename, sizeof(imagename)-2, f ))
\r
378 int l = strlen(imagename);
\r
379 if( l > 0 && imagename[l-1] == '\n' )
\r
380 imagename[--l] = '\0';
\r
383 if( imagename[0] == '#' )
\r
385 view = cvLoadImage( imagename, 1 );
\r
390 IplImage* view0 = cvQueryFrame( capture );
\r
393 view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );
\r
394 if( view0->origin == IPL_ORIGIN_BL )
\r
395 cvFlip( view0, view, 0 );
\r
397 cvCopy( view0, view );
\r
403 if( image_points_seq->total > 0 )
\r
405 image_count = image_points_seq->total;
\r
411 if( flip_vertical )
\r
412 cvFlip( view, view, 0 );
\r
414 img_size = cvGetSize(view);
\r
415 found = cvFindChessboardCorners( view, board_size,
\r
416 image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
\r
419 // improve the found corners' coordinate accuracy
\r
420 view_gray = cvCreateImage( cvGetSize(view), 8, 1 );
\r
421 cvCvtColor( view, view_gray, CV_BGR2GRAY );
\r
422 cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),
\r
423 cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
\r
424 cvReleaseImage( &view_gray );
\r
427 if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )
\r
429 cvSeqPush( image_points_seq, image_points_buf );
\r
430 prev_timestamp = clock();
\r
435 sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );
\r
436 cvSaveImage( imagename, view );
\r
441 cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );
\r
443 cvGetTextSize( "100/100", &font, &text_size, &base_line );
\r
444 text_origin.x = view->width - text_size.width - 10;
\r
445 text_origin.y = view->height - base_line - 10;
\r
447 if( mode == CAPTURING )
\r
449 if( image_count > 0 )
\r
450 sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );
\r
452 sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );
\r
454 else if( mode == CALIBRATED )
\r
455 sprintf( s, "Calibrated" );
\r
457 sprintf( s, "Press 'g' to start" );
\r
459 cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?
\r
460 CV_RGB(255,0,0) : CV_RGB(0,255,0));
\r
463 cvNot( view, view );
\r
465 if( mode == CALIBRATED && undistort_image )
\r
467 IplImage* t = cvCloneImage( view );
\r
468 cvUndistort2( t, view, &camera, &dist_coeffs );
\r
469 cvReleaseImage( &t );
\r
471 if (mode == CALIBRATED/* && input_filename*/)
\r
473 printf ("\n Calibration done\n");
\r
477 cvShowImage( "Image View", view );
\r
478 key = cvWaitKey(capture ? 50 : 500);
\r
483 if( key == 'u' && mode == CALIBRATED )
\r
484 undistort_image = !undistort_image;
\r
486 if( capture && key == 'g' )
\r
489 cvClearMemStorage( storage );
\r
490 image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
\r
493 if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )
\r
496 cvReleaseMat( &extr_params );
\r
497 cvReleaseMat( &reproj_errs );
\r
498 int code = run_calibration( image_points_seq, img_size, board_size,
\r
499 square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params,
\r
500 &reproj_errs, &avg_reproj_err );
\r
501 // save camera parameters in any case, to catch Inf's/NaN's
\r
502 save_camera_params( out_filename, image_count, img_size,
\r
503 board_size, square_size, aspect_ratio, flags,
\r
504 &camera, &dist_coeffs, write_extrinsics ? extr_params : 0,
\r
505 write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err );
\r
514 cvReleaseImage( &view );
\r
516 cvDestroyAllWindows();
\r
518 cvReleaseCapture( &capture );
\r
520 cvReleaseMemStorage( &storage );
\r