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){
            cv::Mat gray;
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
            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){
            cv::Mat gray;
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
            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 (fix K0, K1, k0, k1)
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;
    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;
 
    // calibrate rotation axis 1
    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;
    }
std::cout << "Rr: " << cal.Rr << std::endl;
std::cout << "Tr: " << cal.Tr << std::endl;
 
//    //* Using rotation axis calibration *//
//    // full camera matrices
//    cv::Matx34f P0 = cal.K0*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 = cal.K1*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));
//    cv::Vec3f ez = ex.cross(axis);
 
//    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::Mat(RrMat.t());
//    cal.Tr = 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 86 → 88

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