Subversion Repositories seema-scanner

Rev

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

Rev 26 Rev 27
Line 1... Line 1...
1 
#include "SMCalibrator.h"
 1 
#include "SMCalibrationWorker.h"
- 
 
 2 
#include "SMCalibrationParameters.h"
2 
 
 3 
 
3 
#include <QSettings>
 4 
#include <QSettings>
4 
 
 5 
 
5 
void SMCalibrator::performCalibration(std::vector<CalibrationSet> &calibrationData){
 6 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
6 
 
 7 
 
7 
    // Number of saddle points on calibration pattern
 8 
    // Number of saddle points on calibration pattern
8 
    cv::Size patternSize(10, 9);
 9 
    cv::Size patternSize(10, 9);
9 
 
 10 
 
10 
    int nSets = calibrationData.size();
 11 
    int nSets = calibrationData.size();
Line 12... Line 13...
12 
    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
 13 
    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
13 
 
 14 
 
14 
    // Loop through calibration sets
 15 
    // Loop through calibration sets
15 
    for(int i=0; i<nSets; i++){
 16 
    for(int i=0; i<nSets; i++){
16 
 
 17 
 
17 
        CalibrationSet calibrationSetI = calibrationData[i];
 18 
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
18 
 
 19 
 
19 
        if(!calibrationSetI.checked)
 20 
        if(!SMCalibrationSetI.checked)
20 
            continue;
 21 
            continue;
21 
 
 22 
 
22 
        // Camera 0
 23 
        // Camera 0
23 
        std::vector<cv::Point2f> qci0;
 24 
        std::vector<cv::Point2f> qci0;
24 
        // Extract checker corners
 25 
        // Extract checker corners
25 
        bool success0 = cv::findChessboardCorners(calibrationSetI.frame0, patternSize, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
 26 
        bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, patternSize, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
26 
        if(success0){
 27 
        if(success0){
27 
            cv::Mat gray;
 28 
            cv::Mat gray;
28 
            cv::cvtColor(calibrationSetI.frame0, gray, CV_RGB2GRAY);
 29 
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
29 
            cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
 30 
            cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
30 
            // Draw colored chessboard
 31 
            // Draw colored chessboard
31 
            calibrationSetI.frame0Result = calibrationSetI.frame0.clone();
 32 
            SMCalibrationSetI.frame0Result = SMCalibrationSetI.frame0.clone();
32 
            cv::drawChessboardCorners(calibrationSetI.frame0Result, patternSize, qci0, success0);
 33 
            cv::drawChessboardCorners(SMCalibrationSetI.frame0Result, patternSize, qci0, success0);
- 
 
 34 
 
- 
 
 35 
            emit newFrameResult(i, 0, SMCalibrationSetI.frame0Result);
33 
        }
 36 
        }
34 
 
 37 
 
35 
        // Camera 1
 38 
        // Camera 1
36 
        std::vector<cv::Point2f> qci1;
 39 
        std::vector<cv::Point2f> qci1;
37 
        // Extract checker corners
 40 
        // Extract checker corners
38 
        bool success1 = cv::findChessboardCorners(calibrationSetI.frame1, patternSize, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
 41 
        bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, patternSize, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
39 
        if(success1){
 42 
        if(success1){
40 
            cv::Mat gray;
 43 
            cv::Mat gray;
41 
            cv::cvtColor(calibrationSetI.frame1, gray, CV_RGB2GRAY);
 44 
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
42 
            cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
 45 
            cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
43 
            // Draw colored chessboard
 46 
            // Draw colored chessboard
44 
            calibrationSetI.frame1Result = calibrationSetI.frame1.clone();
 47 
            SMCalibrationSetI.frame1Result = SMCalibrationSetI.frame1.clone();
45 
            cv::drawChessboardCorners(calibrationSetI.frame1Result, patternSize, qci1, success1);
 48 
            cv::drawChessboardCorners(SMCalibrationSetI.frame1Result, patternSize, qci1, success1);
- 
 
 49 
 
- 
 
 50 
            emit newFrameResult(i, 1, SMCalibrationSetI.frame1Result);
46 
        }
 51 
        }
47 
 
 52 
 
48 
        calibrationSetI.success = success0 && success1;
 53 
        SMCalibrationSetI.success = success0 && success1;
49 
 
 54 
 
50 
        // Add to whole set
 55 
        // Add to whole set
51 
        if(calibrationSetI.success){
 56 
        if(SMCalibrationSetI.success){
52 
            qc[0].push_back(qci0);
 57 
            qc[0].push_back(qci0);
53 
            qc[1].push_back(qci1);
 58 
            qc[1].push_back(qci1);
54 
        }
 59 
        }
55 
 
 60 
 
56 
        // Show results
 61 
        // Show progress
57 
        emit newFrameProcessed(i);
 62 
        emit newSetProcessed(i);
58 
    }
 63 
    }
59 
 
 64 
 
60 
    int nValidSets = qc[0].size();
 65 
    int nValidSets = qc[0].size();
61 
    if(nValidSets <= 2){
 66 
    if(nValidSets < 2){
62 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
 67 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
63 
        return;
 68 
        return;
64 
    }
 69 
    }
65 
 
 70 
 
66 
    // Generate world object coordinates [mm]
 71 
    // Generate world object coordinates [mm]
Line 78... Line 83...
78 
    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
 83 
    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
79 
 
 84 
 
80 
    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
 85 
    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
81 
    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
 86 
    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
82 
 
 87 
 
- 
 
 88 
    SMCalibrationParameters cal;
- 
 
 89 
 
83 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
 90 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
84 
    cv::Mat K0, k0;
 91 
    cv::Mat K0, k0;
85 
    double cam0_error = cv::calibrateCamera(Q, qc0, frameSize, K0, k0, cam_rvecs0, cam_tvecs0);
 92 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0);
86 
 
 93 
 
87 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
 94 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
88 
    cv::Mat K1, k1;
 95 
    cv::Mat K1, k1;
89 
    double cam1_error = cv::calibrateCamera(Q, qc1, frameSize, K1, k1, cam_rvecs1, cam_tvecs1);
 96 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1);
90 
 
 97 
 
91 
    // stereo calibration (don't change K0, K1, k0, k1)
 98 
    // stereo calibration (don't change K0, K1, k0, k1)
92 
    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
 99 
    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
93 
    cv::Mat E, F, R1, T1;
 100 
    cv::Mat E, F, R1, T1;
94 
    double stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], K0, k0, K1, k1,
 101 
    cal.stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], K0, k0, K1, k1,
95 
                                              frameSize, R1, T1, E, F,
 102 
                                              frameSize, R1, T1, E, F,
96 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
 103 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
97 
                                              flags_stereo);
 104 
                                              flags_stereo);
98 
 
 105 
 
- 
 
 106 
    // Print to std::cout
- 
 
 107 
    cal.print();
- 
 
 108 
 
- 
 
 109 
    // save to (reentrant qsettings object)
- 
 
 110 
    QSettings settings;
- 
 
 111 
    settings.setValue("calibration", QVariant::fromValue(cal));
- 
 
 112 
 
- 
 
 113 
    emit done();
- 
 
 114 
 
99 
}
 115 
}