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();
 
    // 2D Points collected for OpenCV's calibration procedures
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
 
    std::vector<bool> success0(nSets), success1(nSets);
 
    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
        success0[i] = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success0[i]){
            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[i], 10);
            SMCalibrationSetI.frame0Result = color;
        }
 
        emit newFrameResult(i, 0, success0[i], 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
        success1[i] = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success1[i]){
            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[i], 10);
            SMCalibrationSetI.frame1Result = color;
        }
 
        emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);
 
        if(success0[i])
            qc0.push_back(qci0);
 
        if(success1[i])
            qc1.push_back(qci1);
 
        if(success0[i] && success1[i]){
            qc0Stereo.push_back(qci0);
            qc1Stereo.push_back(qci1);
            angles.push_back(SMCalibrationSetI.rotationAngle);
        }
 
        // Show progress
        emit newSetProcessed(i);
    }
 
    int nValidSets = angles.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> > Q0, Q1, QStereo;
    for(int i=0; i<qc0.size(); i++)
        Q0.push_back(Qi);
    for(int i=0; i<qc1.size(); i++)
        Q1.push_back(Qi);
    for(int i=0; i<nValidSets; i++)
        QStereo.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_ASPECT_RATIO + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;
    // 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(Q0, 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(Q1, 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(QStereo, qc0Stereo, qc1Stereo, 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;
 
    // Determine per-view reprojection errors:
    cal.cam0_errors_per_view.resize(nSets);
    int idx = 0;
    for(unsigned int i = 0; i < nSets; ++i){
        if(success0[i]){
            int n = (int)Q0[idx].size();
            std::vector<cv::Point2f> qc_proj;
            cv::projectPoints(cv::Mat(Q0[idx]), cam_rvecs0[idx], cam_tvecs0[idx], cal.K0,  cal.k0, qc_proj);
            float err = 0;
            for(int j=0; j<qc_proj.size(); j++){
                cv::Point2f d = qc0[idx][j] - qc_proj[j];
                err += cv::sqrt(d.x*d.x + d.y*d.y);
            }
            cal.cam0_errors_per_view[i] = (float)err/n;
            idx++;
        } else
            cal.cam0_errors_per_view[i] = NAN;
    }
    cal.cam1_errors_per_view.resize(nSets);
    idx = 0;
    for(unsigned int i = 0; i < nSets; ++i){
        if(success1[i]){
            int n = (int)Q1[idx].size();
            std::vector<cv::Point2f> qc_proj;
            cv::projectPoints(cv::Mat(Q1[idx]), cam_rvecs1[idx], cam_tvecs1[idx], cal.K1,  cal.k1, qc_proj);
            float err = 0;
            for(int j=0; j<qc_proj.size(); j++){
                cv::Point2f d = qc1[idx][j] - qc_proj[j];
                err += cv::sqrt(d.x*d.x + d.y*d.y);
            }
            cal.cam1_errors_per_view[i] = (float)err/n;
            idx++;
       } else
            cal.cam1_errors_per_view[i] = NAN;
    }
 
//    // 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();
 
}
 

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

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 140 → 148