WebSVN – seema-scanner – Diff – /src/SMCalibrationWorker.cpp


#include "SMCalibrationWorker.h"
#include "SMCalibrationParameters.h"
 
#include "cvtools.h"
 
#include <QSettings>
 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
 
    QSettings settings;
 
    // Number of saddle points on calibration pattern
    int checkerCountX = settings.value("calibration/checkerCountX", 22).toInt();
    int checkerCountY = settings.value("calibration/checkerCountY", 13).toInt();
    cv::Size checkerCount(checkerCountX, checkerCountY);
 
    int nSets = calibrationData.size();
 
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
    std::vector<float> angles;
 
    // Loop through calibration sets
    for(int i=0; i<nSets; i++){
 
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
 
        if(!SMCalibrationSetI.checked)
            continue;
 
        // Camera 0
        std::vector<cv::Point2f> qci0;
        // Extract checker corners
        bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success0){
            // Convert bayer to grayscale
            cv::Mat gray;
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);
 
            cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
            // Draw colored chessboard
            SMCalibrationSetI.frame0Result = SMCalibrationSetI.frame0.clone();
            cvtools::drawChessboardCorners(SMCalibrationSetI.frame0Result, checkerCount, qci0, success0, 10);
        }
 
        emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
 
        // Camera 1
        std::vector<cv::Point2f> qci1;
        // Extract checker corners
        bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success1){
            // Convert bayer to grayscale
            cv::Mat gray;
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);
            cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
            // Draw colored chessboard
            SMCalibrationSetI.frame1Result = SMCalibrationSetI.frame1.clone();
            cvtools::drawChessboardCorners(SMCalibrationSetI.frame1Result, checkerCount, qci1, success1, 10);
        }
 
        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
 
        SMCalibrationSetI.success = success0 && success1;
 
        // Add to whole set
        if(SMCalibrationSetI.success){
            qc0.push_back(qci0);
            qc1.push_back(qci1);
            angles.push_back(SMCalibrationSetI.rotationAngle);
        }
 
        // Show progress
        emit newSetProcessed(i);
    }
 
    int nValidSets = qc0.size();
    if(nValidSets < 2){
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
        emit done();
        return;
    }
 
    // Generate world object coordinates [mm]
    float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm
    std::vector<cv::Point3f> Qi;
    for (int h=0; h<checkerCount.height; h++)
        for (int w=0; w<checkerCount.width; w++)
            Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
    std::vector< std::vector<cv::Point3f> > Q;
    for(int i=0; i<qc0.size(); i++)
        Q.push_back(Qi);
 
    // calibrate the cameras
    SMCalibrationParameters cal;
    cal.frameWidth = calibrationData[0].frame0.cols;
    cal.frameHeight = calibrationData[0].frame0.rows;
    cv::Size frameSize(cal.frameWidth, cal.frameHeight);
 
    // determine only k1, k2 for lens distortion
    int flags = cv::CALIB_FIX_K3;
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
 
    // refine extrinsics for camera 0
    for(int i=0; i<Q.size(); i++)
        cv::solvePnPRansac(Q[i], qc0[i], cal.K0, cal.k0, cam_rvecs0[i], cam_tvecs0[i], true, 100, 0.05, 100, cv::noArray(), CV_ITERATIVE);
 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
 
    // stereo calibration
    int flags_stereo = cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_FIX_K3;
    cv::Mat E, F, R1, T1;
    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
                                              frameSize, R1, T1, E, F,
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
                                              flags_stereo);
 
    cal.R1 = R1;
    cal.T1 = T1;
    cal.E = E;
    cal.F = F;
 
//    // hand-eye calibration
//    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame
//    std::vector<cv::Vec3f> Tc(nValidSets - 1);
//    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
//    std::vector<cv::Vec3f> Tr(nValidSets - 1);
//    for(int i=0; i<nValidSets-1; i++){
//        // relative transformations in camera
//        cv::Mat cRw1, cRw2;
//        cv::Rodrigues(cam_rvecs0[i], cRw1);
//        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
//        cv::Mat cTw1 = cam_tvecs0[i];
//        cv::Mat cTw2 = cam_tvecs0[i+1];
//        cv::Mat w1Rc = cRw1.t();
//        cv::Mat w1Tc = -cRw1.t()*cTw1;
//        Rc[i] = cv::Mat(cRw2*w1Rc);
//        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);
 
//        // relative transformations in rotation stage
//        // we define the rotation axis to be in origo, pointing in positive y direction
//        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;
//        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
//        cv::Mat Rri;
//        cv::Rodrigues(rot_rvec, Rri);
//        Rr[i] = Rri;
//        Tr[i] = 0.0;
 
////        std::cout << i << std::endl;
////        std::cout << "cTw1" << cTw1 << std::endl;
////        std::cout << "cTw2" << cTw2 << std::endl;
////        std::cout << "w2Rc" << w2Rc << std::endl;
////        std::cout << "w2Tc" << w2Tc << std::endl;
 
////        std::cout << "w2Rc" << w2Rc << std::endl;
////        std::cout << "w2Tc" << w2Tc << std::endl;
 
////        cv::Mat Rci;
////        cv::Rodrigues(Rc[i], Rci);
////        std::cout << "Rci" << Rci << std::endl;
////        std::cout << "Tc[i]" << Tc[i] << std::endl;
 
////        std::cout << "rot_rvec" << rot_rvec << std::endl;
////        std::cout << "Tr[i]" << Tr[i] << std::endl;
////        std::cout << std::endl;
//    }
 
//    // determine the transformation from rotation stage to camera 0
//    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
 
//    for(int i=0; i<nValidSets-1; i++){
//        std::cout << i << std::endl;
 
//        cv::Mat Rci;
//        cv::Rodrigues(Rc[i], Rci);
//        std::cout << "Rc[i]" << Rci << std::endl;
//        std::cout << "Tc[i]" << Tc[i] << std::endl;
 
//        cv::Mat Rri;
//        cv::Rodrigues(Rr[i], Rri);
//        std::cout << "Rr[i]" << Rri << std::endl;
//        std::cout << "Tr[i]" << Tr[i] << std::endl;
 
//        cv::Mat Rcr = cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t());
//        cv::Rodrigues(Rcr, Rcr);
//        cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
//        std::cout << "Rcr[i]" << Rcr << std::endl;
//        std::cout << "Tcr[i]" << Tcr << std::endl;
//        std::cout << std::endl;
//    }
 
 
    // Direct rotation axis calibration //
    // full camera matrices
    cv::Matx34f P0 = cv::Matx34f::eye();
    cv::Mat RT1(3, 4, CV_32F);
    cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
    cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
    cv::Matx34f P1 = cv::Matx34f(RT1);
 
    // calibration points in camera 0 frame
    std::vector< std::vector<cv::Point3f> > Qcam;
 
    for(int i=0; i<nValidSets; i++){
        std::vector<cv::Point2f> qc0i, qc1i;
 
        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);
//        qc0i = qc0[i];
//        qc1i = qc1[i];
 
        cv::Mat Qhom, Qcami;
        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
        std::vector<cv::Point3f> QcamiPoints;
        cvtools::matToPoints3f(Qcami, QcamiPoints);
 
        Qcam.push_back(QcamiPoints);
    }
 
    cv::Vec3f axis, point;
    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);
 
    // construct transformation matrix
    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
    ex = cv::normalize(ex);
    cv::Vec3f ez = ex.cross(axis);
    ez = cv::normalize(ez);
 
    cv::Mat RrMat(3, 3, CV_32F);
    cv::Mat(ex).copyTo(RrMat.col(0));
    cv::Mat(axis).copyTo(RrMat.col(1));
    cv::Mat(ez).copyTo(RrMat.col(2));
 
    cal.Rr = cv::Matx33f(RrMat).t();
    cal.Tr = -cv::Matx33f(RrMat).t()*point;
 
    // Print to std::cout
    cal.print();
 
    // save to (reentrant qsettings object)
    settings.setValue("calibration/parameters", QVariant::fromValue(cal));
 
    emit done();
 
}
 

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 113 → 114

Rev 113		Rev 114
1	`#include "SMCalibrationWorker.h"`	1	`#include "SMCalibrationWorker.h"`
2	`#include "SMCalibrationParameters.h"`	2	`#include "SMCalibrationParameters.h"`
3		3
4	`#include "cvtools.h"`	4	`#include "cvtools.h"`
5		5
6	`#include <QSettings>`	6	`#include <QSettings>`
7		7
8	`void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){`	8	`void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){`
9		9
10	`QSettings settings;`	10	`QSettings settings;`
11		11
12	`// Number of saddle points on calibration pattern`	12	`// Number of saddle points on calibration pattern`
13	`int checkerCountX = settings.value("calibration/checkerCountX", 22).toInt();`	13	`int checkerCountX = settings.value("calibration/checkerCountX", 22).toInt();`
14	`int checkerCountY = settings.value("calibration/checkerCountY", 13).toInt();`	14	`int checkerCountY = settings.value("calibration/checkerCountY", 13).toInt();`
15	`cv::Size checkerCount(checkerCountX, checkerCountY);`	15	`cv::Size checkerCount(checkerCountX, checkerCountY);`
16		16
17	`int nSets = calibrationData.size();`	17	`int nSets = calibrationData.size();`
18		18
19	`std::vector< std::vector<cv::Point2f> > qc0, qc1;`	19	`std::vector< std::vector<cv::Point2f> > qc0, qc1;`
20	`std::vector<float> angles;`	20	`std::vector<float> angles;`
21		21
22	`// Loop through calibration sets`	22	`// Loop through calibration sets`
23	`for(int i=0; i<nSets; i++){`	23	`for(int i=0; i<nSets; i++){`
24		24
25	`SMCalibrationSet SMCalibrationSetI = calibrationData[i];`	25	`SMCalibrationSet SMCalibrationSetI = calibrationData[i];`
26		26
27	`if(!SMCalibrationSetI.checked)`	27	`if(!SMCalibrationSetI.checked)`
28	`continue;`	28	`continue;`
29		29
30	`// Camera 0`	30	`// Camera 0`
31	`std::vector<cv::Point2f> qci0;`	31	`std::vector<cv::Point2f> qci0;`
32	`// Extract checker corners`	32	`// Extract checker corners`
33	`bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`	33	`bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`
34	`if(success0){`	34	`if(success0){`
35	`// Convert bayer to grayscale`	35	`// Convert bayer to grayscale`
36	`cv::Mat gray;`	36	`cv::Mat gray;`
37	`cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);`	37	`cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);`
38	`cv::cvtColor(gray, gray, CV_RGB2GRAY);`	-
39	`cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));`	38	`cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));`
40	`// Draw colored chessboard`	39	`// Draw colored chessboard`
41	`SMCalibrationSetI.frame0Result = SMCalibrationSetI.frame0.clone();`	40	`SMCalibrationSetI.frame0Result = SMCalibrationSetI.frame0.clone();`
42	`cvtools::drawChessboardCorners(SMCalibrationSetI.frame0Result, checkerCount, qci0, success0, 10);`	41	`cvtools::drawChessboardCorners(SMCalibrationSetI.frame0Result, checkerCount, qci0, success0, 10);`
43	`}`	42	`}`
44		43
45	`emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);`	44	`emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);`
46		45
47	`// Camera 1`	46	`// Camera 1`
48	`std::vector<cv::Point2f> qci1;`	47	`std::vector<cv::Point2f> qci1;`
49	`// Extract checker corners`	48	`// Extract checker corners`
50	`bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`	49	`bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`
51	`if(success1){`	50	`if(success1){`
52	`// Convert bayer to grayscale`	51	`// Convert bayer to grayscale`
53	`cv::Mat gray;`	52	`cv::Mat gray;`
54	`cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);`	53	`cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);`
55	`cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));`	54	`cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));`
56	`// Draw colored chessboard`	55	`// Draw colored chessboard`
57	`SMCalibrationSetI.frame1Result = SMCalibrationSetI.frame1.clone();`	56	`SMCalibrationSetI.frame1Result = SMCalibrationSetI.frame1.clone();`
58	`cvtools::drawChessboardCorners(SMCalibrationSetI.frame1Result, checkerCount, qci1, success1, 10);`	57	`cvtools::drawChessboardCorners(SMCalibrationSetI.frame1Result, checkerCount, qci1, success1, 10);`
59	`}`	58	`}`
60		59
61	`emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);`	60	`emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);`
62		61
63	`SMCalibrationSetI.success = success0 && success1;`	62	`SMCalibrationSetI.success = success0 && success1;`
64		63
65	`// Add to whole set`	64	`// Add to whole set`
66	`if(SMCalibrationSetI.success){`	65	`if(SMCalibrationSetI.success){`
67	`qc0.push_back(qci0);`	66	`qc0.push_back(qci0);`
68	`qc1.push_back(qci1);`	67	`qc1.push_back(qci1);`
69	`angles.push_back(SMCalibrationSetI.rotationAngle);`	68	`angles.push_back(SMCalibrationSetI.rotationAngle);`
70	`}`	69	`}`
71		70
72	`// Show progress`	71	`// Show progress`
73	`emit newSetProcessed(i);`	72	`emit newSetProcessed(i);`
74	`}`	73	`}`
75		74
76	`int nValidSets = qc0.size();`	75	`int nValidSets = qc0.size();`
77	`if(nValidSets < 2){`	76	`if(nValidSets < 2){`
78	`std::cerr << "Not enough valid calibration sequences!" << std::endl;`	77	`std::cerr << "Not enough valid calibration sequences!" << std::endl;`
79	`emit done();`	78	`emit done();`
80	`return;`	79	`return;`
81	`}`	80	`}`
82		81
83	`// Generate world object coordinates [mm]`	82	`// Generate world object coordinates [mm]`
84	`float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm`	83	`float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm`
85	`std::vector<cv::Point3f> Qi;`	84	`std::vector<cv::Point3f> Qi;`
86	`for (int h=0; h<checkerCount.height; h++)`	85	`for (int h=0; h<checkerCount.height; h++)`
87	`for (int w=0; w<checkerCount.width; w++)`	86	`for (int w=0; w<checkerCount.width; w++)`
88	`Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));`	87	`Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));`
89	`std::vector< std::vector<cv::Point3f> > Q;`	88	`std::vector< std::vector<cv::Point3f> > Q;`
90	`for(int i=0; i<qc0.size(); i++)`	89	`for(int i=0; i<qc0.size(); i++)`
91	`Q.push_back(Qi);`	90	`Q.push_back(Qi);`
92		91
93	`// calibrate the cameras`	92	`// calibrate the cameras`
94	`SMCalibrationParameters cal;`	93	`SMCalibrationParameters cal;`
95	`cal.frameWidth = calibrationData[0].frame0.cols;`	94	`cal.frameWidth = calibrationData[0].frame0.cols;`
96	`cal.frameHeight = calibrationData[0].frame0.rows;`	95	`cal.frameHeight = calibrationData[0].frame0.rows;`
97	`cv::Size frameSize(cal.frameWidth, cal.frameHeight);`	96	`cv::Size frameSize(cal.frameWidth, cal.frameHeight);`
98		97
99	`// determine only k1, k2 for lens distortion`	98	`// determine only k1, k2 for lens distortion`
100	`int flags = cv::CALIB_FIX_K3;`	99	`int flags = cv::CALIB_FIX_K3;`
101	`// Note: several of the output arguments below must be cv::Mat, otherwise segfault`	100	`// Note: several of the output arguments below must be cv::Mat, otherwise segfault`
102	`std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;`	101	`std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;`
103	`cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);`	102	`cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);`
104		103
105	`// refine extrinsics for camera 0`	104	`// refine extrinsics for camera 0`
106	`for(int i=0; i<Q.size(); i++)`	105	`for(int i=0; i<Q.size(); i++)`
107	`cv::solvePnPRansac(Q[i], qc0[i], cal.K0, cal.k0, cam_rvecs0[i], cam_tvecs0[i], true, 100, 0.05, 100, cv::noArray(), CV_ITERATIVE);`	106	`cv::solvePnPRansac(Q[i], qc0[i], cal.K0, cal.k0, cam_rvecs0[i], cam_tvecs0[i], true, 100, 0.05, 100, cv::noArray(), CV_ITERATIVE);`
108		107
109	`std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;`	108	`std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;`
110	`cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);`	109	`cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);`
111		110
112	`// stereo calibration`	111	`// stereo calibration`
113	`int flags_stereo = cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_FIX_K3;`	112	`int flags_stereo = cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_FIX_K3;`
114	`cv::Mat E, F, R1, T1;`	113	`cv::Mat E, F, R1, T1;`
115	`cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,`	114	`cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,`
116	`frameSize, R1, T1, E, F,`	115	`frameSize, R1, T1, E, F,`
117	`cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),`	116	`cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),`
118	`flags_stereo);`	117	`flags_stereo);`
119		118
120	`cal.R1 = R1;`	119	`cal.R1 = R1;`
121	`cal.T1 = T1;`	120	`cal.T1 = T1;`
122	`cal.E = E;`	121	`cal.E = E;`
123	`cal.F = F;`	122	`cal.F = F;`
124		123
125	`// // hand-eye calibration`	124	`// // hand-eye calibration`
126	`// std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame`	125	`// std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame`
127	`// std::vector<cv::Vec3f> Tc(nValidSets - 1);`	126	`// std::vector<cv::Vec3f> Tc(nValidSets - 1);`
128	`// std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame`	127	`// std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame`
129	`// std::vector<cv::Vec3f> Tr(nValidSets - 1);`	128	`// std::vector<cv::Vec3f> Tr(nValidSets - 1);`
130	`// for(int i=0; i<nValidSets-1; i++){`	129	`// for(int i=0; i<nValidSets-1; i++){`
131	`// // relative transformations in camera`	130	`// // relative transformations in camera`
132	`// cv::Mat cRw1, cRw2;`	131	`// cv::Mat cRw1, cRw2;`
133	`// cv::Rodrigues(cam_rvecs0[i], cRw1);`	132	`// cv::Rodrigues(cam_rvecs0[i], cRw1);`
134	`// cv::Rodrigues(cam_rvecs0[i+1], cRw2);`	133	`// cv::Rodrigues(cam_rvecs0[i+1], cRw2);`
135	`// cv::Mat cTw1 = cam_tvecs0[i];`	134	`// cv::Mat cTw1 = cam_tvecs0[i];`
136	`// cv::Mat cTw2 = cam_tvecs0[i+1];`	135	`// cv::Mat cTw2 = cam_tvecs0[i+1];`
137	`// cv::Mat w1Rc = cRw1.t();`	136	`// cv::Mat w1Rc = cRw1.t();`
138	`// cv::Mat w1Tc = -cRw1.t()*cTw1;`	137	`// cv::Mat w1Tc = -cRw1.t()*cTw1;`
139	`// Rc[i] = cv::Mat(cRw2*w1Rc);`	138	`// Rc[i] = cv::Mat(cRw2*w1Rc);`
140	`// Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);`	139	`// Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);`
141		140
142	`// // relative transformations in rotation stage`	141	`// // relative transformations in rotation stage`
143	`// // we define the rotation axis to be in origo, pointing in positive y direction`	142	`// // we define the rotation axis to be in origo, pointing in positive y direction`
144	`// float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;`	143	`// float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;`
145	`// cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`	144	`// cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`
146	`// cv::Mat Rri;`	145	`// cv::Mat Rri;`
147	`// cv::Rodrigues(rot_rvec, Rri);`	146	`// cv::Rodrigues(rot_rvec, Rri);`
148	`// Rr[i] = Rri;`	147	`// Rr[i] = Rri;`
149	`// Tr[i] = 0.0;`	148	`// Tr[i] = 0.0;`
150		149
151	`//// std::cout << i << std::endl;`	150	`//// std::cout << i << std::endl;`
152	`//// std::cout << "cTw1" << cTw1 << std::endl;`	151	`//// std::cout << "cTw1" << cTw1 << std::endl;`
153	`//// std::cout << "cTw2" << cTw2 << std::endl;`	152	`//// std::cout << "cTw2" << cTw2 << std::endl;`
154	`//// std::cout << "w2Rc" << w2Rc << std::endl;`	153	`//// std::cout << "w2Rc" << w2Rc << std::endl;`
155	`//// std::cout << "w2Tc" << w2Tc << std::endl;`	154	`//// std::cout << "w2Tc" << w2Tc << std::endl;`
156		155
157	`//// std::cout << "w2Rc" << w2Rc << std::endl;`	156	`//// std::cout << "w2Rc" << w2Rc << std::endl;`
158	`//// std::cout << "w2Tc" << w2Tc << std::endl;`	157	`//// std::cout << "w2Tc" << w2Tc << std::endl;`
159		158
160	`//// cv::Mat Rci;`	159	`//// cv::Mat Rci;`
161	`//// cv::Rodrigues(Rc[i], Rci);`	160	`//// cv::Rodrigues(Rc[i], Rci);`
162	`//// std::cout << "Rci" << Rci << std::endl;`	161	`//// std::cout << "Rci" << Rci << std::endl;`
163	`//// std::cout << "Tc[i]" << Tc[i] << std::endl;`	162	`//// std::cout << "Tc[i]" << Tc[i] << std::endl;`
164		163
165	`//// std::cout << "rot_rvec" << rot_rvec << std::endl;`	164	`//// std::cout << "rot_rvec" << rot_rvec << std::endl;`
166	`//// std::cout << "Tr[i]" << Tr[i] << std::endl;`	165	`//// std::cout << "Tr[i]" << Tr[i] << std::endl;`
167	`//// std::cout << std::endl;`	166	`//// std::cout << std::endl;`
168	`// }`	167	`// }`
169		168
170	`// // determine the transformation from rotation stage to camera 0`	169	`// // determine the transformation from rotation stage to camera 0`
171	`// cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);`	170	`// cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);`
172		171
173	`// for(int i=0; i<nValidSets-1; i++){`	172	`// for(int i=0; i<nValidSets-1; i++){`
174	`// std::cout << i << std::endl;`	173	`// std::cout << i << std::endl;`
175		174
176	`// cv::Mat Rci;`	175	`// cv::Mat Rci;`
177	`// cv::Rodrigues(Rc[i], Rci);`	176	`// cv::Rodrigues(Rc[i], Rci);`
178	`// std::cout << "Rc[i]" << Rci << std::endl;`	177	`// std::cout << "Rc[i]" << Rci << std::endl;`
179	`// std::cout << "Tc[i]" << Tc[i] << std::endl;`	178	`// std::cout << "Tc[i]" << Tc[i] << std::endl;`
180		179
181	`// cv::Mat Rri;`	180	`// cv::Mat Rri;`
182	`// cv::Rodrigues(Rr[i], Rri);`	181	`// cv::Rodrigues(Rr[i], Rri);`
183	`// std::cout << "Rr[i]" << Rri << std::endl;`	182	`// std::cout << "Rr[i]" << Rri << std::endl;`
184	`// std::cout << "Tr[i]" << Tr[i] << std::endl;`	183	`// std::cout << "Tr[i]" << Tr[i] << std::endl;`
185		184
186	`// cv::Mat Rcr = cv::Mat(cal.Rr)cv::Mat(Rc[i])cv::Mat(cal.Rr.t());`	185	`// cv::Mat Rcr = cv::Mat(cal.Rr)cv::Mat(Rc[i])cv::Mat(cal.Rr.t());`
187	`// cv::Rodrigues(Rcr, Rcr);`	186	`// cv::Rodrigues(Rcr, Rcr);`
188	`// cv::Mat Tcr = -cv::Mat(cal.Rr)cv::Mat(Rc[i])cv::Mat(cal.Rr.t())cv::Mat(cal.Tr) + cv::Mat(cal.Rr)cv::Mat(Tc[i]) + cv::Mat(cal.Tr);`	187	`// cv::Mat Tcr = -cv::Mat(cal.Rr)cv::Mat(Rc[i])cv::Mat(cal.Rr.t())cv::Mat(cal.Tr) + cv::Mat(cal.Rr)cv::Mat(Tc[i]) + cv::Mat(cal.Tr);`
189	`// std::cout << "Rcr[i]" << Rcr << std::endl;`	188	`// std::cout << "Rcr[i]" << Rcr << std::endl;`
190	`// std::cout << "Tcr[i]" << Tcr << std::endl;`	189	`// std::cout << "Tcr[i]" << Tcr << std::endl;`
191	`// std::cout << std::endl;`	190	`// std::cout << std::endl;`
192	`// }`	191	`// }`
193		192
194		193
195	`// Direct rotation axis calibration //`	194	`// Direct rotation axis calibration //`
196	`// full camera matrices`	195	`// full camera matrices`
197	`cv::Matx34f P0 = cv::Matx34f::eye();`	196	`cv::Matx34f P0 = cv::Matx34f::eye();`
198	`cv::Mat RT1(3, 4, CV_32F);`	197	`cv::Mat RT1(3, 4, CV_32F);`
199	`cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));`	198	`cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));`
200	`cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));`	199	`cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));`
201	`cv::Matx34f P1 = cv::Matx34f(RT1);`	200	`cv::Matx34f P1 = cv::Matx34f(RT1);`
202		201
203	`// calibration points in camera 0 frame`	202	`// calibration points in camera 0 frame`
204	`std::vector< std::vector<cv::Point3f> > Qcam;`	203	`std::vector< std::vector<cv::Point3f> > Qcam;`
205		204
206	`for(int i=0; i<nValidSets; i++){`	205	`for(int i=0; i<nValidSets; i++){`
207	`std::vector<cv::Point2f> qc0i, qc1i;`	206	`std::vector<cv::Point2f> qc0i, qc1i;`
208		207
209	`cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);`	208	`cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);`
210	`cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);`	209	`cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);`
211	`// qc0i = qc0[i];`	210	`// qc0i = qc0[i];`
212	`// qc1i = qc1[i];`	211	`// qc1i = qc1[i];`
213		212
214	`cv::Mat Qhom, Qcami;`	213	`cv::Mat Qhom, Qcami;`
215	`cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);`	214	`cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);`
216	`cvtools::convertMatFromHomogeneous(Qhom, Qcami);`	215	`cvtools::convertMatFromHomogeneous(Qhom, Qcami);`
217	`std::vector<cv::Point3f> QcamiPoints;`	216	`std::vector<cv::Point3f> QcamiPoints;`
218	`cvtools::matToPoints3f(Qcami, QcamiPoints);`	217	`cvtools::matToPoints3f(Qcami, QcamiPoints);`
219		218
220	`Qcam.push_back(QcamiPoints);`	219	`Qcam.push_back(QcamiPoints);`
221	`}`	220	`}`
222		221
223	`cv::Vec3f axis, point;`	222	`cv::Vec3f axis, point;`
224	`cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);`	223	`cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);`
225		224
226	`// construct transformation matrix`	225	`// construct transformation matrix`
227	`cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));`	226	`cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));`
228	`ex = cv::normalize(ex);`	227	`ex = cv::normalize(ex);`
229	`cv::Vec3f ez = ex.cross(axis);`	228	`cv::Vec3f ez = ex.cross(axis);`
230	`ez = cv::normalize(ez);`	229	`ez = cv::normalize(ez);`
231		230
232	`cv::Mat RrMat(3, 3, CV_32F);`	231	`cv::Mat RrMat(3, 3, CV_32F);`
233	`cv::Mat(ex).copyTo(RrMat.col(0));`	232	`cv::Mat(ex).copyTo(RrMat.col(0));`
234	`cv::Mat(axis).copyTo(RrMat.col(1));`	233	`cv::Mat(axis).copyTo(RrMat.col(1));`
235	`cv::Mat(ez).copyTo(RrMat.col(2));`	234	`cv::Mat(ez).copyTo(RrMat.col(2));`
236		235
237	`cal.Rr = cv::Matx33f(RrMat).t();`	236	`cal.Rr = cv::Matx33f(RrMat).t();`
238	`cal.Tr = -cv::Matx33f(RrMat).t()*point;`	237	`cal.Tr = -cv::Matx33f(RrMat).t()*point;`
239		238
240	`// Print to std::cout`	239	`// Print to std::cout`
241	`cal.print();`	240	`cal.print();`
242		241
243	`// save to (reentrant qsettings object)`	242	`// save to (reentrant qsettings object)`
244	`settings.setValue("calibration/parameters", QVariant::fromValue(cal));`	243	`settings.setValue("calibration/parameters", QVariant::fromValue(cal));`
245		244
246	`emit done();`	245	`emit done();`
247		246
248	`}`	247	`}`
249		248