Subversion Repositories seema-scanner

Rev

Rev 29 | Rev 33 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 29 Rev 31
Line 1... Line 1...
1 
#include "SMCalibrationWorker.h"
 1 
#include "SMCalibrationWorker.h"
2 
#include "SMCalibrationParameters.h"
 2 
#include "SMCalibrationParameters.h"
3 
 
 3 
 
- 
 
 4 
#include "cvtools.h"
- 
 
 5 
 
4 
#include <QSettings>
 6 
#include <QSettings>
5 
 
 7 
 
6 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
 8 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
7 
 
 9 
 
8 
    // Number of saddle points on calibration pattern
 10 
    // Number of saddle points on calibration pattern
9 
    cv::Size patternSize(10, 9);
 11 
    cv::Size patternSize(10, 9);
10 
 
 12 
 
11 
    int nSets = calibrationData.size();
 13 
    int nSets = calibrationData.size();
12 
 
 14 
 
13 
    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
 15 
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
- 
 
 16 
    std::vector<float> angles;
14 
 
 17 
 
15 
    // Loop through calibration sets
 18 
    // Loop through calibration sets
16 
    for(int i=0; i<nSets; i++){
 19 
    for(int i=0; i<nSets; i++){
17 
 
 20 
 
18 
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
 21 
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
Line 52... Line 55...
52 
 
 55 
 
53 
        SMCalibrationSetI.success = success0 && success1;
 56 
        SMCalibrationSetI.success = success0 && success1;
54 
 
 57 
 
55 
        // Add to whole set
 58 
        // Add to whole set
56 
        if(SMCalibrationSetI.success){
 59 
        if(SMCalibrationSetI.success){
57 
            qc[0].push_back(qci0);
 60 
            qc0.push_back(qci0);
58 
            qc[1].push_back(qci1);
 61 
            qc1.push_back(qci1);
- 
 
 62 
            angles.push_back(SMCalibrationSetI.rotationAngle);
59 
        }
 63 
        }
60 
 
 64 
 
61 
        // Show progress
 65 
        // Show progress
62 
        emit newSetProcessed(i);
 66 
        emit newSetProcessed(i);
63 
    }
 67 
    }
64 
 
 68 
 
65 
    int nValidSets = qc[0].size();
 69 
    int nValidSets = qc0.size();
66 
    if(nValidSets < 2){
 70 
    if(nValidSets < 2){
67 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
 71 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
68 
        emit done();
 72 
        emit done();
69 
        return;
 73 
        return;
70 
    }
 74 
    }
Line 73... Line 77...
73 
    std::vector<cv::Point3f> Qi;
 77 
    std::vector<cv::Point3f> Qi;
74 
    for (int h=0; h<patternSize.height; h++)
 78 
    for (int h=0; h<patternSize.height; h++)
75 
        for (int w=0; w<patternSize.width; w++)
 79 
        for (int w=0; w<patternSize.width; w++)
76 
            Qi.push_back(cv::Point3f(5 * w, 5* h, 0.0)); // 5mm chess field size
 80 
            Qi.push_back(cv::Point3f(5 * w, 5* h, 0.0)); // 5mm chess field size
77 
    std::vector< std::vector<cv::Point3f> > Q;
 81 
    std::vector< std::vector<cv::Point3f> > Q;
78 
    for(int i=0; i<qc[0].size(); i++)
 82 
    for(int i=0; i<qc0.size(); i++)
79 
        Q.push_back(Qi);
 83 
        Q.push_back(Qi);
80 
 
 84 
 
81 
    // calibrate the cameras
 85 
    // calibrate the cameras
82 
    cv::Size frameSize(calibrationData[0].frame0.cols, calibrationData[0].frame0.rows);
 - 
 
83 
//    cv::Size frameSize(640, 480);
 - 
 
84 
    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
 - 
 
85 
 
 - 
 
86 
    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
 - 
 
87 
    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
 - 
 
88 
 
 - 
 
89 
    SMCalibrationParameters cal;
 86 
    SMCalibrationParameters cal;
- 
 
 87 
    cal.frameWidth = calibrationData[0].frame0.cols;
- 
 
 88 
    cal.frameHeight = calibrationData[0].frame0.rows;
- 
 
 89 
    cv::Size frameSize(cal.frameWidth, cal.frameHeight);
90 
 
 90 
 
- 
 
 91 
    int flags = 0; //cv::CALIB_FIX_K3;
- 
 
 92 
 
91 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
 93 
    std::vector<cv::Vec3f> cam_rvecs0, cam_tvecs0;
92 
    cv::Mat K0, k0;
 - 
 
93 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0);
 94 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
94 
 
 95 
 
95 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
 96 
    std::vector<cv::Vec3f> cam_rvecs1, cam_tvecs1;
96 
    cv::Mat K1, k1;
 - 
 
97 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1);
 97 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
98 
 
 98 
 
99 
    // stereo calibration (don't change K0, K1, k0, k1)
 99 
    // stereo calibration (fix K0, K1, k0, k1)
100 
    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
 100 
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;
101 
    cv::Mat E, F, R1, T1;
 - 
 
102 
    cal.stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], K0, k0, K1, k1,
 101 
    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
103 
                                              frameSize, R1, T1, E, F,
 102 
                                              frameSize, cal.R1, cal.T1, cal.E, cal.F,
104 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
 103 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
105 
                                              flags_stereo);
 104 
                                              flags_stereo);
106 
 
 105 
 
- 
 
 106 
    // calibrate rotation axis
- 
 
 107 
    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame
- 
 
 108 
    std::vector<cv::Vec3f> Tc(nValidSets - 1);
- 
 
 109 
    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
- 
 
 110 
    std::vector<cv::Vec3f> Tr(nValidSets - 1);
- 
 
 111 
    for(int i=0; i<nValidSets-1; i++){
- 
 
 112 
        // relative transformations in camera
- 
 
 113 
        cv::Matx33f cRw1, cRw2;
- 
 
 114 
        cv::Rodrigues(cam_rvecs0[i], cRw1);
- 
 
 115 
        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
- 
 
 116 
        cv::Vec3f cTw1 = cam_tvecs0[i];
- 
 
 117 
        cv::Vec3f cTw2 = cam_tvecs0[i+1];
- 
 
 118 
 
- 
 
 119 
        cv::composeRT(cRw1, cTw1, cRw2.t(), -cRw2.t()*cTw2, Rc[i], Tc[i]);
- 
 
 120 
 
- 
 
 121 
        // relative transformations in rotation stage
- 
 
 122 
        // we define the rotation axis to be in origo, pointing in positive y direction
- 
 
 123 
        float angleRadians = angles[i]/180.0*M_PI;
- 
 
 124 
        cv::Vec3f rot_rvec(0.0, angleRadians, 0.0);
- 
 
 125 
        cv::Rodrigues(rot_rvec, Rr[i]);
- 
 
 126 
        Tr[i] = 0.0;
- 
 
 127 
    }
- 
 
 128 
 
- 
 
 129 
    cvtools::fitSixDofData(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
- 
 
 130 
 
107 
    // Print to std::cout
 131 
    // Print to std::cout
108 
    cal.print();
 132 
    cal.print();
109 
 
 133 
 
110 
    // save to (reentrant qsettings object)
 134 
    // save to (reentrant qsettings object)
111 
    QSettings settings;
 135 
    QSettings settings;