From 654d447661a6ffda66710a5440aa57e544181de8 Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Sun, 13 Jun 2010 03:50:29 +0000 Subject: [PATCH] calibration sample rewritten using the C++ API --- samples/c/calibration.cpp | 512 ------------------------------------ samples/cpp/calibration.cpp | 467 ++++++++++++++++++++++++++++++++ 2 files changed, 467 insertions(+), 512 deletions(-) delete mode 100644 samples/c/calibration.cpp create mode 100644 samples/cpp/calibration.cpp diff --git a/samples/c/calibration.cpp b/samples/c/calibration.cpp deleted file mode 100644 index 376b659575..0000000000 --- a/samples/c/calibration.cpp +++ /dev/null @@ -1,512 +0,0 @@ -#include "cv.h" -#include "highgui.h" -#include -#include -#include - -// example command line (for copy-n-paste): -// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [] - -/* The list of views may look as following (discard the starting and ending ------ separators): -------------------- -view000.png -view001.png -#view002.png -view003.png -view010.png -one_extra_view.jpg -------------------- -that is, the file will contain 6 lines, view002.png will not be used for calibration, -other ones will be (those, in which the chessboard pattern will be found) -*/ - -enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; - -double compute_reprojection_error( const CvMat* object_points, - const CvMat* rot_vects, const CvMat* trans_vects, - const CvMat* camera_matrix, const CvMat* dist_coeffs, - const CvMat* image_points, const CvMat* point_counts, - CvMat* per_view_errors ) -{ - CvMat* image_points2 = cvCreateMat( image_points->rows, - image_points->cols, image_points->type ); - int i, image_count = rot_vects->rows, points_so_far = 0; - double total_err = 0, err; - - for( i = 0; i < image_count; i++ ) - { - CvMat object_points_i, image_points_i, image_points2_i; - int point_count = point_counts->data.i[i]; - CvMat rot_vect, trans_vect; - - cvGetCols( object_points, &object_points_i, - points_so_far, points_so_far + point_count ); - cvGetCols( image_points, &image_points_i, - points_so_far, points_so_far + point_count ); - cvGetCols( image_points2, &image_points2_i, - points_so_far, points_so_far + point_count ); - points_so_far += point_count; - - cvGetRow( rot_vects, &rot_vect, i ); - cvGetRow( trans_vects, &trans_vect, i ); - - cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect, - camera_matrix, dist_coeffs, &image_points2_i, - 0, 0, 0, 0, 0 ); - err = cvNorm( &image_points_i, &image_points2_i, CV_L1 ); - if( per_view_errors ) - per_view_errors->data.db[i] = err/point_count; - total_err += err; - } - - cvReleaseMat( &image_points2 ); - return total_err/points_so_far; -} - - -int run_calibration( CvSeq* image_points_seq, CvSize img_size, CvSize board_size, - float square_size, float aspect_ratio, int flags, - CvMat* camera_matrix, CvMat* dist_coeffs, CvMat** extr_params, - CvMat** reproj_errs, double* avg_reproj_err ) -{ - int code; - int image_count = image_points_seq->total; - int point_count = board_size.width*board_size.height; - CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2 ); - CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3 ); - CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1 ); - CvMat rot_vects, trans_vects; - int i, j, k; - CvSeqReader reader; - cvStartReadSeq( image_points_seq, &reader ); - - // initialize arrays of points - for( i = 0; i < image_count; i++ ) - { - CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr; - CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i*point_count; - CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i*point_count; - - for( j = 0; j < board_size.height; j++ ) - for( k = 0; k < board_size.width; k++ ) - { - *obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0); - *dst_img_pt++ = *src_img_pt++; - } - CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader ); - } - - cvSet( point_counts, cvScalar(point_count) ); - - *extr_params = cvCreateMat( image_count, 6, CV_32FC1 ); - cvGetCols( *extr_params, &rot_vects, 0, 3 ); - cvGetCols( *extr_params, &trans_vects, 3, 6 ); - - cvZero( camera_matrix ); - cvZero( dist_coeffs ); - - if( flags & CV_CALIB_FIX_ASPECT_RATIO ) - { - camera_matrix->data.db[0] = aspect_ratio; - camera_matrix->data.db[4] = 1.; - } - - cvCalibrateCamera2( object_points, image_points, point_counts, - img_size, camera_matrix, dist_coeffs, - &rot_vects, &trans_vects, flags ); - - code = cvCheckArr( camera_matrix, CV_CHECK_QUIET ) && - cvCheckArr( dist_coeffs, CV_CHECK_QUIET ) && - cvCheckArr( *extr_params, CV_CHECK_QUIET ); - - *reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 ); - *avg_reproj_err = - compute_reprojection_error( object_points, &rot_vects, &trans_vects, - camera_matrix, dist_coeffs, image_points, point_counts, *reproj_errs ); - - cvReleaseMat( &object_points ); - cvReleaseMat( &image_points ); - cvReleaseMat( &point_counts ); - - return code; -} - - -void save_camera_params( const char* out_filename, int image_count, CvSize img_size, - CvSize board_size, float square_size, - float aspect_ratio, int flags, - const CvMat* camera_matrix, CvMat* dist_coeffs, - const CvMat* extr_params, const CvSeq* image_points_seq, - const CvMat* reproj_errs, double avg_reproj_err ) -{ - CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE ); - - time_t t; - time( &t ); - struct tm *t2 = localtime( &t ); - char buf[1024]; - strftime( buf, sizeof(buf)-1, "%c", t2 ); - - cvWriteString( fs, "calibration_time", buf ); - - cvWriteInt( fs, "image_count", image_count ); - cvWriteInt( fs, "image_width", img_size.width ); - cvWriteInt( fs, "image_height", img_size.height ); - cvWriteInt( fs, "board_width", board_size.width ); - cvWriteInt( fs, "board_height", board_size.height ); - cvWriteReal( fs, "square_size", square_size ); - - if( flags & CV_CALIB_FIX_ASPECT_RATIO ) - cvWriteReal( fs, "aspect_ratio", aspect_ratio ); - - if( flags != 0 ) - { - sprintf( buf, "flags: %s%s%s%s", - flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "", - flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspect_ratio" : "", - flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "", - flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" ); - cvWriteComment( fs, buf, 0 ); - } - - cvWriteInt( fs, "flags", flags ); - - cvWrite( fs, "camera_matrix", camera_matrix ); - cvWrite( fs, "distortion_coefficients", dist_coeffs ); - - cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err ); - if( reproj_errs ) - cvWrite( fs, "per_view_reprojection_errors", reproj_errs ); - - if( extr_params ) - { - cvWriteComment( fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); - cvWrite( fs, "extrinsic_parameters", extr_params ); - } - - if( image_points_seq ) - { - cvWriteComment( fs, "the array of board corners projections used for calibration", 0 ); - assert( image_points_seq->total == image_count ); - CvMat* image_points = cvCreateMat( 1, image_count*board_size.width*board_size.height, CV_32FC2 ); - cvCvtSeqToArray( image_points_seq, image_points->data.fl ); - - cvWrite( fs, "image_points", image_points ); - cvReleaseMat( &image_points ); - } - - cvReleaseFileStorage( &fs ); -} - - -int main( int argc, char** argv ) -{ - CvSize board_size = {0,0}; - float square_size = 1.f, aspect_ratio = 1.f; - const char* out_filename = "out_camera_data.yml"; - const char* input_filename = 0; - int i, image_count = 10; - int write_extrinsics = 0, write_points = 0; - int flags = 0; - CvCapture* capture = 0; - FILE* f = 0; - char imagename[1024]; - CvMemStorage* storage; - CvSeq* image_points_seq = 0; - int elem_size, flip_vertical = 0; - int delay = 1000; - clock_t prev_timestamp = 0; - CvPoint2D32f* image_points_buf = 0; - CvFont font = cvFont( 1, 1 ); - double _camera[9], _dist_coeffs[4]; - CvMat camera = cvMat( 3, 3, CV_64F, _camera ); - CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs ); - CvMat *extr_params = 0, *reproj_errs = 0; - double avg_reproj_err = 0; - int mode = DETECTION; - int undistort_image = 0; - CvSize img_size = {0,0}; - int cameraId = 0; - - const char* live_capture_help = - "When the live video from camera is used as input, the following hot-keys may be used:\n" - " , 'q' - quit the program\n" - " 'g' - start capturing images\n" - " 'u' - switch undistortion on/off\n"; - - if( argc < 2 ) - { - printf( "This is a camera calibration sample.\n" - "Usage: calibration\n" - " -w # the number of inner corners per one of board dimension\n" - " -h # the number of inner corners per another board dimension\n" - " [-n ] # the number of frames to use for calibration\n" - " # (if not specified, it will be set to the number\n" - " # of board views actually available)\n" - " [-d ] # a minimum delay in ms between subsequent attempts to capture a next view\n" - " # (used only for video capturing)\n" - " [-s ] # square size in some user-defined units (1 by default)\n" - " [-o ] # the output filename for intrinsic [and extrinsic] parameters\n" - " [-op] # write detected feature points\n" - " [-oe] # write extrinsic parameters\n" - " [-zt] # assume zero tangential distortion\n" - " [-a ] # fix aspect ratio (fx/fy)\n" - " [-p] # fix the principal point at the center\n" - " [-v] # flip the captured images around the horizontal axis\n" - " [input_data] # input data, one of the following:\n" - " # - text file with a list of the images of the board\n" - " # - name of video file with a video of the board\n" - " # if input_data not specified, a live view from the camera is used\n" - "\n" ); - printf( "%s", live_capture_help ); - return 0; - } - - for( i = 1; i < argc; i++ ) - { - const char* s = argv[i]; - if( strcmp( s, "-w" ) == 0 ) - { - if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 ) - return fprintf( stderr, "Invalid board width\n" ), -1; - } - else if( strcmp( s, "-h" ) == 0 ) - { - if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 ) - return fprintf( stderr, "Invalid board height\n" ), -1; - } - else if( strcmp( s, "-s" ) == 0 ) - { - if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 ) - return fprintf( stderr, "Invalid board square width\n" ), -1; - } - else if( strcmp( s, "-n" ) == 0 ) - { - if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 ) - return printf("Invalid number of images\n" ), -1; - } - else if( strcmp( s, "-a" ) == 0 ) - { - if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 ) - return printf("Invalid aspect ratio\n" ), -1; - } - else if( strcmp( s, "-d" ) == 0 ) - { - if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 ) - return printf("Invalid delay\n" ), -1; - } - else if( strcmp( s, "-op" ) == 0 ) - { - write_points = 1; - } - else if( strcmp( s, "-oe" ) == 0 ) - { - write_extrinsics = 1; - } - else if( strcmp( s, "-zt" ) == 0 ) - { - flags |= CV_CALIB_ZERO_TANGENT_DIST; - } - else if( strcmp( s, "-p" ) == 0 ) - { - flags |= CV_CALIB_FIX_PRINCIPAL_POINT; - } - else if( strcmp( s, "-v" ) == 0 ) - { - flip_vertical = 1; - } - else if( strcmp( s, "-o" ) == 0 ) - { - out_filename = argv[++i]; - } - else if( s[0] != '-' ) - { - if( isdigit(s[0]) ) - sscanf(s, "%d", &cameraId); - else - input_filename = s; - } - else - return fprintf( stderr, "Unknown option %s", s ), -1; - } - - if( input_filename ) - { - capture = cvCreateFileCapture( input_filename ); - if( !capture ) - { - f = fopen( input_filename, "rt" ); - if( !f ) - return fprintf( stderr, "The input file could not be opened\n" ), -1; - image_count = -1; - } - mode = CAPTURING; - } - else - capture = cvCreateCameraCapture(cameraId); - - if( !capture && !f ) - return fprintf( stderr, "Could not initialize video capture\n" ), -2; - - if( capture ) - printf( "%s", live_capture_help ); - - elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]); - storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 )); - image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size ); - image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage ); - - cvNamedWindow( "Image View", 1 ); - - for(;;) - { - IplImage *view = 0, *view_gray = 0; - int count = 0, found, blink = 0; - CvPoint text_origin; - CvSize text_size = {0,0}; - int base_line = 0; - char s[100]; - int key; - - if( f && fgets( imagename, sizeof(imagename)-2, f )) - { - int l = strlen(imagename); - if( l > 0 && imagename[l-1] == '\n' ) - imagename[--l] = '\0'; - if( l > 0 ) - { - if( imagename[0] == '#' ) - continue; - view = cvLoadImage( imagename, 1 ); - } - } - else if( capture ) - { - IplImage* view0 = cvQueryFrame( capture ); - if( view0 ) - { - view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels ); - if( view0->origin == IPL_ORIGIN_BL ) - cvFlip( view0, view, 0 ); - else - cvCopy( view0, view ); - } - } - - if( !view ) - { - if( image_points_seq->total > 0 ) - { - image_count = image_points_seq->total; - goto calibrate; - } - break; - } - - if( flip_vertical ) - cvFlip( view, view, 0 ); - - img_size = cvGetSize(view); - found = cvFindChessboardCorners( view, board_size, - image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH ); - -#if 1 - // improve the found corners' coordinate accuracy - view_gray = cvCreateImage( cvGetSize(view), 8, 1 ); - cvCvtColor( view, view_gray, CV_BGR2GRAY ); - cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11), - cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); - cvReleaseImage( &view_gray ); -#endif - - if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) ) - { - cvSeqPush( image_points_seq, image_points_buf ); - prev_timestamp = clock(); - blink = !f; -#if 1 - if( capture ) - { - sprintf( imagename, "view%03d.png", image_points_seq->total - 1 ); - cvSaveImage( imagename, view ); - } -#endif - } - - cvDrawChessboardCorners( view, board_size, image_points_buf, count, found ); - - cvGetTextSize( "100/100", &font, &text_size, &base_line ); - text_origin.x = view->width - text_size.width - 10; - text_origin.y = view->height - base_line - 10; - - if( mode == CAPTURING ) - { - if( image_count > 0 ) - sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count ); - else - sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 ); - } - else if( mode == CALIBRATED ) - sprintf( s, "Calibrated" ); - else - sprintf( s, "Press 'g' to start" ); - - cvPutText( view, s, text_origin, &font, mode != CALIBRATED ? - CV_RGB(255,0,0) : CV_RGB(0,255,0)); - - if( blink ) - cvNot( view, view ); - - if( mode == CALIBRATED && undistort_image ) - { - IplImage* t = cvCloneImage( view ); - cvUndistort2( t, view, &camera, &dist_coeffs ); - cvReleaseImage( &t ); - } - - cvShowImage( "Image View", view ); - key = cvWaitKey(capture ? 50 : 500); - - if( key == 27 ) - break; - - if( key == 'u' && mode == CALIBRATED ) - undistort_image = !undistort_image; - - if( capture && key == 'g' ) - { - mode = CAPTURING; - cvClearMemStorage( storage ); - image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage ); - } - - if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count ) - { -calibrate: - cvReleaseMat( &extr_params ); - cvReleaseMat( &reproj_errs ); - int code = run_calibration( image_points_seq, img_size, board_size, - square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params, - &reproj_errs, &avg_reproj_err ); - // save camera parameters in any case, to catch Inf's/NaN's - save_camera_params( out_filename, image_count, img_size, - board_size, square_size, aspect_ratio, flags, - &camera, &dist_coeffs, write_extrinsics ? extr_params : 0, - write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err ); - if( code ) - mode = CALIBRATED; - else - mode = DETECTION; - } - - if( !view ) - break; - cvReleaseImage( &view ); - } - - if( capture ) - cvReleaseCapture( &capture ); - if( storage ) - cvReleaseMemStorage( &storage ); - return 0; -} diff --git a/samples/cpp/calibration.cpp b/samples/cpp/calibration.cpp new file mode 100644 index 0000000000..b929fbfbaf --- /dev/null +++ b/samples/cpp/calibration.cpp @@ -0,0 +1,467 @@ +#include "opencv2/core/core.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/calib3d/calib3d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include +#include +#include + +using namespace cv; +using namespace std; + +// example command line (for copy-n-paste): +// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [] + +/* The list of views may look as following (discard the starting and ending ------ separators): +------------------- +view000.png +view001.png +#view002.png +view003.png +view010.png +one_extra_view.jpg +------------------- +that is, the file will contain 6 lines, view002.png will not be used for calibration, +other ones will be (those, in which the chessboard pattern will be found) +*/ + +enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; + +static double computeReprojectionErrors( + const vector >& objectPoints, + const vector >& imagePoints, + const vector& rvecs, const vector& tvecs, + const Mat& cameraMatrix, const Mat& distCoeffs, + vector& perViewErrors ) +{ + vector imagePoints2; + int i, totalPoints = 0; + double totalErr = 0, err; + perViewErrors.resize(objectPoints.size()); + + for( i = 0; i < (int)objectPoints.size(); i++ ) + { + projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], + cameraMatrix, distCoeffs, imagePoints2); + err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L1 ); + int n = (int)objectPoints[i].size(); + perViewErrors[i] = err/n; + totalErr += err; + totalPoints += n; + } + + return totalErr/totalPoints; +} + +static void calcChessboardCorners(Size boardSize, float squareSize, vector& corners) +{ + corners.resize(0); + + for( int i = 0; i < boardSize.height; i++ ) + for( int j = 0; j < boardSize.width; j++ ) + corners.push_back(Point3f(float(j*squareSize), + float(i*squareSize), 0)); +} + +static bool runCalibration( vector > imagePoints, + Size imageSize, Size boardSize, + float squareSize, float aspectRatio, + int flags, Mat& cameraMatrix, Mat& distCoeffs, + vector& rvecs, vector& tvecs, + vector& reprojErrs, + double& totalAvgErr) +{ + cameraMatrix = Mat::eye(3, 3, CV_64F); + if( flags & CV_CALIB_FIX_ASPECT_RATIO ) + cameraMatrix.at(0,0) = aspectRatio; + + distCoeffs = Mat::zeros(5, 1, CV_64F); + + vector > objectPoints(1); + calcChessboardCorners(boardSize, squareSize, objectPoints[0]); + for( size_t i = 1; i < imagePoints.size(); i++ ) + objectPoints.push_back(objectPoints[0]); + + calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, + distCoeffs, rvecs, tvecs, flags); + + bool ok = checkRange( cameraMatrix, CV_CHECK_QUIET ) && + checkRange( distCoeffs, CV_CHECK_QUIET ); + + totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, + rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs); + + return ok; +} + + +void saveCameraParams( const string& filename, + Size imageSize, Size boardSize, + float squareSize, float aspectRatio, int flags, + const Mat& cameraMatrix, const Mat& distCoeffs, + const vector& rvecs, const vector& tvecs, + const vector& reprojErrs, + const vector >& imagePoints, + double totalAvgErr ) +{ + FileStorage fs( filename, FileStorage::WRITE ); + + time_t t; + time( &t ); + struct tm *t2 = localtime( &t ); + char buf[1024]; + strftime( buf, sizeof(buf)-1, "%c", t2 ); + + fs << "calibration_time" << buf; + + if( !rvecs.empty() || !reprojErrs.empty() ) + fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size()); + fs << "image_width" << imageSize.width; + fs << "image_height" << imageSize.height; + fs << "board_width" << boardSize.width; + fs << "board_height" << boardSize.height; + fs << "squareSize" << squareSize; + + if( flags & CV_CALIB_FIX_ASPECT_RATIO ) + fs << "aspectRatio" << aspectRatio; + + if( flags != 0 ) + { + sprintf( buf, "flags: %s%s%s%s", + flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "", + flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "", + flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "", + flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" ); + cvWriteComment( *fs, buf, 0 ); + } + + fs << "flags" << flags; + + fs << "camera_matrix" << cameraMatrix; + fs << "distortion_coefficients" << distCoeffs; + + fs << "avg_reprojection_error" << totalAvgErr; + if( !reprojErrs.empty() ) + fs << "per_view_reprojection_errors" << Mat(reprojErrs); + + if( !rvecs.empty() && !tvecs.empty() ) + { + Mat bigmat(rvecs.size(), 6, CV_32F); + for( size_t i = 0; i < rvecs.size(); i++ ) + { + Mat r = bigmat(Range(i, i+1), Range(0,3)); + Mat t = bigmat(Range(i, i+1), Range(3,6)); + rvecs[i].copyTo(r); + tvecs[i].copyTo(t); + } + cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); + fs << "extrinsic_parameters" << bigmat; + } + + if( !imagePoints.empty() ) + { + Mat imagePtMat(imagePoints.size(), imagePoints[0].size(), CV_32FC2); + for( size_t i = 0; i < imagePoints.size(); i++ ) + { + Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols); + Mat(imagePoints[i]).copyTo(r); + } + fs << "image_points" << imagePtMat; + } +} + +bool readStringList( const string& filename, vector& l ) +{ + l.resize(0); + FILE* f = fopen(filename.c_str(), "rt"); + if(!f) + return false; + + for(;;) + { + char buf[1000]; + if( !fgets( buf, sizeof(buf)-2, f )) + break; + char* ptr = strchr(buf, '\n'); + if( ptr ) *ptr = '\0'; + if( buf[0] != '\0' && buf[0] != '#' ) + l.push_back(string(buf)); + } + fclose(f); + return true; +} + + +bool runAndSave(const string& outputFilename, + const vector >& imagePoints, + Size imageSize, Size boardSize, float squareSize, + float aspectRatio, int flags, Mat& cameraMatrix, + Mat& distCoeffs, bool writeExtrinsics, bool writePoints ) +{ + vector rvecs, tvecs; + vector reprojErrs; + double totalAvgErr = 0; + + bool ok = runCalibration(imagePoints, imageSize, boardSize, squareSize, + aspectRatio, flags, cameraMatrix, distCoeffs, + rvecs, tvecs, reprojErrs, totalAvgErr); + printf("%s. avg reprojection error = %.2f\n", + ok ? "Calibration succeeded" : "Calibration failed", + totalAvgErr); + + if( ok ) + saveCameraParams( outputFilename, imageSize, + boardSize, squareSize, aspectRatio, + flags, cameraMatrix, distCoeffs, + writeExtrinsics ? rvecs : vector(), + writeExtrinsics ? tvecs : vector(), + writeExtrinsics ? reprojErrs : vector(), + writePoints ? imagePoints : vector >(), + totalAvgErr ); + return ok; +} + + +int main( int argc, char** argv ) +{ + Size boardSize, imageSize; + float squareSize = 1.f, aspectRatio = 1.f; + Mat cameraMatrix, distCoeffs; + const char* outputFilename = "out_camera_data.yml"; + const char* inputFilename = 0; + + int i, nframes = 10; + bool writeExtrinsics = false, writePoints = false; + bool undistortImage = false; + int flags = 0; + VideoCapture capture; + bool flipVertical = false; + int delay = 1000; + clock_t prevTimestamp = 0; + int mode = DETECTION; + int cameraId = 0; + vector > imagePoints; + vector imageList; + + const char* liveCaptureHelp = + "When the live video from camera is used as input, the following hot-keys may be used:\n" + " , 'q' - quit the program\n" + " 'g' - start capturing images\n" + " 'u' - switch undistortion on/off\n"; + + if( argc < 2 ) + { + printf( "This is a camera calibration sample.\n" + "Usage: calibration\n" + " -w # the number of inner corners per one of board dimension\n" + " -h # the number of inner corners per another board dimension\n" + " [-n ] # the number of frames to use for calibration\n" + " # (if not specified, it will be set to the number\n" + " # of board views actually available)\n" + " [-d ] # a minimum delay in ms between subsequent attempts to capture a next view\n" + " # (used only for video capturing)\n" + " [-s ] # square size in some user-defined units (1 by default)\n" + " [-o ] # the output filename for intrinsic [and extrinsic] parameters\n" + " [-op] # write detected feature points\n" + " [-oe] # write extrinsic parameters\n" + " [-zt] # assume zero tangential distortion\n" + " [-a ] # fix aspect ratio (fx/fy)\n" + " [-p] # fix the principal point at the center\n" + " [-v] # flip the captured images around the horizontal axis\n" + " [input_data] # input data, one of the following:\n" + " # - text file with a list of the images of the board\n" + " # - name of video file with a video of the board\n" + " # if input_data not specified, a live view from the camera is used\n" + "\n" ); + printf( "%s", liveCaptureHelp ); + return 0; + } + + for( i = 1; i < argc; i++ ) + { + const char* s = argv[i]; + if( strcmp( s, "-w" ) == 0 ) + { + if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 ) + return fprintf( stderr, "Invalid board width\n" ), -1; + } + else if( strcmp( s, "-h" ) == 0 ) + { + if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 ) + return fprintf( stderr, "Invalid board height\n" ), -1; + } + else if( strcmp( s, "-s" ) == 0 ) + { + if( sscanf( argv[++i], "%f", &squareSize ) != 1 || squareSize <= 0 ) + return fprintf( stderr, "Invalid board square width\n" ), -1; + } + else if( strcmp( s, "-n" ) == 0 ) + { + if( sscanf( argv[++i], "%u", &nframes ) != 1 || nframes <= 3 ) + return printf("Invalid number of images\n" ), -1; + } + else if( strcmp( s, "-a" ) == 0 ) + { + if( sscanf( argv[++i], "%f", &aspectRatio ) != 1 || aspectRatio <= 0 ) + return printf("Invalid aspect ratio\n" ), -1; + } + else if( strcmp( s, "-d" ) == 0 ) + { + if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 ) + return printf("Invalid delay\n" ), -1; + } + else if( strcmp( s, "-op" ) == 0 ) + { + writePoints = 1; + } + else if( strcmp( s, "-oe" ) == 0 ) + { + writeExtrinsics = 1; + } + else if( strcmp( s, "-zt" ) == 0 ) + { + flags |= CV_CALIB_ZERO_TANGENT_DIST; + } + else if( strcmp( s, "-p" ) == 0 ) + { + flags |= CV_CALIB_FIX_PRINCIPAL_POINT; + } + else if( strcmp( s, "-v" ) == 0 ) + { + flipVertical = 1; + } + else if( strcmp( s, "-o" ) == 0 ) + { + outputFilename = argv[++i]; + } + else if( s[0] != '-' ) + { + if( isdigit(s[0]) ) + sscanf(s, "%d", &cameraId); + else + inputFilename = s; + } + else + return fprintf( stderr, "Unknown option %s", s ), -1; + } + + if( inputFilename ) + { + capture.open(inputFilename); + if( !capture.isOpened() && !readStringList(inputFilename, imageList) ) + { + fprintf( stderr, "The input file could not be opened\n" ); + return -1; + } + mode = CAPTURING; + } + else + capture.open(cameraId); + + if( !capture.isOpened() && imageList.empty() ) + return fprintf( stderr, "Could not initialize video capture\n" ), -2; + + if( !imageList.empty() ) + nframes = (int)imageList.size(); + + if( capture.isOpened() ) + printf( "%s", liveCaptureHelp ); + + namedWindow( "Image View", 1 ); + + for(i = 0;;i++) + { + Mat view, viewGray; + bool blink = false; + + if( capture.isOpened() ) + { + Mat view0; + capture >> view0; + view0.copyTo(view); + } + else if( i < (int)imageList.size() ) + view = imread(imageList[i], 1); + + if(!view.data) + { + if( imagePoints.size() > 0 ) + runAndSave(outputFilename, imagePoints, imageSize, + boardSize, squareSize, aspectRatio, + flags, cameraMatrix, distCoeffs, + writeExtrinsics, writePoints); + break; + } + + imageSize = view.size(); + + if( flipVertical ) + flip( view, view, 0 ); + + vector pointbuf; + bool found = findChessboardCorners( view, boardSize, pointbuf, CV_CALIB_CB_ADAPTIVE_THRESH ); + + // improve the found corners' coordinate accuracy + cvtColor(view, viewGray, CV_BGR2GRAY); + cornerSubPix( viewGray, pointbuf, Size(11,11), + Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); + + if( mode == CAPTURING && found && + (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) ) + { + imagePoints.push_back(pointbuf); + prevTimestamp = clock(); + blink = capture.isOpened(); + } + + drawChessboardCorners( view, boardSize, Mat(pointbuf), found ); + + string msg = mode == CAPTURING ? "100/100" : + mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; + int baseLine = 0; + Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); + Point textOrigin(view.cols - textSize.width - 10, view.rows - baseLine - 10); + + if( mode == CAPTURING ) + msg = format( "%d/%d", (int)imagePoints.size(), nframes ); + + putText( view, msg, textOrigin, 1, 1, + mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0)); + + if( blink ) + bitwise_not(view, view); + + if( mode == CALIBRATED && undistortImage ) + { + Mat temp = view.clone(); + undistort(temp, view, cameraMatrix, distCoeffs); + } + + imshow("Image View", view); + int key = waitKey(capture.isOpened() ? 50 : 500); + + if( (key & 255) == 27 ) + break; + + if( key == 'u' && mode == CALIBRATED ) + undistortImage = !undistortImage; + + if( capture.isOpened() && key == 'g' ) + { + mode = CAPTURING; + imagePoints.clear(); + } + + if( mode == CAPTURING && imagePoints.size() >= (unsigned)nframes ) + { + if( runAndSave(outputFilename, imagePoints, imageSize, + boardSize, squareSize, aspectRatio, + flags, cameraMatrix, distCoeffs, + writeExtrinsics, writePoints)) + mode = CALIBRATED; + else + mode = DETECTION; + } + } + return 0; +}