Subversion Repositories seema-scanner

Rev

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

Rev 114 Rev 120
Line 27... Line 27...
27 
        if(!SMCalibrationSetI.checked)
 27 
        if(!SMCalibrationSetI.checked)
28 
            continue;
 28 
            continue;
29 
 
 29 
 
30 
        // Camera 0
 30 
        // Camera 0
31 
        std::vector<cv::Point2f> qci0;
 31 
        std::vector<cv::Point2f> qci0;
- 
 
 32 
        // Convert bayer to grayscale
- 
 
 33 
        cv::Mat temp, gray;
- 
 
 34 
        SMCalibrationSetI.frame0.convertTo(temp, CV_8UC1, 1.0/256.0);
- 
 
 35 
        cv::cvtColor(temp, gray, CV_BayerBG2GRAY);
32 
        // Extract checker corners
 36 
        // Extract checker corners
33 
        bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
 37 
        bool success0 = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
34 
        if(success0){
 38 
        if(success0){
35 
            // Convert bayer to grayscale
 - 
 
36 
            cv::Mat gray;
 - 
 
37 
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);
 - 
 
38 
            cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
 39 
            cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
39 
            // Draw colored chessboard
 40 
            // Draw colored chessboard
- 
 
 41 
            cv::Mat color;
40 
            SMCalibrationSetI.frame0Result = SMCalibrationSetI.frame0.clone();
 42 
            cv::cvtColor(temp, color, CV_BayerBG2RGB);
41 
            cvtools::drawChessboardCorners(SMCalibrationSetI.frame0Result, checkerCount, qci0, success0, 10);
 43 
            cvtools::drawChessboardCorners(color, checkerCount, qci0, success0, 10);
- 
 
 44 
            SMCalibrationSetI.frame0Result = color;
42 
        }
 45 
        }
43 
 
 46 
 
44 
        emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
 47 
        emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
45 
 
 48 
 
46 
        // Camera 1
 49 
        // Camera 1
47 
        std::vector<cv::Point2f> qci1;
 50 
        std::vector<cv::Point2f> qci1;
- 
 
 51 
        // Convert bayer to grayscale
- 
 
 52 
        SMCalibrationSetI.frame1.convertTo(temp, CV_8UC1, 1.0/256.0);
- 
 
 53 
        cv::cvtColor(temp, gray, CV_BayerBG2GRAY);
48 
        // Extract checker corners
 54 
        // Extract checker corners
49 
        bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
 55 
        bool success1 = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
50 
        if(success1){
 56 
        if(success1){
51 
            // Convert bayer to grayscale
 - 
 
52 
            cv::Mat gray;
 - 
 
53 
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);
 - 
 
54 
            cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
 57 
            cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
55 
            // Draw colored chessboard
 58 
            // Draw colored chessboard
- 
 
 59 
            cv::Mat color;
56 
            SMCalibrationSetI.frame1Result = SMCalibrationSetI.frame1.clone();
 60 
            cv::cvtColor(temp, color, CV_BayerBG2RGB);
57 
            cvtools::drawChessboardCorners(SMCalibrationSetI.frame1Result, checkerCount, qci1, success1, 10);
 61 
            cvtools::drawChessboardCorners(color, checkerCount, qci1, success1, 10);
- 
 
 62 
            SMCalibrationSetI.frame1Result = color;
58 
        }
 63 
        }
59 
 
 64 
 
60 
        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
 65 
        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
61 
 
 66 
 
62 
        SMCalibrationSetI.success = success0 && success1;
 67 
        SMCalibrationSetI.success = success0 && success1;
Line 99... Line 104...
99 
    int flags = cv::CALIB_FIX_K3;
 104 
    int flags = cv::CALIB_FIX_K3;
100 
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
 105 
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
101 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
 106 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
102 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
 107 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
103 
 
 108 
 
104 
    // refine extrinsics for camera 0
 109 
//    // refine extrinsics for camera 0
105 
    for(int i=0; i<Q.size(); i++)
 110 
//    for(int i=0; i<Q.size(); i++)
106 
        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);
 111 
//        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);
107 
 
 112 
 
108 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
 113 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
109 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
 114 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
110 
 
 115 
 
111 
    // stereo calibration
 116 
    // stereo calibration
112 
    int flags_stereo = cv::CALIB_USE_INTRINSIC_GUESS + cv::CALIB_FIX_K3;
 117 
    int flags_stereo = cv::CALIB_FIX_K3;
113 
    cv::Mat E, F, R1, T1;
 118 
    cv::Mat E, F, R1, T1;
114 
    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
 119 
    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
115 
                                              frameSize, R1, T1, E, F,
 120 
                                              frameSize, R1, T1, E, F,
116 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
 121 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 100, DBL_EPSILON),
117 
                                              flags_stereo);
 122 
                                              flags_stereo);
118 
 
 123 
 
119 
    cal.R1 = R1;
 124 
    cal.R1 = R1;
120 
    cal.T1 = T1;
 125 
    cal.T1 = T1;
121 
    cal.E = E;
 126 
    cal.E = E;