WebSVN – seema-scanner – Diff – /src/SMCalibrationWorker.cpp


#include "SMCalibrationWorker.h"
#include "SMCalibrationParameters.h"
 
#include <QSettings>
 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
 
    // Number of saddle points on calibration pattern
    cv::Size patternSize(10, 9);
 
    int nSets = calibrationData.size();
 
    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
 
    // 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, patternSize, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
        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();
            cv::drawChessboardCorners(SMCalibrationSetI.frame0Result, patternSize, qci0, success0);
 
 
        }
 
        emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
 
        // Camera 1
        std::vector<cv::Point2f> qci1;
        // Extract checker corners
        bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, patternSize, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
        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();
            cv::drawChessboardCorners(SMCalibrationSetI.frame1Result, patternSize, qci1, success1);
 
 
        }
 
        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
 
        SMCalibrationSetI.success = success0 && success1;
 
        // Add to whole set
        if(SMCalibrationSetI.success){
            qc[0].push_back(qci0);
            qc[1].push_back(qci1);
        }
 
        // Show progress
        emit newSetProcessed(i);
    }
 
    int nValidSets = qc[0].size();
    if(nValidSets < 2){
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
        emit done();
        return;
    }
 
    // Generate world object coordinates [mm]
    std::vector<cv::Point3f> Qi;
    for (int h=0; h<patternSize.height; h++)
        for (int w=0; w<patternSize.width; w++)
            Qi.push_back(cv::Point3f(5 * w, 5* h, 0.0)); // 5mm chess field size
    std::vector< std::vector<cv::Point3f> > Q;
    for(int i=0; i<qc[0].size(); i++)
        Q.push_back(Qi);
 
    // calibrate the cameras
    cv::Size frameSize(calibrationData[0].frame0.cols, calibrationData[0].frame0.rows);
//    cv::Size frameSize(640, 480);
    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
 
    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
 
    SMCalibrationParameters cal;
 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
    cv::Mat K0, k0;
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0);
 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
    cv::Mat K1, k1;
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1);
 
    // stereo calibration (don't change K0, K1, k0, k1)
    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
    cv::Mat E, F, R1, T1;
    cal.stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], K0, k0, K1, k1,
                                              frameSize, R1, T1, E, F,
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
                                              flags_stereo);
 
    // Print to std::cout
    cal.print();
 
    // save to (reentrant qsettings object)
    QSettings settings;
    settings.setValue("calibration", QVariant::fromValue(cal));
 
    emit done();
 
}
 

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

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 27 → 29