/**
 * OpenCV camera calibration utility.
 * 
 * Based on OpenCV 2.3.1 calibration.cpp.
 *
 * Modifications by Marsette Vona:
 *
 * - option -mo specifies model offset transform (takes pts in chessboard frame to model frame, def identity)
 *
 * - option -cm fixes camera matrix, enables switch from calibrateCamera() to solvePnP()
 *
 * - option -oe also outputs camera extrinsics in model frame
 *
 * - option -zr assumes zero radial distortions
 *
 * - documented camera index cmd line option and added optional stream index
 *
 * - options -cw and -ch set capture width and height in pixels
 *
 * - make 'q' work to quit, as advertised
 *
 * - changed min num images to 1
 *
 **/

#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;

/**
 * vona
 *
 * Format ITEMS as a string.
 *
 * ITEMS is a standard cout-style << sequence, e.g.
 *
 * std::string s = FMT(19<<std::hex<<" 0x"<<19<<" float "<<19.2);
 **/
#define FMT(ITEMS) \
  ((dynamic_cast<ostringstream &> (ostringstream()<<flush<<ITEMS)).str())

const char * usage =
" \nexample command line for calibration from a live feed.\n"
"   calibration -w 4 -h 5 -s 0.025 -o camera.yml -op -oe\n"
" \n"
" example command line for calibration from a list of stored images:\n"
//"   imagelist_creator image_list.xml *.png\n"
"   calibration -w 4 -h 5 -s 0.025 -o camera.yml -op -oe image_list.xml\n"
" where image_list.xml is the standard OpenCV XML/YAML\n"
//" use imagelist_creator to create the xml or yaml list\n"
" file consisting of the list of strings, e.g.:\n"
" \n"
"<?xml version=\"1.0\"?>\n"
"<opencv_storage>\n"
"<images>\n"
"view000.png\n"
"view001.png\n"
"<!-- view002.png -->\n"
"view003.png\n"
"view010.png\n"
"one_extra_view.jpg\n"
"</images>\n"
"</opencv_storage>\n";




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"
        "  'u' - switch undistortion on/off\n";

void help()
{
    printf( "This is a camera calibration sample.\n"
        "Usage: calibration\n"
        "     -w <board_width>         # the number of inner corners per one of board dimension\n"
        "     -h <board_height>        # the number of inner corners per another board dimension\n"
        "     -cw <cam_width>          # width in pixels for live camera capture\n" //vona
        "     -ch <cam_height>         # height in pixels for live camera capture\n" //vona
        "     [-pt <pattern>]          # the type of pattern: chessboard or circles' grid\n"
        "     [-n <number_of_frames>]  # 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 <delay>]             # a minimum delay in ms between subsequent attempts to capture a next view\n"
        "                              # (used only for video capturing)\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"
//vona
        "     [-mo <rx ry rz tx ty tz>] # specify model offset transform taking pts in chessboard frame to model frame\n"
        "     [-cm <fx fy cx cy>]      # specify camera matrix in pixels\n"
        "     [-oe]                    # write extrinsic parameters\n"
        "     [-zt]                    # assume zero tangential distortion\n"
        "     [-zr]                    # assume zero radial distortion\n" //vona
        "     [-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"
        "     [-V]                     # use a video file, and not an image list, uses\n"
        "                              # [input_data] string for the video file name\n"
        "     [-su]                    # show undistorted images after calibration\n"
        "     [input_data]             # input data, one of the following:\n"
        "                              #  - text file with a list of the images of the board\n"
        "                              #    see below for an example\n"
//        "                              #    the text file can be generated with imagelist_creator\n"
        "                              #  - name of video file with a video of the board\n"
//vona        "                              # if input_data not specified, a live view from the camera is used\n"
        "                              #  - I[:S] where I is camera index and S is stream index\n" //vona
        "\n" );
    printf("\n%s",usage);
    printf( "\n%s", liveCaptureHelp );
}

enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };

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 )
{
    vector<Point2f> 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_L2);
        int n = (int)objectPoints[i].size();
        perViewErrors[i] = (float)std::sqrt(err*err/n);
        totalErr += err*err;
        totalPoints += n;
    }
    
    return std::sqrt(totalErr/totalPoints);
}

static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
{
    corners.resize(0);
    
    switch(patternType)
    {
      case CHESSBOARD:
      case CIRCLES_GRID:
        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));
        break;

      case ASYMMETRIC_CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f(float((2*j + i % 2)*squareSize),
                                          float(i*squareSize), 0));
        break;

      default:
        CV_Error(CV_StsBadArg, "Unknown pattern type\n");
    }
}

static bool runCalibration( vector<vector<Point2f> > imagePoints,
                    Size imageSize, Size boardSize, Pattern patternType,
                    float squareSize, float aspectRatio,
                    int flags, Mat& cameraMatrix, Mat& distCoeffs,
                    vector<Mat>& rvecs, vector<Mat>& tvecs,
                    vector<float>& reprojErrs,
//vona                    double& totalAvgErr)
                    double& totalAvgErr, bool fixCM)
{
  /* vona
    cameraMatrix = Mat::eye(3, 3, CV_64F);
    if( flags & CV_CALIB_FIX_ASPECT_RATIO )
        cameraMatrix.at<double>(0,0) = aspectRatio;
  */
   
    distCoeffs = Mat::zeros(8, 1, CV_64F);
    
    vector<vector<Point3f> > objectPoints(1);
    calcChessboardCorners(boardSize, squareSize, objectPoints[0], patternType);

    objectPoints.resize(imagePoints.size(),objectPoints[0]);
   
    if ((flags & (CV_CALIB_ZERO_TANGENT_DIST|CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3)) &&
        fixCM && (cameraMatrix.size() == Size(3,3))) { //vona
     
      printf("**intrinsics fixed, using solvePnP for extrinsics**\n");
      int n = imagePoints.size();
      rvecs.resize(n); tvecs.resize(n);
      for (int i = 0; i < n; i++) solvePnP(objectPoints[i], imagePoints[i], cameraMatrix, distCoeffs, rvecs[i], tvecs[i]);

    } else {

      cameraMatrix = Mat::eye(3, 3, CV_64F);
      if( flags & CV_CALIB_FIX_ASPECT_RATIO )
        cameraMatrix.at<double>(0,0) = aspectRatio;

      double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
                                   distCoeffs, rvecs, tvecs, flags|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
                                                              ///*|CV_CALIB_FIX_K3*/|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
      printf("RMS error reported by calibrateCamera: %g\n", rms);
    }
    
    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
    
    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<Mat>& rvecs, const vector<Mat>& tvecs,
                       const vector<float>& reprojErrs,
                       const Mat& modelOffset, //vona
                       const vector<vector<Point2f> >& 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 << "square_size" << squareSize;
    
    if( flags & CV_CALIB_FIX_ASPECT_RATIO )
        fs << "aspectRatio" << aspectRatio;

    if( flags != 0 )
    {
      //vona        sprintf( buf, "flags: %s%s%s%s",
      sprintf( buf, "flags: %s%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" : "",
            flags & (CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3) ?
               "+zero_radial_dist" : ""); //vona
        cvWriteComment( *fs, buf, 0 );
    }
    
    fs << "flags" << flags;

    fs << "camera_matrix" << cameraMatrix;

    //vona
    float fx = cameraMatrix.at<double>(0,0), fy = cameraMatrix.at<double>(1,1);
    cvWriteComment(*fs,FMT("fx [px] @ w="<<imageSize.width<<": "<<fx<<
                           ", @"<<800<<": "<<fx*800.0/imageSize.width<<
                           ", @"<<640<<": "<<fx*640.0/imageSize.width<<
                           ", @"<<320<<": "<<fx*320.0/imageSize.width).c_str(),0);
    cvWriteComment(*fs,FMT("fy [px] @ h="<<imageSize.height<<": "<<fy<<
                           ", @"<<600<<": "<<fy*600.0/imageSize.height<<
                           ", @"<<480<<": "<<fy*480.0/imageSize.height<<
                           ", @"<<240<<": "<<fy*240.0/imageSize.height).c_str(),0);
    float cx = cameraMatrix.at<double>(0,2), cy = cameraMatrix.at<double>(1,2);
    cvWriteComment(*fs,FMT("cx [px] @ w="<<imageSize.width<<": "<<cx<<
                           ", @"<<800<<": "<<cx*800.0/imageSize.width<<
                           ", @"<<640<<": "<<cx*640.0/imageSize.width<<
                           ", @"<<320<<": "<<cx*320.0/imageSize.width).c_str(),0);
    cvWriteComment(*fs,FMT("cy [px] @ h="<<imageSize.height<<": "<<cy<<
                           ", @"<<600<<": "<<cy*600.0/imageSize.height<<
                           ", @"<<480<<": "<<cy*480.0/imageSize.height<<
                           ", @"<<240<<": "<<cy*240.0/imageSize.height).c_str(),0);

    fs << "distortion_coefficients" << distCoeffs;

    fs << "avg_reprojection_error" << totalAvgErr;
    if( !reprojErrs.empty() )
        fs << "per_view_reprojection_errors" << Mat(reprojErrs);
    
    if( !rvecs.empty() && !tvecs.empty() )
    {
        CV_Assert(rvecs[0].type() == tvecs[0].type());
        Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
        for( int i = 0; i < (int)rvecs.size(); i++ )
        {
            Mat r = bigmat(Range(i, i+1), Range(0,3));
            Mat t = bigmat(Range(i, i+1), Range(3,6));

            CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
            CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
            //*.t() is MatExpr (not Mat) so we can use assignment operator
            r = rvecs[i].t();
            t = tvecs[i].t();
        }
        cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
        fs << "extrinsic_parameters" << bigmat;

        //vona

        fs << "model_offset" << modelOffset;

        Mat rMB(3,3,CV_32FC1), tMB(3,1,CV_32FC1); //rotmat & transvec model<-chessboard
        Rodrigues(modelOffset(Range(0,3), Range(0,1)), rMB); tMB = modelOffset(Range(3,6), Range(0,1));
        //        fs << "board_to_model_r" << rMB; fs << "board_to_model_t" << tMB; 

        Mat rvecCB(3,1,CV_32FC1), rCB(3,3,CV_32FC1), tCB(3,1,CV_32FC1); //rotvec, rotmat & transvec camera<-chessboard
        Mat rMC(3,3,CV_32FC1), tMC(3,1,CV_32FC1); //rotmat & transvec model<-camera
        Mat MC = Mat::eye(4,4,CV_32FC1); //homogenous transform model<-camera

        for( int i = 0; i < (int)rvecs.size(); i++ ) {
          rvecs[i].convertTo(rvecCB,CV_32F); Rodrigues(rvecCB, rCB); tvecs[i].convertTo(tCB,CV_32F);
          //          fs << "board_to_camera_r" << rCB; fs << "board_to_camera_t" << tCB; 
          rMC = (rMB*rCB.t()); tMC = -rMB*rCB.t()*tCB+tMB; //(model<-cam) = (model<-board)(cam<-board)^{-1}
          //          fs << "camera_to_model_r" << rMC; fs << "camera_to_model_t" << tMC; 
          MC(Range(0,3),Range(0,3)) = rMC*1; MC(Range(0,3),Range(3,4)) = tMC*1; //*1 makes MatExpr for assignment
          fs << FMT("camera_to_model_"<<i) << MC;
          cvWriteComment(*fs,FMT("cam "<<i<<" C (center) in model: "<<tMC).c_str(),0);
          cvWriteComment(*fs,FMT("cam "<<i<<" A (z axis) in model: "<<rMC(Range(0,3),Range(2,3))).c_str(),0);
          cvWriteComment(*fs,FMT("cam "<<i<<" H (x axis) in model: "<<rMC(Range(0,3),Range(0,1))).c_str(),0);
          cvWriteComment(*fs,FMT("cam "<<i<<" V (y axis) in model: "<<rMC(Range(0,3),Range(1,2))).c_str(),0);
          cvWriteComment(*fs,FMT("camera frame is right handed with z out, x right, and y down in image").c_str(),0);
        }
    }
    
    if( !imagePoints.empty() )
    {
        Mat imagePtMat((int)imagePoints.size(), imagePoints[0].size(), CV_32FC2);
        for( int i = 0; i < (int)imagePoints.size(); i++ )
        {
            Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
            Mat imgpti(imagePoints[i]);
            imgpti.copyTo(r);
        }
        fs << "image_points" << imagePtMat;
    }
}

static bool readStringList( const string& filename, vector<string>& l )
{
    l.resize(0);
    FileStorage fs(filename, FileStorage::READ);
    if( !fs.isOpened() )
        return false;
    FileNode n = fs.getFirstTopLevelNode();
    if( n.type() != FileNode::SEQ )
        return false;
    FileNodeIterator it = n.begin(), it_end = n.end();
    for( ; it != it_end; ++it )
        l.push_back((string)*it);
    return true;
}


bool runAndSave(const string& outputFilename,
                const vector<vector<Point2f> >& imagePoints,
                Size imageSize, Size boardSize, Pattern patternType, float squareSize,
                float aspectRatio, int flags, Mat& cameraMatrix,
//vona                Mat& distCoeffs, bool writeExtrinsics, bool writePoints )
                Mat& distCoeffs, bool writeExtrinsics, Mat& modelOffset, bool writePoints, bool fixCM )
{
    vector<Mat> rvecs, tvecs;
    vector<float> reprojErrs;
    double totalAvgErr = 0;
    
    bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize,
                   aspectRatio, flags, cameraMatrix, distCoeffs,
                   rvecs, tvecs, reprojErrs, totalAvgErr, fixCM);
    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>(),
                         modelOffset, //vona
                         writePoints ? imagePoints : vector<vector<Point2f> >(),
                         totalAvgErr );
    return ok;
}


int main( int argc, char** argv )
{
  //vona    Size boardSize, imageSize;
    Size boardSize(-1,-1), imageSize(-1,-1);
    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;
    bool showUndistorted = false;
    bool videofile = false;
    int delay = 1000;
    clock_t prevTimestamp = 0;
    int mode = DETECTION;
    int cameraId = 0;
    int streamId = 0; //vona
    vector<vector<Point2f> > imagePoints;
    vector<string> imageList;
    Pattern pattern = CHESSBOARD;

    Mat modelOffset(6,1,CV_32FC1,Scalar(0)); bool fixCM = false; //vona

    /* vona    if( argc < 2 )
    {
        help();
        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, "-cw" ) == 0 ) //vona
        {
            if( sscanf( argv[++i], "%u", &imageSize.width ) != 1 || imageSize.width <= 0 )
                return fprintf( stderr, "Invalid camera width\n" ), -1;
        }
        else if( strcmp( s, "-ch" ) == 0 ) //vona
        {
            if( sscanf( argv[++i], "%u", &imageSize.height ) != 1 || imageSize.height <= 0 )
                return fprintf( stderr, "Invalid camera height\n" ), -1;
        }
        else if( strcmp( s, "-pt" ) == 0 )
        {
            i++;
            if( !strcmp( argv[i], "circles" ) )
                pattern = CIRCLES_GRID;
            else if( !strcmp( argv[i], "acircles" ) )
                pattern = ASYMMETRIC_CIRCLES_GRID;
            else if( !strcmp( argv[i], "chessboard" ) )
                pattern = CHESSBOARD;
            else
                return fprintf( stderr, "Invalid pattern type: must be chessboard or circles\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 )
        {
          //vona            if( sscanf( argv[++i], "%u", &nframes ) != 1 || nframes <= 3 )
          if( sscanf( argv[++i], "%u", &nframes ) != 1 || nframes <= 0 )
                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;
            flags |= CV_CALIB_FIX_ASPECT_RATIO;
        }
        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 = true;
        }
        else if( strcmp( s, "-mo" ) == 0 ) //vona
        {
          for (int j = 0; j < 6; j++)
            if( sscanf( argv[++i], "%f", modelOffset.ptr<float>(j)) != 1 )
              return printf("Invalid model offset transform\n" ), -1;
        }
        else if( strcmp( s, "-cm" ) == 0 ) //vona
        {
          float fx, fy, cx, cy;
          if( sscanf( argv[++i], "%f", &fx) != 1 ) return printf("Invalid fx\n" ), -1;
          if( sscanf( argv[++i], "%f", &fy) != 1 ) return printf("Invalid fy\n" ), -1;
          if( sscanf( argv[++i], "%f", &cx) != 1 ) return printf("Invalid cx\n" ), -1;
          if( sscanf( argv[++i], "%f", &cy) != 1 ) return printf("Invalid cy\n" ), -1;
          cameraMatrix = Mat::eye(3, 3, CV_64F);
          cameraMatrix.at<double>(0,0) = fx; cameraMatrix.at<double>(1,1) = fy;
          cameraMatrix.at<double>(0,2) = cx; cameraMatrix.at<double>(1,2) = cy;
          fixCM = true;
        }
        else if( strcmp( s, "-oe" ) == 0 )
        {
            writeExtrinsics = true;
        }
        else if( strcmp( s, "-zt" ) == 0 )
        {
            flags |= CV_CALIB_ZERO_TANGENT_DIST;
        }
        else if( strcmp( s, "-zr" ) == 0 ) //vona
        {
            flags |= CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3;
        }
        else if( strcmp( s, "-p" ) == 0 )
        {
            flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
        }
        else if( strcmp( s, "-v" ) == 0 )
        {
            flipVertical = true;
        }
        else if( strcmp( s, "-V" ) == 0 )
        {
            videofile = true;
        }
        else if( strcmp( s, "-o" ) == 0 )
        {
            outputFilename = argv[++i];
        }
        else if( strcmp( s, "-su" ) == 0 )
        {
            showUndistorted = true;
        }
        else if( s[0] != '-' )
        {
            if( isdigit(s[0]) )
              //vona                sscanf(s, "%d", &cameraId);
              sscanf(s, "%d:%d", &cameraId, &streamId);
            else
                inputFilename = s;
        }
        else
            return fprintf( stderr, "Unknown option %s", s ), -1;
    }

    if((boardSize.width <= 0) || (boardSize.height <= 0)) //vona
    {
      help();
      return 0;
    }
    
    if( inputFilename )
    {
      if( !videofile && readStringList(inputFilename, imageList) ) {
            mode = CAPTURING;
            printf("taking input images from list file \"%s\"\n", inputFilename); //vona
      }
      else     {
            capture.open(inputFilename);
            printf("taking input images from video file \"%s\"\n", inputFilename); //vona
      }
    }
    else {
        capture.open(cameraId);
        printf("taking %dx%d input images from camera %d stream %d\n",
               imageSize.width, imageSize.height, cameraId, streamId); //vona
        if(imageSize.width >= 0) capture.set(CV_CAP_PROP_FRAME_WIDTH, imageSize.width); //vona
        if(imageSize.height >= 0) capture.set(CV_CAP_PROP_FRAME_HEIGHT, imageSize.height); //vona
    }

    if( !capture.isOpened() && imageList.empty() )
        return fprintf( stderr, "Could not initialize video (%d) capture\n",cameraId ), -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;
            //vona            capture >> view0;
            capture.grab(); capture.retrieve(view0, streamId);
            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, pattern, squareSize, aspectRatio,
                           flags, cameraMatrix, distCoeffs,
//vona                           writeExtrinsics, writePoints);
                           writeExtrinsics, modelOffset, writePoints, fixCM);
            break;
        }
        
        imageSize = view.size();

        if( flipVertical )
            flip( view, view, 0 );

        vector<Point2f> pointbuf;
        cvtColor(view, viewGray, CV_BGR2GRAY); 

        bool found;
        switch( pattern )
        {
            case CHESSBOARD:
                found = findChessboardCorners( view, boardSize, pointbuf,
                                               CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
                break;
            case CIRCLES_GRID:
                found = findCirclesGrid( view, boardSize, pointbuf );
                break;
            case ASYMMETRIC_CIRCLES_GRID:
                found = findCirclesGrid( view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID );
                break;
            default:
                return fprintf( stderr, "Unknown pattern type\n" ), -1;
        }

       // improve the found corners' coordinate accuracy
        if( pattern == CHESSBOARD && found) 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();
        }
        
        if(found)
            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 - 2*textSize.width - 10, view.rows - 2*baseLine - 10);

        if( mode == CAPTURING )
        {
            if(undistortImage)
                msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes );
            else
                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 = 0xff & waitKey(capture.isOpened() ? 50 : 500);

        //vona        if( (key & 255) == 27 )
        if(( (key & 255) == 27 ) || ( capture.isOpened() && key == 'q' ))
            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, pattern, squareSize, aspectRatio,
                       flags, cameraMatrix, distCoeffs,
//vona                           writeExtrinsics, writePoints))
                           writeExtrinsics, modelOffset, writePoints, fixCM))
                mode = CALIBRATED;
            else
                mode = DETECTION;
            if( !capture.isOpened() )
                break;
        }
    }
    
    if( !capture.isOpened() && showUndistorted )
    {
        Mat view, rview, map1, map2;
        initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
                                getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
                                imageSize, CV_16SC2, map1, map2);
        
        for( i = 0; i < (int)imageList.size(); i++ )
        {
            view = imread(imageList[i], 1);
            if(!view.data)
                continue;
            //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix );
            remap(view, rview, map1, map2, INTER_LINEAR);
            imshow("Image View", rview);
            int c = 0xff & waitKey();
            if( (c & 255) == 27 || c == 'q' || c == 'Q' )
                break;
        }
    }
                                                
    return 0;
}