Subversion Repositories seema-scanner

#include "SMCalibrationWorker.h"

#include "SMCalibrationParameters.h"

#include "cvtools.h"

#include <QSettings>

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

    // Number of saddle points on calibration pattern

    cv::Size patternSize(10, 9);

    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, patternSize, 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, patternSize, 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, patternSize, 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, patternSize, 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]

    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<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;

    std::vector<cv::Vec3f> cam_rvecs0, cam_tvecs0;

    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);

    std::vector<cv::Vec3f> 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;

    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,

                                              frameSize, cal.R1, cal.T1, cal.E, cal.F,

                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),

                                              flags_stereo);

    // 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::Matx33f cRw1, cRw2;

        cv::Rodrigues(cam_rvecs0[i], cRw1);

        cv::Rodrigues(cam_rvecs0[i+1], cRw2);

        cv::Vec3f cTw1 = cam_tvecs0[i];

        cv::Vec3f cTw2 = cam_tvecs0[i+1];

        cv::composeRT(cRw1, cTw1, cRw2.t(), -cRw2.t()*cTw2, Rc[i], Tc[i]);

        // relative transformations in rotation stage

        // we define the rotation axis to be in origo, pointing in positive y direction

        float angleRadians = angles[i]/180.0*M_PI;

        cv::Vec3f rot_rvec(0.0, angleRadians, 0.0);

        cv::Rodrigues(rot_rvec, Rr[i]);

        Tr[i] = 0.0;

    }

    cvtools::fitSixDofData(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);

    // Print to std::cout

    cal.print();

    // save to (reentrant qsettings object)

    QSettings settings;

    settings.setValue("calibration", QVariant::fromValue(cal));

    emit done();

}