vcs.maemo.org Git - opencv/blob - samples/c/calibration.cpp

   1 #include "cv.h"
   2 #include "highgui.h"
   3 #include <stdio.h>
   4 #include <string.h>
   5 #include <time.h>
   6
   7 // example command line (for copy-n-paste):
   8 // calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>]
   9
  10 /* The list of views may look as following (discard the starting and ending ------ separators):
  11 -------------------
  12 view000.png
  13 view001.png
  14 #view002.png
  15 view003.png
  16 view010.png
  17 one_extra_view.jpg
  18 -------------------
  19 that is, the file will contain 6 lines, view002.png will not be used for calibration,
  20 other ones will be (those, in which the chessboard pattern will be found)
  21 */
  22
  23 enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
  24
  25 double compute_reprojection_error( const CvMat* object_points,
  26         const CvMat* rot_vects, const CvMat* trans_vects,
  27         const CvMat* camera_matrix, const CvMat* dist_coeffs,
  28         const CvMat* image_points, const CvMat* point_counts,
  29         CvMat* per_view_errors )
  30 {
  31     CvMat* image_points2 = cvCreateMat( image_points->rows,
  32         image_points->cols, image_points->type );
  33     int i, image_count = rot_vects->rows, points_so_far = 0;
  34     double total_err = 0, err;
  35
  36     for( i = 0; i < image_count; i++ )
  37     {
  38         CvMat object_points_i, image_points_i, image_points2_i;
  39         int point_count = point_counts->data.i[i];
  40         CvMat rot_vect, trans_vect;
  41
  42         cvGetCols( object_points, &object_points_i,
  43             points_so_far, points_so_far + point_count );
  44         cvGetCols( image_points, &image_points_i,
  45             points_so_far, points_so_far + point_count );
  46         cvGetCols( image_points2, &image_points2_i,
  47             points_so_far, points_so_far + point_count );
  48         points_so_far += point_count;
  49
  50         cvGetRow( rot_vects, &rot_vect, i );
  51         cvGetRow( trans_vects, &trans_vect, i );
  52
  53         cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,
  54                           camera_matrix, dist_coeffs, &image_points2_i,
  55                           0, 0, 0, 0, 0 );
  56         err = cvNorm( &image_points_i, &image_points2_i, CV_L1 );
  57         if( per_view_errors )
  58             per_view_errors->data.db[i] = err/point_count;
  59         total_err += err;
  60     }
  61
  62     cvReleaseMat( &image_points2 );
  63     return total_err/points_so_far;
  64 }
  65
  66
  67 int run_calibration( CvSeq* image_points_seq, CvSize img_size, CvSize board_size,
  68                      float square_size, float aspect_ratio, int flags,
  69                      CvMat* camera_matrix, CvMat* dist_coeffs, CvMat** extr_params,
  70                      CvMat** reproj_errs, double* avg_reproj_err )
  71 {
  72     int code;
  73     int image_count = image_points_seq->total;
  74     int point_count = board_size.width*board_size.height;
  75     CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2 );
  76     CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3 );
  77     CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1 );
  78     CvMat rot_vects, trans_vects;
  79     int i, j, k;
  80     CvSeqReader reader;
  81     cvStartReadSeq( image_points_seq, &reader );
  82
  83     // initialize arrays of points
  84     for( i = 0; i < image_count; i++ )
  85     {
  86         CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;
  87         CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i*point_count;
  88         CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i*point_count;
  89
  90         for( j = 0; j < board_size.height; j++ )
  91             for( k = 0; k < board_size.width; k++ )
  92             {
  93                 *obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);
  94                 *dst_img_pt++ = *src_img_pt++;
  95             }
  96         CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader );
  97     }
  98
  99     cvSet( point_counts, cvScalar(point_count) );
 100
 101     *extr_params = cvCreateMat( image_count, 6, CV_32FC1 );
 102     cvGetCols( *extr_params, &rot_vects, 0, 3 );
 103     cvGetCols( *extr_params, &trans_vects, 3, 6 );
 104
 105     cvZero( camera_matrix );
 106     cvZero( dist_coeffs );
 107
 108     if( flags & CV_CALIB_FIX_ASPECT_RATIO )
 109     {
 110         camera_matrix->data.db[0] = aspect_ratio;
 111         camera_matrix->data.db[4] = 1.;
 112     }
 113
 114     cvCalibrateCamera2( object_points, image_points, point_counts,
 115                         img_size, camera_matrix, dist_coeffs,
 116                         &rot_vects, &trans_vects, flags );
 117
 118     code = cvCheckArr( camera_matrix, CV_CHECK_QUIET ) &&
 119         cvCheckArr( dist_coeffs, CV_CHECK_QUIET ) &&
 120         cvCheckArr( *extr_params, CV_CHECK_QUIET );
 121
 122     *reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 );
 123     *avg_reproj_err =
 124         compute_reprojection_error( object_points, &rot_vects, &trans_vects,
 125             camera_matrix, dist_coeffs, image_points, point_counts, *reproj_errs );
 126
 127     cvReleaseMat( &object_points );
 128     cvReleaseMat( &image_points );
 129     cvReleaseMat( &point_counts );
 130
 131     return code;
 132 }
 133
 134
 135 void save_camera_params( const char* out_filename, int image_count, CvSize img_size,
 136                          CvSize board_size, float square_size,
 137                          float aspect_ratio, int flags,
 138                          const CvMat* camera_matrix, CvMat* dist_coeffs,
 139                          const CvMat* extr_params, const CvSeq* image_points_seq,
 140                          const CvMat* reproj_errs, double avg_reproj_err )
 141 {
 142     CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE );
 143
 144     time_t t;
 145     time( &t );
 146     struct tm *t2 = localtime( &t );
 147     char buf[1024];
 148     strftime( buf, sizeof(buf)-1, "%c", t2 );
 149
 150     cvWriteString( fs, "calibration_time", buf );
 151
 152     cvWriteInt( fs, "image_count", image_count );
 153     cvWriteInt( fs, "image_width", img_size.width );
 154     cvWriteInt( fs, "image_height", img_size.height );
 155     cvWriteInt( fs, "board_width", board_size.width );
 156     cvWriteInt( fs, "board_height", board_size.height );
 157     cvWriteReal( fs, "square_size", square_size );
 158
 159     if( flags & CV_CALIB_FIX_ASPECT_RATIO )
 160         cvWriteReal( fs, "aspect_ratio", aspect_ratio );
 161
 162     if( flags != 0 )
 163     {
 164         sprintf( buf, "flags: %s%s%s%s",
 165             flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
 166             flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspect_ratio" : "",
 167             flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
 168             flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" );
 169         cvWriteComment( fs, buf, 0 );
 170     }
 171
 172     cvWriteInt( fs, "flags", flags );
 173
 174     cvWrite( fs, "camera_matrix", camera_matrix );
 175     cvWrite( fs, "distortion_coefficients", dist_coeffs );
 176
 177     cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err );
 178     if( reproj_errs )
 179         cvWrite( fs, "per_view_reprojection_errors", reproj_errs );
 180
 181     if( extr_params )
 182     {
 183         cvWriteComment( fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
 184         cvWrite( fs, "extrinsic_parameters", extr_params );
 185     }
 186
 187     if( image_points_seq )
 188     {
 189         cvWriteComment( fs, "the array of board corners projections used for calibration", 0 );
 190         assert( image_points_seq->total == image_count );
 191         CvMat* image_points = cvCreateMat( 1, image_count*board_size.width*board_size.height, CV_32FC2 );
 192         cvCvtSeqToArray( image_points_seq, image_points->data.fl );
 193
 194         cvWrite( fs, "image_points", image_points );
 195         cvReleaseMat( &image_points );
 196     }
 197
 198     cvReleaseFileStorage( &fs );
 199 }
 200
 201
 202 int main( int argc, char** argv )
 203 {
 204     CvSize board_size = {0,0};
 205     float square_size = 1.f, aspect_ratio = 1.f;
 206     const char* out_filename = "out_camera_data.yml";
 207     const char* input_filename = 0;
 208     int i, image_count = 10;
 209     int write_extrinsics = 0, write_points = 0;
 210     int flags = 0;
 211     CvCapture* capture = 0;
 212     FILE* f = 0;
 213     char imagename[1024];
 214     CvMemStorage* storage;
 215     CvSeq* image_points_seq = 0;
 216     int elem_size, flip_vertical = 0;
 217     int delay = 1000;
 218     clock_t prev_timestamp = 0;
 219     CvPoint2D32f* image_points_buf = 0;
 220     CvFont font = cvFont( 1, 1 );
 221     double _camera[9], _dist_coeffs[4];
 222     CvMat camera = cvMat( 3, 3, CV_64F, _camera );
 223     CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs );
 224     CvMat *extr_params = 0, *reproj_errs = 0;
 225     double avg_reproj_err = 0;
 226     int mode = DETECTION;
 227     int undistort_image = 0;
 228     CvSize img_size = {0,0};
 229     const char* live_capture_help =
 230         "When the live video from camera is used as input, the following hot-keys may be used:\n"
 231             "  <ESC>, 'q' - quit the program\n"
 232             "  'g' - start capturing images\n"
 233             "  'u' - switch undistortion on/off\n";
 234
 235     if( argc < 2 )
 236     {
 237         printf( "This is a camera calibration sample.\n"
 238             "Usage: calibration\n"
 239             "     -w <board_width>         # the number of inner corners per one of board dimension\n"
 240             "     -h <board_height>        # the number of inner corners per another board dimension\n"
 241             "     [-n <number_of_frames>]  # the number of frames to use for calibration\n"
 242             "                              # (if not specified, it will be set to the number\n"
 243             "                              #  of board views actually available)\n"
 244             "     [-d <delay>]             # a minimum delay in ms between subsequent attempts to capture a next view\n"
 245             "                              # (used only for video capturing)\n"
 246             "     [-s <square_size>]       # square size in some user-defined units (1 by default)\n"
 247             "     [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
 248             "     [-op]                    # write detected feature points\n"
 249             "     [-oe]                    # write extrinsic parameters\n"
 250             "     [-zt]                    # assume zero tangential distortion\n"
 251             "     [-a <aspect_ratio>]      # fix aspect ratio (fx/fy)\n"
 252             "     [-p]                     # fix the principal point at the center\n"
 253             "     [-v]                     # flip the captured images around the horizontal axis\n"
 254             "     [input_data]             # input data, one of the following:\n"
 255             "                              #  - text file with a list of the images of the board\n"
 256             "                              #  - name of video file with a video of the board\n"
 257             "                              # if input_data not specified, a live view from the camera is used\n"
 258             "\n" );
 259         printf( "%s", live_capture_help );
 260         return 0;
 261     }
 262
 263     for( i = 1; i < argc; i++ )
 264     {
 265         const char* s = argv[i];
 266         if( strcmp( s, "-w" ) == 0 )
 267         {
 268             if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 )
 269                 return fprintf( stderr, "Invalid board width\n" ), -1;
 270         }
 271         else if( strcmp( s, "-h" ) == 0 )
 272         {
 273             if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 )
 274                 return fprintf( stderr, "Invalid board height\n" ), -1;
 275         }
 276         else if( strcmp( s, "-s" ) == 0 )
 277         {
 278             if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 )
 279                 return fprintf( stderr, "Invalid board square width\n" ), -1;
 280         }
 281         else if( strcmp( s, "-n" ) == 0 )
 282         {
 283             if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )
 284                 return printf("Invalid number of images\n" ), -1;
 285         }
 286         else if( strcmp( s, "-a" ) == 0 )
 287         {
 288             if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )
 289                 return printf("Invalid aspect ratio\n" ), -1;
 290         }
 291         else if( strcmp( s, "-d" ) == 0 )
 292         {
 293             if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )
 294                 return printf("Invalid delay\n" ), -1;
 295         }
 296         else if( strcmp( s, "-op" ) == 0 )
 297         {
 298             write_points = 1;
 299         }
 300         else if( strcmp( s, "-oe" ) == 0 )
 301         {
 302             write_extrinsics = 1;
 303         }
 304         else if( strcmp( s, "-zt" ) == 0 )
 305         {
 306             flags |= CV_CALIB_ZERO_TANGENT_DIST;
 307         }
 308         else if( strcmp( s, "-p" ) == 0 )
 309         {
 310             flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
 311         }
 312         else if( strcmp( s, "-v" ) == 0 )
 313         {
 314             flip_vertical = 1;
 315         }
 316         else if( strcmp( s, "-o" ) == 0 )
 317         {
 318             out_filename = argv[++i];
 319         }
 320         else if( s[0] != '-' )
 321             input_filename = s;
 322         else
 323             return fprintf( stderr, "Unknown option %s", s ), -1;
 324     }
 325
 326     if( input_filename )
 327     {
 328         capture = cvCreateFileCapture( input_filename );
 329         if( !capture )
 330         {
 331             f = fopen( input_filename, "rt" );
 332             if( !f )
 333                 return fprintf( stderr, "The input file could not be opened\n" ), -1;
 334             image_count = -1;
 335         }
 336         mode = CAPTURING;
 337     }
 338     else
 339         capture = cvCreateCameraCapture(0);
 340
 341     if( !capture && !f )
 342         return fprintf( stderr, "Could not initialize video capture\n" ), -2;
 343
 344     if( capture )
 345         printf( "%s", live_capture_help );
 346
 347     elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]);
 348     storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 ));
 349     image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size );
 350     image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
 351
 352     cvNamedWindow( "Image View", 1 );
 353
 354     for(;;)
 355     {
 356         IplImage *view = 0, *view_gray = 0;
 357         int count = 0, found, blink = 0;
 358         CvPoint text_origin;
 359         CvSize text_size = {0,0};
 360         int base_line = 0;
 361         char s[100];
 362         int key;
 363
 364         if( f && fgets( imagename, sizeof(imagename)-2, f ))
 365         {
 366             int l = strlen(imagename);
 367             if( l > 0 && imagename[l-1] == '\n' )
 368                 imagename[--l] = '\0';
 369             if( l > 0 )
 370             {
 371                 if( imagename[0] == '#' )
 372                     continue;
 373                 view = cvLoadImage( imagename, 1 );
 374             }
 375         }
 376         else if( capture )
 377         {
 378             IplImage* view0 = cvQueryFrame( capture );
 379             if( view0 )
 380             {
 381                 view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );
 382                 if( view0->origin == IPL_ORIGIN_BL )
 383                     cvFlip( view0, view, 0 );
 384                 else
 385                     cvCopy( view0, view );
 386             }
 387         }
 388
 389         if( !view )
 390         {
 391             if( image_points_seq->total > 0 )
 392             {
 393                 image_count = image_points_seq->total;
 394                 goto calibrate;
 395             }
 396             break;
 397         }
 398
 399         if( flip_vertical )
 400             cvFlip( view, view, 0 );
 401
 402         img_size = cvGetSize(view);
 403         found = cvFindChessboardCorners( view, board_size,
 404             image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
 405
 406 #if 1
 407         // improve the found corners' coordinate accuracy
 408         view_gray = cvCreateImage( cvGetSize(view), 8, 1 );
 409         cvCvtColor( view, view_gray, CV_BGR2GRAY );
 410         cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),
 411             cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
 412         cvReleaseImage( &view_gray );
 413 #endif
 414
 415         if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )
 416         {
 417             cvSeqPush( image_points_seq, image_points_buf );
 418             prev_timestamp = clock();
 419             blink = !f;
 420 #if 1
 421             if( capture )
 422             {
 423                 sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );
 424                 cvSaveImage( imagename, view );
 425             }
 426 #endif
 427         }
 428
 429         cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );
 430
 431         cvGetTextSize( "100/100", &font, &text_size, &base_line );
 432         text_origin.x = view->width - text_size.width - 10;
 433         text_origin.y = view->height - base_line - 10;
 434
 435         if( mode == CAPTURING )
 436         {
 437             if( image_count > 0 )
 438                 sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );
 439             else
 440                 sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );
 441         }
 442         else if( mode == CALIBRATED )
 443             sprintf( s, "Calibrated" );
 444         else
 445             sprintf( s, "Press 'g' to start" );
 446
 447         cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?
 448                                    CV_RGB(255,0,0) : CV_RGB(0,255,0));
 449
 450         if( blink )
 451             cvNot( view, view );
 452
 453         if( mode == CALIBRATED && undistort_image )
 454         {
 455             IplImage* t = cvCloneImage( view );
 456             cvUndistort2( t, view, &camera, &dist_coeffs );
 457             cvReleaseImage( &t );
 458         }
 459
 460         cvShowImage( "Image View", view );
 461         key = cvWaitKey(capture ? 50 : 500);
 462
 463         if( key == 27 )
 464             break;
 465
 466         if( key == 'u' && mode == CALIBRATED )
 467             undistort_image = !undistort_image;
 468
 469         if( capture && key == 'g' )
 470         {
 471             mode = CAPTURING;
 472             cvClearMemStorage( storage );
 473             image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
 474         }
 475
 476         if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )
 477         {
 478 calibrate:
 479             cvReleaseMat( &extr_params );
 480             cvReleaseMat( &reproj_errs );
 481             int code = run_calibration( image_points_seq, img_size, board_size,
 482                 square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params,
 483                 &reproj_errs, &avg_reproj_err );
 484             // save camera parameters in any case, to catch Inf's/NaN's
 485             save_camera_params( out_filename, image_count, img_size,
 486                 board_size, square_size, aspect_ratio, flags,
 487                 &camera, &dist_coeffs, write_extrinsics ? extr_params : 0,
 488                 write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err );
 489             if( code )
 490                 mode = CALIBRATED;
 491             else
 492                 mode = DETECTION;
 493         }
 494
 495         if( !view )
 496             break;
 497         cvReleaseImage( &view );
 498     }
 499
 500     if( capture )
 501         cvReleaseCapture( &capture );
 502     if( storage )
 503         cvReleaseMemStorage( &storage );
 504     return 0;
 505 }