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


#include "SMCalibrator.h"
 
#include <QSettings>
 
void SMCalibrator::performCalibration(std::vector<CalibrationSet> &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++){
 
        CalibrationSet calibrationSetI = calibrationData[i];
 
        if(!calibrationSetI.checked)
            continue;
 
        // Camera 0
        std::vector<cv::Point2f> qci0;
        // Extract checker corners
        bool success0 = cv::findChessboardCorners(calibrationSetI.frame0, patternSize, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
        if(success0){
            cv::Mat gray;
            cv::cvtColor(calibrationSetI.frame0, gray, CV_RGB2GRAY);
            cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
            // Draw colored chessboard
            calibrationSetI.frame0Result = calibrationSetI.frame0.clone();
            cv::drawChessboardCorners(calibrationSetI.frame0Result, patternSize, qci0, success0);
        }
 
        // Camera 1
        std::vector<cv::Point2f> qci1;
        // Extract checker corners
        bool success1 = cv::findChessboardCorners(calibrationSetI.frame1, patternSize, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
        if(success1){
            cv::Mat gray;
            cv::cvtColor(calibrationSetI.frame1, gray, CV_RGB2GRAY);
            cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
            // Draw colored chessboard
            calibrationSetI.frame1Result = calibrationSetI.frame1.clone();
            cv::drawChessboardCorners(calibrationSetI.frame1Result, patternSize, qci1, success1);
        }
 
        calibrationSetI.success = success0 && success1;
 
        // Add to whole set
        if(calibrationSetI.success){
            qc[0].push_back(qci0);
            qc[1].push_back(qci1);
        }
 
        // Show results
        emit newFrameProcessed(i);
    }
 
    int nValidSets = qc[0].size();
    if(nValidSets <= 2){
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
        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];
 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
    cv::Mat K0, k0;
    double cam0_error = cv::calibrateCamera(Q, qc0, frameSize, K0, k0, cam_rvecs0, cam_tvecs0);
 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
    cv::Mat K1, k1;
    double cam1_error = cv::calibrateCamera(Q, qc1, frameSize, K1, 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;
    double 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);
 
}
 

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

Subversion Repositories seema-scanner

(root)/src/SMCalibrator.cpp @ 26 – Rev 25 → 26