Subversion Repositories seema-scanner

Rev

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

Rev 136 Rev 137
Line 15... Line 15...
15 
    cv::Size checkerCount(checkerCountX, checkerCountY);
 15 
    cv::Size checkerCount(checkerCountX, checkerCountY);
16 
 
 16 
 
17 
    int nSets = calibrationData.size();
 17 
    int nSets = calibrationData.size();
18 
 
 18 
 
19 
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
 19 
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
- 
 
 20 
    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
- 
 
 21 
 
20 
    std::vector<float> angles;
 22 
    std::vector<float> angles;
21 
 
 23 
 
22 
    // Loop through calibration sets
 24 
    // Loop through calibration sets
23 
    for(int i=0; i<nSets; i++){
 25 
    for(int i=0; i<nSets; i++){
24 
 
 26 
 
Line 78... Line 80...
78 
            SMCalibrationSetI.frame1Result = color;
 80 
            SMCalibrationSetI.frame1Result = color;
79 
        }
 81 
        }
80 
 
 82 
 
81 
        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
 83 
        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
82 
 
 84 
 
83 
        SMCalibrationSetI.success = success0 && success1;
 - 
 
84 
 
 - 
 
85 
        // Add to whole set
 - 
 
86 
        if(SMCalibrationSetI.success){
 85 
        if(success0)
87 
            qc0.push_back(qci0);
 86 
            qc0.push_back(qci0);
- 
 
 87 
 
- 
 
 88 
        if(success1)
88 
            qc1.push_back(qci1);
 89 
            qc1.push_back(qci1);
- 
 
 90 
 
- 
 
 91 
        if(success0 && success1){
- 
 
 92 
            qc0Stereo.push_back(qci0);
- 
 
 93 
            qc1Stereo.push_back(qci1);
89 
            angles.push_back(SMCalibrationSetI.rotationAngle);
 94 
            angles.push_back(SMCalibrationSetI.rotationAngle);
90 
        }
 95 
        }
91 
 
 96 
 
92 
        // Show progress
 97 
        // Show progress
93 
        emit newSetProcessed(i);
 98 
        emit newSetProcessed(i);
94 
    }
 99 
    }
95 
 
 100 
 
96 
    int nValidSets = qc0.size();
 101 
    int nValidSets = angles.size();
97 
    if(nValidSets < 2){
 102 
    if(nValidSets < 2){
98 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
 103 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
99 
        emit done();
 104 
        emit done();
100 
        return;
 105 
        return;
101 
    }
 106 
    }
Line 104... Line 109...
104 
    float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm
 109 
    float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm
105 
    std::vector<cv::Point3f> Qi;
 110 
    std::vector<cv::Point3f> Qi;
106 
    for (int h=0; h<checkerCount.height; h++)
 111 
    for (int h=0; h<checkerCount.height; h++)
107 
        for (int w=0; w<checkerCount.width; w++)
 112 
        for (int w=0; w<checkerCount.width; w++)
108 
            Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
 113 
            Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
- 
 
 114 
 
109 
    std::vector< std::vector<cv::Point3f> > Q;
 115 
    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
110 
    for(int i=0; i<qc0.size(); i++)
 116 
    for(int i=0; i<qc0.size(); i++)
111 
        Q.push_back(Qi);
 117 
        Q0.push_back(Qi);
- 
 
 118 
    for(int i=0; i<qc1.size(); i++)
- 
 
 119 
        Q1.push_back(Qi);
- 
 
 120 
    for(int i=0; i<nValidSets; i++)
- 
 
 121 
        QStereo.push_back(Qi);
112 
 
 122 
 
113 
    // calibrate the cameras
 123 
    // calibrate the cameras
114 
    SMCalibrationParameters cal;
 124 
    SMCalibrationParameters cal;
115 
    cal.frameWidth = calibrationData[0].frame0.cols;
 125 
    cal.frameWidth = calibrationData[0].frame0.cols;
116 
    cal.frameHeight = calibrationData[0].frame0.rows;
 126 
    cal.frameHeight = calibrationData[0].frame0.rows;
Line 118... Line 128...
118 
 
 128 
 
119 
    // determine only k1, k2 for lens distortion
 129 
    // determine only k1, k2 for lens distortion
120 
    int flags = cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
 130 
    int flags = cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
121 
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
 131 
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
122 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
 132 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
123 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags,
 133 
    cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags,
124 
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
 134 
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
125 
//std::cout << cal.k0 << std::endl;
 135 
//std::cout << cal.k0 << std::endl;
126 
//    // refine extrinsics for camera 0
 136 
//    // refine extrinsics for camera 0
127 
//    for(int i=0; i<Q.size(); i++)
 137 
//    for(int i=0; i<Q.size(); i++)
128 
//        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);
 138 
//        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);
129 
 
 139 
 
130 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
 140 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
131 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,
 141 
    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,
132 
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
 142 
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
133 
//std::cout << cal.k1 << std::endl;
 143 
//std::cout << cal.k1 << std::endl;
134 
    // stereo calibration
 144 
    // stereo calibration
135 
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;// + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
 145 
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;// + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
136 
    cv::Mat E, F, R1, T1;
 146 
    cv::Mat E, F, R1, T1;
137 
    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
 147 
    cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,
138 
                                              frameSize, R1, T1, E, F,
 148 
                                              frameSize, R1, T1, E, F,
139 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON),
 149 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON),
140 
                                              flags_stereo);
 150 
                                              flags_stereo);
141 
 
 151 
 
142 
    cal.R1 = R1;
 152 
    cal.R1 = R1;