Subversion Repositories seema-scanner

#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::cornerSubPix(calibrationSetI.frame0, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
            // Draw colored chessboard
            cv::cvtColor(calibrationSetI.frame0, calibrationSetI.frame0Result, CV_GRAY2RGB);
            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::cornerSubPix(calibrationSetI.frame1, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
            // Draw colored chessboard
            cv::cvtColor(calibrationSetI.frame1, calibrationSetI.frame1Result, CV_GRAY2RGB);
            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);

}