calibration sample rewritten using the C++ API

samples/c/calibration.cpp → samples/cpp/calibration.cpp

-#include "cv.h"
-#include "highgui.h"
+#include "opencv2/core/core.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+#include "opencv2/highgui/highgui.hpp"
 #include <stdio.h>
 #include <string.h>
 #include <time.h>
 
+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 [<list_of_views.txt>]
 
@@ -22,124 +27,84 @@ 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 )
+static double computeReprojectionErrors(
+        const vector<vector<Point3f> >& objectPoints,
+        const vector<vector<Point2f> >& imagePoints,
+        const vector<Mat>& rvecs, const vector<Mat>& tvecs,
+        const Mat& cameraMatrix, const Mat& distCoeffs,
+        vector<float>& perViewErrors )
 {
-    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;
+    vector<Point2f> imagePoints2;
+    int i, totalPoints = 0;
+    double totalErr = 0, err;
+    perViewErrors.resize(objectPoints.size());
     
-    for( i = 0; i < image_count; i++ )
+    for( i = 0; i < (int)objectPoints.size(); 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;
+        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;
     }
     
-    cvReleaseMat( &image_points2 );
-    return total_err/points_so_far;
+    return totalErr/totalPoints;
 }
 
-
-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 )
+static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
 {
-    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 );
-    }
+    corners.resize(0);
     
-    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 );
+    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<vector<Point2f> > imagePoints,
+                    Size imageSize, Size boardSize,
+                    float squareSize, float aspectRatio,
+                    int flags, Mat& cameraMatrix, Mat& distCoeffs,
+                    vector<Mat>& rvecs, vector<Mat>& tvecs,
+                    vector<float>& reprojErrs,
+                    double& totalAvgErr)
+{
+    cameraMatrix = Mat::eye(3, 3, CV_64F);
     if( flags & CV_CALIB_FIX_ASPECT_RATIO )
-    {
-        camera_matrix->data.db[0] = aspect_ratio;
-        camera_matrix->data.db[4] = 1.;
-    }
+        cameraMatrix.at<double>(0,0) = aspectRatio;
     
-    cvCalibrateCamera2( object_points, image_points, point_counts,
-                        img_size, camera_matrix, dist_coeffs,
-                        &rot_vects, &trans_vects, flags );
+    distCoeffs = Mat::zeros(5, 1, CV_64F);
     
-    code = cvCheckArr( camera_matrix, CV_CHECK_QUIET ) &&
-        cvCheckArr( dist_coeffs, CV_CHECK_QUIET ) &&
-        cvCheckArr( *extr_params, CV_CHECK_QUIET );
+    vector<vector<Point3f> > objectPoints(1);
+    calcChessboardCorners(boardSize, squareSize, objectPoints[0]);
+    for( size_t i = 1; i < imagePoints.size(); i++ )
+        objectPoints.push_back(objectPoints[0]);
     
-    *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 );
+    calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
+                    distCoeffs, rvecs, tvecs, flags);
     
-    cvReleaseMat( &object_points );
-    cvReleaseMat( &image_points );
-    cvReleaseMat( &point_counts );
+    bool ok = checkRange( cameraMatrix, CV_CHECK_QUIET ) &&
+            checkRange( distCoeffs, CV_CHECK_QUIET );
     
-    return code;
+    totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
+                rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
+
+    return ok;
 }
 
 
-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 )
+void saveCameraParams( const string& filename,
+                       Size imageSize, Size boardSize,
+                       float squareSize, float aspectRatio, int flags,
+                       const Mat& cameraMatrix, const Mat& distCoeffs,
+                       const vector<Mat>& rvecs, const vector<Mat>& tvecs,
+                       const vector<float>& reprojErrs,
+                       const vector<vector<Point2f> >& imagePoints,
+                       double totalAvgErr )
 {
-    CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE );
+    FileStorage fs( filename, FileStorage::WRITE );
     
     time_t t;
     time( &t );
@@ -147,88 +112,138 @@ void save_camera_params( const char* out_filename, int image_count, CvSize img_s
     char buf[1024];
     strftime( buf, sizeof(buf)-1, "%c", t2 );
 
-    cvWriteString( fs, "calibration_time", buf );
+    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( !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 )
-        cvWriteReal( fs, "aspect_ratio", 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_aspect_ratio" : "",
+            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 );
+        cvWriteComment( *fs, buf, 0 );
     }
     
-    cvWriteInt( fs, "flags", flags );
+    fs << "flags" << flags;
 
-    cvWrite( fs, "camera_matrix", camera_matrix );
-    cvWrite( fs, "distortion_coefficients", dist_coeffs );
+    fs << "camera_matrix" << cameraMatrix;
+    fs << "distortion_coefficients" << distCoeffs;
 
-    cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err );
-    if( reproj_errs )
-        cvWrite( fs, "per_view_reprojection_errors", reproj_errs );
+    fs << "avg_reprojection_error" << totalAvgErr;
+    if( !reprojErrs.empty() )
+        fs << "per_view_reprojection_errors" << Mat(reprojErrs);
     
-    if( extr_params )
+    if( !rvecs.empty() && !tvecs.empty() )
+    {
+        Mat bigmat(rvecs.size(), 6, CV_32F);
+        for( size_t i = 0; i < rvecs.size(); i++ )
         {
-        cvWriteComment( fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
-        cvWrite( fs, "extrinsic_parameters", extr_params );
+            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( image_points_seq )
+    if( !imagePoints.empty() )
     {
-        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 );
+        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<string>& l )
+{
+    l.resize(0);
+    FILE* f = fopen(filename.c_str(), "rt");
+    if(!f)
+        return false;
     
-        cvWrite( fs, "image_points", image_points );
-        cvReleaseMat( &image_points );
+    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;
+}
+
 
-    cvReleaseFileStorage( &fs );
+bool runAndSave(const string& outputFilename,
+                const vector<vector<Point2f> >& imagePoints,
+                Size imageSize, Size boardSize, float squareSize,
+                float aspectRatio, int flags, Mat& cameraMatrix,
+                Mat& distCoeffs, bool writeExtrinsics, bool writePoints )
+{
+    vector<Mat> rvecs, tvecs;
+    vector<float> 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<Mat>(),
+                         writeExtrinsics ? tvecs : vector<Mat>(),
+                         writeExtrinsics ? reprojErrs : vector<float>(),
+                         writePoints ? imagePoints : vector<vector<Point2f> >(),
+                         totalAvgErr );
+    return ok;
 }
 
 
 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;
+    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;
-    CvCapture* capture = 0;
-    FILE* f = 0;
-    char imagename[1024];
-    CvMemStorage* storage;
-    CvSeq* image_points_seq = 0;
-    int elem_size, flip_vertical = 0;
+    VideoCapture capture;
+    bool flipVertical = false;
     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;
+    clock_t prevTimestamp = 0;
     int mode = DETECTION;
-    int undistort_image = 0;
-    CvSize img_size = {0,0};
     int cameraId = 0;
+    vector<vector<Point2f> > imagePoints;
+    vector<string> imageList;
 
-    const char* live_capture_help = 
+    const char* liveCaptureHelp = 
         "When the live video from camera is used as input, the following hot-keys may be used:\n"
             "  <ESC>, 'q' - quit the program\n"
             "  'g' - start capturing images\n"
@@ -245,12 +260,12 @@ int main( int argc, char** argv )
             "                              #  of board views actually available)\n"
             "     [-d <delay>]             # a minimum delay in ms between subsequent attempts to capture a next view\n"
             "                              # (used only for video capturing)\n"
-            "     [-s <square_size>]       # square size in some user-defined units (1 by default)\n"
+            "     [-s <squareSize>]       # square size in some user-defined units (1 by default)\n"
             "     [-o <out_camera_params>] # 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 <aspect_ratio>]      # fix aspect ratio (fx/fy)\n"
+            "     [-a <aspectRatio>]      # 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"
@@ -258,7 +273,7 @@ int main( int argc, char** argv )
             "                              #  - 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 );
+        printf( "%s", liveCaptureHelp );
         return 0;
     }
 
@@ -267,27 +282,27 @@ int main( int argc, char** argv )
         const char* s = argv[i];
         if( strcmp( s, "-w" ) == 0 )
         {
-            if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 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", &board_size.height ) != 1 || board_size.height <= 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", &square_size ) != 1 || square_size <= 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", &image_count ) != 1 || image_count <= 3 )
+            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", &aspect_ratio ) != 1 || aspect_ratio <= 0 )
+            if( sscanf( argv[++i], "%f", &aspectRatio ) != 1 || aspectRatio <= 0 )
                 return printf("Invalid aspect ratio\n" ), -1;
         }
         else if( strcmp( s, "-d" ) == 0 )
@@ -297,11 +312,11 @@ int main( int argc, char** argv )
         }
         else if( strcmp( s, "-op" ) == 0 )
         {
-            write_points = 1;
+            writePoints = 1;
         }
         else if( strcmp( s, "-oe" ) == 0 )
         {
-            write_extrinsics = 1;
+            writeExtrinsics = 1;
         }
         else if( strcmp( s, "-zt" ) == 0 )
         {
@@ -313,200 +328,140 @@ int main( int argc, char** argv )
         }
         else if( strcmp( s, "-v" ) == 0 )
         {
-            flip_vertical = 1;
+            flipVertical = 1;
         }
         else if( strcmp( s, "-o" ) == 0 )
         {
-            out_filename = argv[++i];
+            outputFilename = argv[++i];
         }
         else if( s[0] != '-' )
         {
 			if( isdigit(s[0]) )
 				sscanf(s, "%d", &cameraId);
 			else
-				input_filename = s;
+				inputFilename = s;
 		}
         else
             return fprintf( stderr, "Unknown option %s", s ), -1;
     }
 
-    if( input_filename )
+    if( inputFilename )
     {
-        capture = cvCreateFileCapture( input_filename );
-        if( !capture )
+        capture.open(inputFilename);
+        if( !capture.isOpened() && !readStringList(inputFilename, imageList) )
         {
-            f = fopen( input_filename, "rt" );
-            if( !f )
-                return fprintf( stderr, "The input file could not be opened\n" ), -1;
-            image_count = -1;
+            fprintf( stderr, "The input file could not be opened\n" );
+            return -1;
         }
         mode = CAPTURING;
     }
     else
-        capture = cvCreateCameraCapture(cameraId);
+        capture.open(cameraId);
 
-    if( !capture && !f )
+    if( !capture.isOpened() && imageList.empty() )
         return fprintf( stderr, "Could not initialize video capture\n" ), -2;
     
-    if( capture )
-        printf( "%s", live_capture_help );
+    if( !imageList.empty() )
+        nframes = (int)imageList.size();
 
-    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 );
+    if( capture.isOpened() )
+        printf( "%s", liveCaptureHelp );
 
-    cvNamedWindow( "Image View", 1 );
+    namedWindow( "Image View", 1 );
 
-    for(;;)
+    for(i = 0;;i++)
     {
-        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 )
+        Mat view, viewGray;
+        bool blink = false;
+        
+        if( capture.isOpened() )
         {
-                view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );
-                if( view0->origin == IPL_ORIGIN_BL )
-                    cvFlip( view0, view, 0 );
-                else
-                    cvCopy( view0, view );
-            }
+            Mat view0;
+            capture >> view0;
+            view0.copyTo(view);
         }
+        else if( i < (int)imageList.size() )
+            view = imread(imageList[i], 1);
         
-        if( !view )
-        {
-            if( image_points_seq->total > 0 )
+        if(!view.data)
         {
-                image_count = image_points_seq->total;
-                goto calibrate;
-            }
+            if( imagePoints.size() > 0 )
+                runAndSave(outputFilename, imagePoints, imageSize,
+                           boardSize, squareSize, aspectRatio,
+                           flags, cameraMatrix, distCoeffs,
+                           writeExtrinsics, writePoints);
             break;
         }
         
-        if( flip_vertical )
-            cvFlip( view, view, 0 );
+        imageSize = view.size();
+
+        if( flipVertical )
+            flip( view, view, 0 );
 
-        img_size = cvGetSize(view);
-        found = cvFindChessboardCorners( view, board_size,
-            image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
+        vector<Point2f> pointbuf;
+        bool found = findChessboardCorners( view, boardSize, pointbuf, 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 )
+        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) )
         {
-                sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );
-                cvSaveImage( imagename, view );
-            }
-#endif
+            imagePoints.push_back(pointbuf);
+            prevTimestamp = clock();
+            blink = capture.isOpened();
         }
 
-        cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );
+        drawChessboardCorners( view, boardSize, Mat(pointbuf), 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;
+        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 )
-        {
-            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" );
+            msg = format( "%d/%d", (int)imagePoints.size(), nframes );
 
-        cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?
-                                   CV_RGB(255,0,0) : CV_RGB(0,255,0));
+        putText( view, msg, textOrigin, 1, 1,
+                 mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0));
 
         if( blink )
-            cvNot( view, view );
+            bitwise_not(view, view);
 
-        if( mode == CALIBRATED && undistort_image )
+        if( mode == CALIBRATED && undistortImage )
         {
-            IplImage* t = cvCloneImage( view );
-            cvUndistort2( t, view, &camera, &dist_coeffs );
-            cvReleaseImage( &t );
+            Mat temp = view.clone();
+            undistort(temp, view, cameraMatrix, distCoeffs);
         }
 
-        cvShowImage( "Image View", view );
-        key = cvWaitKey(capture ? 50 : 500);
+        imshow("Image View", view);
+        int key = waitKey(capture.isOpened() ? 50 : 500);
 
-        if( key == 27 )
+        if( (key & 255) == 27 )
             break;
         
         if( key == 'u' && mode == CALIBRATED )
-            undistort_image = !undistort_image;
+            undistortImage = !undistortImage;
 
-        if( capture && key == 'g' )
+        if( capture.isOpened() && key == 'g' )
         {
             mode = CAPTURING;
-            cvClearMemStorage( storage );
-            image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
+            imagePoints.clear();
         }
 
-        if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )
+        if( mode == CAPTURING && imagePoints.size() >= (unsigned)nframes )
         {
-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 )
+            if( runAndSave(outputFilename, imagePoints, imageSize,
+                       boardSize, squareSize, aspectRatio,
+                       flags, cameraMatrix, distCoeffs,
+                       writeExtrinsics, writePoints))
                 mode = CALIBRATED;
             else
                 mode = DETECTION;
         }
-
-        if( !view )
-            break;
-        cvReleaseImage( &view );
     }
-
-    if( capture )
-        cvReleaseCapture( &capture );
-    if( storage )
-        cvReleaseMemStorage( &storage );
     return 0;
 }