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;
 
        // Convert to grayscale
        cv::Mat gray;
        if(SMCalibrationSetI.frame0.channels() == 1)
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);
        else
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
 
        // Extract checker corners
        bool success0 = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success0){
            cv::cornerSubPix(gray, qci0, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
            // Draw colored chessboard
            cv::Mat color;
            if(SMCalibrationSetI.frame0.channels() == 1)
                cv::cvtColor(SMCalibrationSetI.frame0, color, CV_BayerBG2RGB);
            else
                color = SMCalibrationSetI.frame0.clone();
 
            cvtools::drawChessboardCorners(color, checkerCount, qci0, success0, 10);
            SMCalibrationSetI.frame0Result = color;
        }
 
        emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
 
        // Camera 1
        std::vector<cv::Point2f> qci1;
 
        // Convert to grayscale
        if(SMCalibrationSetI.frame1.channels() == 1)
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);
        else
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
 
        // Extract checker corners
        bool success1 = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success1){
            cv::cornerSubPix(gray, qci1, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
            // Draw colored chessboard
            cv::Mat color;
            if(SMCalibrationSetI.frame1.channels() == 1)
                cv::cvtColor(SMCalibrationSetI.frame1, color, CV_BayerBG2RGB);
            else
                color = SMCalibrationSetI.frame1.clone();
 
            cvtools::drawChessboardCorners(color, checkerCount, qci1, success1, 10);
            SMCalibrationSetI.frame1Result = color;
        }
 
        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 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
    // 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,
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
//std::cout << cal.k0 << std::endl;
//    // 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,
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
//std::cout << cal.k1 << std::endl;
    // stereo calibration
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;// + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
    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, 200, 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 134 → 136

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