]> rtime.felk.cvut.cz Git - opencv.git/blob - opencv_extra/3d/tracker3D/tracker_calibration.cpp
Corrected 3d tracker application and added sample data
[opencv.git] / opencv_extra / 3d / tracker3D / tracker_calibration.cpp
1 #ifdef WIN32\r
2 #include "cv.h"\r
3 #include "highgui.h"\r
4 #else\r
5 #include "opencv/cv.h"\r
6 #include "opencv/highgui.h"\r
7 #endif\r
8 #include <stdio.h>\r
9 #include <string.h>\r
10 #include <time.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
14 \r
15 /* The list of views may look as following (discard the starting and ending ------ separators):\r
16 -------------------\r
17 view000.png\r
18 view001.png\r
19 #view002.png\r
20 view003.png\r
21 view010.png\r
22 one_extra_view.jpg\r
23 -------------------\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
26 */\r
27 \r
28 \r
29 \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
35 {\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
40     \r
41     for( i = 0; i < image_count; i++ )\r
42     {\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
46 \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
54 \r
55         cvGetRow( rot_vects, &rot_vect, i );\r
56         cvGetRow( trans_vects, &trans_vect, i );\r
57 \r
58         cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,\r
59                           camera_matrix, dist_coeffs, &image_points2_i,\r
60                           0, 0, 0, 0, 0 );\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
64         total_err += err;\r
65     }\r
66     \r
67     cvReleaseMat( &image_points2 );\r
68     return total_err/points_so_far;\r
69 }\r
70 \r
71 \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
76 {\r
77     int code;\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
84     int i, j, k;\r
85     CvSeqReader reader;\r
86     cvStartReadSeq( image_points_seq, &reader );\r
87 \r
88     // initialize arrays of points\r
89     for( i = 0; i < image_count; i++ )\r
90     {\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
94 \r
95         for( j = 0; j < board_size.height; j++ )\r
96             for( k = 0; k < board_size.width; k++ )\r
97             {\r
98                 *obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);\r
99                 *dst_img_pt++ = *src_img_pt++;\r
100             }\r
101         CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader );\r
102     }\r
103 \r
104     cvSet( point_counts, cvScalar(point_count) );\r
105 \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
109 \r
110     cvZero( camera_matrix );\r
111     cvZero( dist_coeffs );\r
112 \r
113     if( flags & CV_CALIB_FIX_ASPECT_RATIO )\r
114     {\r
115         camera_matrix->data.db[0] = aspect_ratio;\r
116         camera_matrix->data.db[4] = 1.;\r
117     }\r
118 \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
122 \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
126 \r
127     *reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 );\r
128     *avg_reproj_err =\r
129         compute_reprojection_error( object_points, &rot_vects, &trans_vects,\r
130             camera_matrix, dist_coeffs, image_points, point_counts, *reproj_errs );\r
131 \r
132     cvReleaseMat( &object_points );\r
133     cvReleaseMat( &image_points );\r
134     cvReleaseMat( &point_counts );\r
135 \r
136     return code;\r
137 }\r
138 \r
139 \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
146 {\r
147     CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE );\r
148     \r
149     time_t t;\r
150     time( &t );\r
151     struct tm *t2 = localtime( &t );\r
152     char buf[1024];\r
153     strftime( buf, sizeof(buf)-1, "%c", t2 );\r
154 \r
155     cvWriteString( fs, "calibration_time", buf );\r
156     \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
163     \r
164     if( flags & CV_CALIB_FIX_ASPECT_RATIO )\r
165         cvWriteReal( fs, "aspect_ratio", aspect_ratio );\r
166 \r
167     if( flags != 0 )\r
168     {\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
175     }\r
176     \r
177     cvWriteInt( fs, "flags", flags );\r
178 \r
179     cvWrite( fs, "camera_matrix", camera_matrix );\r
180     cvWrite( fs, "distortion_coefficients", dist_coeffs );\r
181 \r
182     cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err );\r
183     if( reproj_errs )\r
184         cvWrite( fs, "per_view_reprojection_errors", reproj_errs );\r
185 \r
186     if( extr_params )\r
187     {\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
190     }\r
191 \r
192     if( image_points_seq )\r
193     {\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
198 \r
199         cvWrite( fs, "image_points", image_points );\r
200         cvReleaseMat( &image_points );\r
201     }\r
202 \r
203     cvReleaseFileStorage( &fs );\r
204 }\r
205 \r
206 \r
207 int calibrate( int argc, char** argv )\r
208 {\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
215     int flags = 0;\r
216     CvCapture* capture = 0;\r
217     FILE* f = 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
222     int delay = 1000;\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
241 \r
242     if( argc < 2 )\r
243     {\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
265             "\n" );\r
266         printf( "%s", live_capture_help );\r
267         return 0;\r
268     }\r
269 \r
270     for( i = 1; i < argc; i++ )\r
271     {\r
272         const char* s = argv[i];\r
273         if( strcmp( s, "-w" ) == 0 )\r
274         {\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
277         }\r
278         else if( strcmp( s, "-h" ) == 0 )\r
279         {\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
282         }\r
283         else if( strcmp( s, "-s" ) == 0 )\r
284         {\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
287         }\r
288         else if( strcmp( s, "-n" ) == 0 )\r
289         {\r
290             if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )\r
291                 return printf("Invalid number of images\n" ), -1;\r
292         }\r
293         else if( strcmp( s, "-a" ) == 0 )\r
294         {\r
295             if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )\r
296                 return printf("Invalid aspect ratio\n" ), -1;\r
297         }\r
298         else if( strcmp( s, "-d" ) == 0 )\r
299         {\r
300             if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )\r
301                 return printf("Invalid delay\n" ), -1;\r
302         }\r
303         else if( strcmp( s, "-op" ) == 0 )\r
304         {\r
305             write_points = 1;\r
306         }\r
307         else if( strcmp( s, "-oe" ) == 0 )\r
308         {\r
309             write_extrinsics = 1;\r
310         }\r
311         else if( strcmp( s, "-zt" ) == 0 )\r
312         {\r
313             flags |= CV_CALIB_ZERO_TANGENT_DIST;\r
314         }\r
315         else if( strcmp( s, "-p" ) == 0 )\r
316         {\r
317             flags |= CV_CALIB_FIX_PRINCIPAL_POINT;\r
318         }\r
319         else if( strcmp( s, "-v" ) == 0 )\r
320         {\r
321             flip_vertical = 1;\r
322         }\r
323         else if( strcmp( s, "-o" ) == 0 )\r
324         {\r
325             out_filename = argv[++i];\r
326         }\r
327         else if( s[0] != '-' )\r
328             input_filename = s;\r
329         else\r
330             return fprintf( stderr, "Unknown option %s", s ), -1;\r
331     }\r
332 \r
333     if( input_filename )\r
334     {\r
335         capture = cvCreateFileCapture( input_filename );\r
336         if( !capture )\r
337         {\r
338             f = fopen( input_filename, "rt" );\r
339             if( !f )\r
340                 return fprintf( stderr, "The input file could not be opened\n" ), -1;\r
341             image_count = -1;\r
342         }\r
343         mode = CAPTURING;\r
344     }\r
345     else\r
346         {\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
350 \r
351         }\r
352 \r
353     if( !capture && !f )\r
354         return fprintf( stderr, "Could not initialize video capture\n" ), -2;\r
355 \r
356     if( capture && !input_filename)\r
357         printf( "%s", live_capture_help );\r
358 \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
363 \r
364     cvNamedWindow( "Image View", 1 );\r
365 \r
366     for(;;)\r
367     {\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
372         int base_line = 0;\r
373         char s[100];\r
374         int key;\r
375         \r
376         if( f && fgets( imagename, sizeof(imagename)-2, f ))\r
377         {\r
378             int l = strlen(imagename);\r
379             if( l > 0 && imagename[l-1] == '\n' )\r
380                 imagename[--l] = '\0';\r
381             if( l > 0 )\r
382             {\r
383                 if( imagename[0] == '#' )\r
384                     continue;\r
385                 view = cvLoadImage( imagename, 1 );\r
386             }\r
387         }\r
388         else if( capture )\r
389         {\r
390             IplImage* view0 = cvQueryFrame( capture );\r
391             if( view0 )\r
392             {\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
396                 else\r
397                     cvCopy( view0, view );\r
398             }\r
399         }\r
400 \r
401         if( !view )\r
402         {\r
403             if( image_points_seq->total > 0 )\r
404             {\r
405                 image_count = image_points_seq->total;\r
406                 goto calibrate;\r
407             }\r
408             break;\r
409         }\r
410 \r
411         if( flip_vertical )\r
412             cvFlip( view, view, 0 );\r
413 \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
417 \r
418 #if 1\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
425 #endif\r
426 \r
427         if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )\r
428         {\r
429             cvSeqPush( image_points_seq, image_points_buf );\r
430             prev_timestamp = clock();\r
431             blink = !f;\r
432 #if 1\r
433             if( capture )\r
434             {\r
435                 sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );\r
436                 cvSaveImage( imagename, view );\r
437             }\r
438 #endif\r
439         }\r
440 \r
441         cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );\r
442 \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
446 \r
447         if( mode == CAPTURING )\r
448         {\r
449             if( image_count > 0 )\r
450                 sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );\r
451             else\r
452                 sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );\r
453         }\r
454         else if( mode == CALIBRATED )\r
455             sprintf( s, "Calibrated" );\r
456         else\r
457             sprintf( s, "Press 'g' to start" );\r
458 \r
459         cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?\r
460                                    CV_RGB(255,0,0) : CV_RGB(0,255,0));\r
461 \r
462         if( blink )\r
463             cvNot( view, view );\r
464 \r
465         if( mode == CALIBRATED && undistort_image )\r
466         {\r
467             IplImage* t = cvCloneImage( view );\r
468             cvUndistort2( t, view, &camera, &dist_coeffs );\r
469             cvReleaseImage( &t );\r
470         }\r
471                 if (mode == CALIBRATED/* && input_filename*/)\r
472                 {\r
473                         printf ("\n Calibration done\n");\r
474                         break;\r
475                 }\r
476 \r
477         cvShowImage( "Image View", view );\r
478         key = cvWaitKey(capture ? 50 : 500);\r
479 \r
480         if( key == 27 )\r
481             break;\r
482         \r
483         if( key == 'u' && mode == CALIBRATED )\r
484             undistort_image = !undistort_image;\r
485 \r
486         if( capture && key == 'g' )\r
487         {\r
488             mode = CAPTURING;\r
489             cvClearMemStorage( storage );\r
490             image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );\r
491         }\r
492 \r
493         if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )\r
494         {\r
495 calibrate:\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
506             if( code )\r
507                 mode = CALIBRATED;\r
508             else\r
509                 mode = DETECTION;\r
510         }\r
511 \r
512         if( !view )\r
513             break;\r
514         cvReleaseImage( &view );\r
515     }\r
516         cvDestroyAllWindows();\r
517     if( capture )\r
518         cvReleaseCapture( &capture );\r
519     if( storage )\r
520         cvReleaseMemStorage( &storage );\r
521     return 0;\r
522 }\r