Subversion Repositories seema-scanner

#include "SMCalibrationWorker.h"
#include "SMCalibrationParameters.h"

#include "cvtools.h"

#include <QSettings>

void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){

    QSettings settings;

    // Number of saddle points on calibration pattern
    int checkerCountX = settings.value("calibration/checkerCountX", 10).toInt();
    int checkerCountY = settings.value("calibration/checkerCountY", 9).toInt();
    cv::Size checkerCount(checkerCountX, checkerCountY);

    int nSets = calibrationData.size();

    std::vector< std::vector<cv::Point2f> > qc0, qc1;
    std::vector<float> angles;

    // 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, checkerCount, 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, checkerCount, 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, checkerCount, 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, checkerCount, qci1, success1);
        }

        emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);

        SMCalibrationSetI.success = success0 && success1;

        // Add to whole set
        if(SMCalibrationSetI.success){
            qc0.push_back(qci0);
            qc1.push_back(qci1);
            angles.push_back(SMCalibrationSetI.rotationAngle);
        }

        // Show progress
        emit newSetProcessed(i);
    }

    int nValidSets = qc0.size();
    if(nValidSets < 2){
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
        emit done();
        return;
    }

    // Generate world object coordinates [mm]
    float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm
    std::vector<cv::Point3f> Qi;
    for (int h=0; h<checkerCount.height; h++)
        for (int w=0; w<checkerCount.width; w++)
            Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
    std::vector< std::vector<cv::Point3f> > Q;
    for(int i=0; i<qc0.size(); i++)
        Q.push_back(Qi);

    // calibrate the cameras
    SMCalibrationParameters cal;
    cal.frameWidth = calibrationData[0].frame0.cols;
    cal.frameHeight = calibrationData[0].frame0.rows;
    cv::Size frameSize(cal.frameWidth, cal.frameHeight);

    int flags = 0; //cv::CALIB_FIX_K3;

    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);

    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);

    // stereo calibration (fix K0, K1, k0, k1)
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;
    cv::Mat E, F, R1, T1;
    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
                                              frameSize, R1, T1, E, F,
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
                                              flags_stereo);

    cal.R1 = R1;
    cal.T1 = T1;
    cal.E = E;
    cal.F = F;

    // calibrate rotation axis
    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame
    std::vector<cv::Vec3f> Tc(nValidSets - 1);
    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
    std::vector<cv::Vec3f> Tr(nValidSets - 1);
    for(int i=0; i<nValidSets-1; i++){
        // relative transformations in camera
        cv::Mat cRw1, cRw2;
        cv::Rodrigues(cam_rvecs0[i], cRw1);
        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
        cv::Mat cTw1 = cam_tvecs0[i];
        cv::Mat cTw2 = cam_tvecs0[i+1];
        cv::Mat w1Rc = cRw1.t();
        cv::Mat w1Tc = -cRw1.t()*cTw1;
        Rc[i] = cv::Mat(cRw2*w1Rc);
        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);

        // relative transformations in rotation stage
        // we define the rotation axis to be in origo, pointing in positive y direction
        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;
        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
        cv::Mat Rri;
        cv::Rodrigues(rot_rvec, Rri);
        Rr[i] = Rri;
        Tr[i] = 0.0;

        std::cout << i << std::endl;
//        std::cout << "cTw1" << cTw1 << std::endl;
//        std::cout << "cTw2" << cTw2 << std::endl;
//        std::cout << "w2Rc" << w2Rc << std::endl;
//        std::cout << "w2Tc" << w2Tc << std::endl;

//        std::cout << "w2Rc" << w2Rc << std::endl;
//        std::cout << "w2Tc" << w2Tc << std::endl;

        cv::Mat Rci;
        cv::Rodrigues(Rc[i], Rci);
        std::cout << "Rci" << Rci << std::endl;
        std::cout << "Tc[i]" << Tc[i] << std::endl;

        std::cout << "rot_rvec" << rot_rvec << std::endl;
        std::cout << "Tr[i]" << Tr[i] << std::endl;
        std::cout << std::endl;
    }

    // determine the transformation from rotation stage to camera 0
    cvtools::fitSixDofData(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
    cal.Rr = cal.Rr.t();
    cal.Tr = -cal.Rr*cal.Tr;

    for(int i=0; i<Rc.size(); i++){

//        cv::Matx33f a = cal.Rr*Rc[i];
//        cv::Matx33f b = Rr[i];

//        std::cout << "a:" << a << std::endl;
//        std::cout << "b:" << b << std::endl;

        cv::Vec3f a = cal.Rr*Tr[i]+cal.Tr;
        cv::Vec3f b = Tc[i];

        std::cout << "a:" << a << std::endl;
        std::cout << "b:" << b << std::endl;
    }
    cv::Mat rrvec;
    cv::Rodrigues(cal.Rr, rrvec);
    std::cout << "rrvec:" << rrvec << std::endl;

    // Print to std::cout
    cal.print();

    // save to (reentrant qsettings object)
    settings.setValue("calibration/parameters", QVariant::fromValue(cal));

    emit done();

}