Subversion Repositories seema-scanner

#include "SMCalibrationWorker.h"

#include "SMCalibrationParameters.h"

#include "cvtools.h"

#include <opencv2/aruco/charuco.hpp>

#include <QSettings>

#include <QTextStream>

#include <ceres/ceres.h>

// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points

// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014

// DTU, 2014, Jakob Wilm

static void rotationAxisEstimation(const std::vector< std::vector<cv::Point3f> > Qcam,

                                   const std::vector<cv::Point3f> Qobj,

                                   cv::Vec3f &axis, cv::Vec3f &point){

    assert(Qobj.size() == Qcam[0].size());

    // number of frames (points on each arch)

    auto l = Qcam.size();

    // number of points in each frame

    size_t mn = Qobj.size();

    // construct matrix for axis determination

    cv::Mat M(6, 6, CV_32F, cv::Scalar(0));

    for(int k=0; k<l; k++){

        for(unsigned int idx=0; idx<mn; idx++){

            //            float i = Qobj[idx].x+4;

            //            float j = Qobj[idx].y+4;

            float i = Qobj[idx].x;

            float j = Qobj[idx].y;

            float x = Qcam[k][idx].x;

            float y = Qcam[k][idx].y;

            float z = Qcam[k][idx].z;

            M += (cv::Mat_<float>(6,6) << x*x, x*y, x*z, x, i*x, j*x,

                  0, y*y, y*z, y, i*y, j*y,

                  0,   0, z*z, z, i*z, j*z,

                  0,   0,   0, 1,   i,   j,

                  0,   0,   0, 0, i*i, i*j,

                  0,   0,   0, 0,   0, j*j);

        }

    }

    cv::completeSymm(M, false);

    // solve for axis

    std::vector<float> lambda;

    cv::Mat u;

    cv::eigen(M, lambda, u);

    float minLambda = std::abs(lambda[0]);

    int idx = 0;

    for(unsigned int i=1; i<lambda.size(); i++){

        if(abs(lambda[i]) < minLambda){

            minLambda = lambda[i];

            idx = i;

        }

    }

    axis = u.row(idx).colRange(0, 3);

    axis = cv::normalize(axis);

    float nx = u.at<float>(idx, 0);

    float ny = u.at<float>(idx, 1);

    float nz = u.at<float>(idx, 2);

    //float d  = u.at<float>(idx, 3);

    float dh = u.at<float>(idx, 4);

    float dv = u.at<float>(idx, 5);

    // Paper version: c is initially eliminated

    /*cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));

        cv::Mat bb(l*mn, 1, CV_32F);

        for(int k=0; k<l; k++){

            for(unsigned int idx=0; idx<mn; idx++){

                float i = Qobj[idx].x;

                float j = Qobj[idx].y;

                float x = Qcam[k][idx].x;

                float y = Qcam[k][idx].y;

                float z = Qcam[k][idx].z;

                float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);

                int row = k*mn+idx;

                A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;

                A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;

                A.at<float>(row, idx+2) = 1.0;

                bb.at<float>(row, 0) = f + (2*z*d)/nz;

            }

        }

        // solve for point

        cv::Mat abe;

        cv::solve(A, bb, abe, cv::DECOMP_SVD);

        float a = abe.at<float>(0, 0);

        float b = abe.at<float>(1, 0);

        float c = -(nx*a+ny*b+d)/nz;

        */

    // Our version: solve simultanously for a,b,c

    cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));

    cv::Mat bb(l*mn, 1, CV_32F);

    for(int k=0; k<l; k++){

        for(unsigned int idx=0; idx<mn; idx++){

            float i = Qobj[idx].x;

            float j = Qobj[idx].y;

            float x = Qcam[k][idx].x;

            float y = Qcam[k][idx].y;

            float z = Qcam[k][idx].z;

            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);

            int row = k*mn+idx;

            A.at<float>(row, 0) = 2*x;

            A.at<float>(row, 1) = 2*y;

            A.at<float>(row, 2) = 2*z;

            A.at<float>(row, idx+3) = 1.0;

            bb.at<float>(row, 0) = f;

        }

    }

    // solve for point

    cv::Mat abe;

    cv::solve(A, bb, abe, cv::DECOMP_SVD);

    float a = abe.at<float>(0, 0);

    float b = abe.at<float>(1, 0);

    float c = abe.at<float>(2, 0);

    point[0] = a;

    point[1] = b;

    point[2] = c;

}

struct CircleResidual {

    CircleResidual(std::vector<cv::Point3f> _pointsOnArc)

        : pointsOnArc(_pointsOnArc) {}

    template <typename T>

    bool operator()(const T* point, const T* axis, T* residual) const {

        T axisSqNorm = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];

        unsigned int l = pointsOnArc.size();

        std::vector<T> dI(l);

        // note, this is automatically initialized to 0

        T sum(0.0);

        for(unsigned int i=0; i<l; i++){

            cv::Point3d p = pointsOnArc[i];

            //T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};

            // point to line distance

            T dotProd = (point[0]-p.x)*axis[0] + (point[1]-p.y)*axis[1] + (point[2]-p.z)*axis[2];

            T dIx = point[0] - p.x - (dotProd*axis[0]/axisSqNorm);

            T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);

            T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);

            dI[i] = ceres::sqrt(dIx*dIx + dIy*dIy + dIz*dIz);

            sum += dI[i];

        }

        T mean = sum / double(l);

        for(unsigned int i=0; i<l; i++){

            residual[i] = dI[i] - mean;

        }

        return true;

    }

    private:

        // Observations for one checkerboard corner.

        const std::vector<cv::Point3f> pointsOnArc;

};

static void rotationAxisOptimization(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point, float &error){

    // number of frames (points on each arch)

    size_t l = Qcam.size();

    // number of points in each frame

    size_t mn = Qobj.size();

    // read initial guess

    double pointArray[] = {point[0], point[1], point[2]};

    double axisArray[] = {axis[0], axis[1], axis[2]};

    ceres::Problem problem;

    // loop through saddle points

    for(unsigned int idx=0; idx<mn; idx++){

        std::vector<cv::Point3f> pointsOnArch(l);

        for(unsigned int k=0; k<l; k++){

            pointsOnArch[k] = Qcam[k][idx];

        }

        ceres::CostFunction* cost_function =

                new ceres::AutoDiffCostFunction<CircleResidual, ceres::DYNAMIC, 3, 3>(

                    new CircleResidual(pointsOnArch), l);

        problem.AddResidualBlock(cost_function, NULL, pointArray, axisArray);

    }

    // Run the solver!

    ceres::Solver::Options options;

    options.linear_solver_type = ceres::DENSE_QR;

    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;

    ceres::Solve(options, &problem, &summary);

    std::cout << summary.BriefReport() << "\n";

    point = cv::Vec3f(pointArray[0], pointArray[1], pointArray[2]);

    axis = cv::Vec3f(axisArray[0], axisArray[1], axisArray[2]);

    axis /= cv::norm(axis);

    // Error estimate (sum of squared differences)

    error = 0;

    // loop through saddle points

    for(unsigned int idx=0; idx<mn; idx++){

        // vector of distances from rotation axis

        std::vector<float> dI(l);

        // loop through angular positions

        for(unsigned int k=0; k<l; k++){

            cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);

            // point to line distance

            dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);

        }

        float sum = std::accumulate(dI.begin(), dI.end(), 0.0);

        float mean = sum / dI.size();

        float meanDev = 0;

        for(unsigned int k=0; k<l; k++){

            meanDev += std::abs(dI[k] - mean);

        }

        meanDev /= l;

        error += meanDev;

    }

    error /= mn;

}

static std::vector<cv::Point3f> generateWorldCoords(const cv::Size checkerCount, const float checkerSize){

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

    return Qi;

}

static bool detectCheckerBoardCorners(const cv::Size & checkerCount,

                                    const cv::Mat & frame,

                                    cv::Mat & frameResult,

                                    std::vector<cv::Point2f> & qc){

    // Convert to grayscale

    cv::Mat gray;

    if(frame.channels() == 1)

        cv::cvtColor(frame, gray, CV_BayerBG2GRAY);

    else

        cv::cvtColor(frame, gray, CV_RGB2GRAY);

    // Extract checker corners

    bool success = cv::findChessboardCorners(gray, checkerCount, qc, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);

    if(success){

        cv::cornerSubPix(gray, qc, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));

        // Draw colored chessboard

        if(frame.channels() == 1)

            cv::cvtColor(frame, frameResult, CV_BayerBG2RGB);

        else

            frameResult = frame.clone();

        cvtools::drawChessboardCorners(frameResult, checkerCount, qc, success, 10);

    } else {

        qc.clear();

    }

    return success;

}

void SMCalibrationWorker::checkerboardDetection(SMCalibrationSet calibrationSet){

    QSettings settings;

    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));

    bool success0 = detectCheckerBoardCorners(checkerCount, calibrationSet.frame0, calibrationSet.frame0Result, calibrationSet.qc0);

    if(!success0){

//        calibrationSet.qc0.clear();

        emit logMessage(QString("Could not detect checkerboard on set %1 camera0").arg(calibrationSet.id));

        std::cerr << "Could not detect checkerboard on set " << calibrationSet.id << " camera0" << std::endl;

    }

    bool success1 = detectCheckerBoardCorners(checkerCount, calibrationSet.frame1, calibrationSet.frame1Result, calibrationSet.qc1);

    if(!success1){

//        calibrationSet.qc1.clear();

        emit logMessage(QString("Could not detect checkerboard on set %1 camera1").arg(calibrationSet.id));

        std::cerr << "Could not detect checkerboard on set " << calibrationSet.id << " camera1" << std::endl;

    }

    emit newCheckerboardResult(calibrationSet.id, calibrationSet);

}

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

    QSettings settings;

    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));

    unsigned int nSets = calibrationData.size();

    // 2D Points collected for OpenCV's calibration procedures

    std::vector< std::vector<cv::Point2f> > qc0, qc1, qc0Stereo, qc1Stereo;

    for(unsigned int i=0; i<nSets; i++){

        if(!calibrationData[i].selected)

            continue;

        // Note: avoiding push_back has only minor theoretical value

        if(!calibrationData[i].qc0.empty())

            qc0.push_back(calibrationData[i].qc0);

        if(!calibrationData[i].qc1.empty())

            qc1.push_back(calibrationData[i].qc1);

        if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){

            qc0Stereo.push_back(calibrationData[i].qc0);

            qc1Stereo.push_back(calibrationData[i].qc1);

        }

    }

    // Generate world object coordinates [mm]

    std::vector<cv::Point3f> Qi = generateWorldCoords(checkerCount, settings.value("calibration/squareSize", 10.0).toFloat());

    std::vector< std::vector<cv::Point3f> > Q0(qc0.size(), Qi), Q1(qc1.size(), Qi), QStereo(qc0Stereo.size(), 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);

    // determine only k1, k2 for lens distortion

    int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;

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

    cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, cal.cam0_intrinsic_std, cal.cam0_extrinsic_std, cal.cam0_errors_per_view, flags,

                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));

    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;

    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, cal.cam1_intrinsic_std, cal.cam1_extrinsic_std, cal.cam1_errors_per_view, flags,

                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));

    // Stereo calibration

    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;

    cv::Mat E, F, R1, T1;

    #if CV_MAJOR_VERSION < 3

        cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,

                                               frameSize, R1, T1, E, F,

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

                                               flags_stereo);

    #else

        cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,

                                               frameSize, R1, T1, E, F, flags_stereo,

                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));

    #endif

    cal.R1 = R1;

    cal.T1 = T1;

    cal.E = E;

    cal.F = F;

    // Print to log

    std::stringstream out;

    out << "## Camera Calibration ##" << std::endl

        << "No. images used for intrinsics of cam0: " << qc0.size() << std::endl

        << "No. images used for intrinsics of cam1: " << qc1.size() << std::endl

        << "No. images used for stereo calibration: " << qc0Stereo.size() << std::endl;

    cal.printCamera(out);

    out << std::endl << std::endl;

    emit logMessage(QString::fromStdString(out.str()));

    // save to (reentrant) qsettings object

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

    emit done();

}

static void drawDetectedMarkersCharuco(cv::InputOutputArray _image, cv::InputArrayOfArrays _corners,

                                cv::InputArray _ids, cv::Scalar borderColor) {

    using namespace cv;

    using namespace std;

    CV_Assert(_image.getMat().total() != 0 &&

              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));

    CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0);

    // calculate colors

    Scalar textColor, cornerColor;

    textColor = cornerColor = borderColor;

    swap(textColor.val[0], textColor.val[1]);     // text color just sawp G and R

    swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B

    int nMarkers = (int)_corners.total();

    for(int i = 0; i < nMarkers; i++) {

        Mat currentMarker = _corners.getMat(i);

        CV_Assert(currentMarker.total() == 4 && currentMarker.type() == CV_32FC2);

        // draw marker sides

        for(int j = 0; j < 4; j++) {

            Point2f p0, p1;

            p0 = currentMarker.ptr< Point2f >(0)[j];

            p1 = currentMarker.ptr< Point2f >(0)[(j + 1) % 4];

            line(_image, p0, p1, borderColor, 2, LINE_AA);

        }

        // draw first corner mark

        rectangle(_image, currentMarker.ptr< Point2f >(0)[0] - Point2f(3, 3),

                  currentMarker.ptr< Point2f >(0)[0] + Point2f(3, 3), cornerColor, 1, LINE_AA);

        // draw ID

        if(_ids.total() != 0) {

            Point2f cent(0, 0);

            for(int p = 0; p < 4; p++)

                cent += currentMarker.ptr< Point2f >(0)[p];

            cent = cent / 4.;

            stringstream s;

            s << _ids.getMat().ptr< int >(0)[i];

            putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 1, textColor, 2, LINE_AA);

        }

    }

}

static void drawDetectedCornersCharuco(cv::InputOutputArray _image, cv::InputArray _charucoCorners,

                                       cv::InputArray _charucoIds, cv::Scalar cornerColor) {

    using namespace cv;

    using namespace std;

    CV_Assert(_image.getMat().total() != 0 &&

              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));

    CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()) ||

              _charucoIds.getMat().total() == 0);

    unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();

    for(unsigned int i = 0; i < nCorners; i++) {

        Point2f corner = _charucoCorners.getMat().at< Point2f >(i);

        // draw first corner mark

        rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 2, LINE_AA);

        // draw ID

        if(_charucoIds.total() != 0) {

            int id = _charucoIds.getMat().at< int >(i);

            stringstream s;

            s << id;

            putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 1.0,

                    cornerColor, 4, cv::LINE_AA);

        }

    }

}

static bool detectCheckerBoardCornersCharuco(const cv::Mat & frame,

                                            const cv::Ptr<cv::aruco::Dictionary> dict,

                                            const cv::Ptr<cv::aruco::CharucoBoard> board,

                                            const cv::Ptr<cv::aruco::DetectorParameters> parameters,

                                            cv::Mat& frameResult,

                                            std::vector<cv::Point2f>& qc,

                                            std::vector<int>& qci){

    // Convert to grayscale

    cv::Mat gray;

    if(frame.channels() == 1)

        cv::cvtColor(frame, gray, CV_BayerBG2GRAY);

    else

        cv::cvtColor(frame, gray, CV_RGB2GRAY);

    std::vector<std::vector<cv::Point2f>> mc;

    std::vector<int> mi;

    cv::Mat grayBlurred;

    cv::GaussianBlur(gray, grayBlurred, cv::Size(0, 0), 3);

    // Extract checker corners

    cv::aruco::detectMarkers(grayBlurred, dict, mc, mi, parameters);

    //std::vector<std::vector<cv::Point2f>> mc, rejecti;

    //cv::aruco::refineDetectedMarkers(gray0, board, mc0, mi0, rejecti0);

    //cv::Mat cci, cidsi;

    cv::aruco::interpolateCornersCharuco(mc, mi, gray, board, qc, qci, cv::noArray(), cv::noArray(), 1);

    bool success = (qc.size() >= 4);

    if(success){

        cv::cornerSubPix(gray, qc, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));

        // Draw colored chessboard

        if(frame.channels() == 1)

            cv::cvtColor(frame, frameResult, CV_BayerBG2RGB);

        else

            frameResult = frame.clone();

        // Draw colored chessboard

        drawDetectedMarkersCharuco(frameResult, mc, mi, cv::Scalar(0, 255, 0));

        drawDetectedCornersCharuco(frameResult, qc, qci, cv::Scalar(0, 255, 0));

    } else {

        qc.clear();

    }

    return success;

}

void SMCalibrationWorker::checkerboardDetectionCharuco(SMCalibrationSet calibrationSet){

    QSettings settings;

    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));

    const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();

    float markerLength = 0.8f*checkerSize;

    // Charuco board and dictionary

    cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

    cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);

    //    cv::Mat boardImg;

    //    board->draw(cv::Size((saddlePointCountX+1)*100, (saddlePointCountY+1)*100), boardImg, 0, 1);

    //    cv::Mat boardImgRes;

    //    cv::resize(boardImg, boardImgRes, cv::Size((saddlePointCountX+1)*10, (saddlePointCountY+1)*10), 0, 0, cv::INTER_NEAREST);

    //    cv::imwrite("boardImg.png", boardImg);

    //    cv::imwrite("boardImgRes.png", boardImgRes);

    cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();

//    parameters->adaptiveThreshWinSizeMin = 3;

//    parameters->adaptiveThreshWinSizeMax = 153;

//    parameters->adaptiveThreshWinSizeStep = 5;

//    parameters->adaptiveThreshConstant = 20;

//    parameters->minMarkerPerimeterRate = 0.01;

    parameters->maxMarkerPerimeterRate = 5.0;

//    parameters->minCornerDistanceRate = 0.10;

//    parameters->perspectiveRemovePixelPerCell = 6;

//    parameters->maxErroneousBitsInBorderRate = 0.8;

//    parameters->minOtsuStdDev = 1;

//    parameters->errorCorrectionRate = 0.1;

    parameters->cornerRefinementWinSize = 10;

    bool success0 = detectCheckerBoardCornersCharuco(calibrationSet.frame0, dict, board, parameters, calibrationSet.frame0Result, calibrationSet.qc0, calibrationSet.qc0id);

    if(!success0){

        emit logMessage(QString("Could not detect  Charuco corners on set %1 camera0").arg(calibrationSet.id));

        std::cerr << "Could not detect Charuco corners on set " << calibrationSet.id << " camera0" << std::endl;

    }

    bool success1 = detectCheckerBoardCornersCharuco(calibrationSet.frame1, dict, board, parameters, calibrationSet.frame1Result, calibrationSet.qc1, calibrationSet.qc1id);

    if(!success1){

        emit logMessage(QString("Could not detect Charuco corners on set %1 camera1").arg(calibrationSet.id));

        std::cerr << "Could not detect Charuco corners on set " << calibrationSet.id << " camera1" << std::endl;

    }

    emit newCheckerboardResult(calibrationSet.id, calibrationSet);

}

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

    QSettings settings;

    // Number of saddle points on calibration pattern

    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));

    const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();

    float markerLength = 0.8f*checkerSize;

    cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

    cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);

    auto nSets = calibrationData.size();

    // 2D Points collected for OpenCV's calibration procedures

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

    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;

    // 3D object points

    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;

    // Loop through calibration sets

    for(unsigned int i=0; i<nSets; i++){

        if(!calibrationData[i].selected)

            continue;

        bool success0 = !calibrationData[i].qc0.empty();

        bool success1 = !calibrationData[i].qc1.empty();

        if(success0){

            std::vector<cv::Point3f> Q0i;

            for(std::size_t j=0; j<calibrationData[i].qc0id.size(); j++){

                int id = calibrationData[i].qc0id[j];

                Q0i.push_back(board->chessboardCorners[id]);

            }

            qc0.push_back(calibrationData[i].qc0);

            Q0.push_back(Q0i);

        }

        if(success1){

            std::vector<cv::Point3f> Q1i;

            for(std::size_t j=0; j<calibrationData[i].qc1id.size(); j++){

                int id = calibrationData[i].qc1id[j];

                Q1i.push_back(board->chessboardCorners[id]);

            }

            qc1.push_back(calibrationData[i].qc1);

            Q1.push_back(Q1i);

        }

        if(success0 && success1){

            std::vector<cv::Point2f> qc0iStereo, qc1iStereo;

            std::vector<cv::Point3f> QiStereo;

            int j0 = 0;

            int j1 = 0;

            while(j0<calibrationData[i].qc0.size() && j1<calibrationData[i].qc1.size()){

                int id0 = calibrationData[i].qc0id[j0];

                int id1 = calibrationData[i].qc1id[j1];

                if(id0 < id1)

                    j0++;

                else if (id1 < id0)

                    j1++;

                else{

                    assert(id0 == id1);

                    qc0iStereo.push_back(calibrationData[i].qc0[j0]);

                    qc1iStereo.push_back(calibrationData[i].qc1[j1]);

                    QiStereo.push_back(board->chessboardCorners[id0]);

                    j0++;

                    j1++;

                }

            }

            if(QiStereo.size() > 0){

                qc0Stereo.push_back(qc0iStereo);

                qc1Stereo.push_back(qc1iStereo);

                QStereo.push_back(QiStereo);

            }

        }

    }

    size_t nValidSets = qc0Stereo.size();

    if(nValidSets < 2){

        std::cerr << "Not enough valid calibration sequences!" << std::endl;

        emit done();

        return;

    }

    // calibrate the cameras

    SMCalibrationParameters cal;

    cal.frameWidth = calibrationData[0].frame0.cols;

    cal.frameHeight = calibrationData[0].frame0.rows;

    cv::Size frameSize(cal.frameWidth, cal.frameHeight);

    // determine only k1, k2 for lens distortion

    int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;

    // 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(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, cal.cam0_intrinsic_std, cal.cam0_extrinsic_std, cal.cam0_errors_per_view, flags,

                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));

    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;

    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, cal.cam1_intrinsic_std, cal.cam1_extrinsic_std, cal.cam1_errors_per_view, flags,

                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));

    // stereo calibration

    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;

    cv::Mat E, F, R1, T1;

    #if CV_MAJOR_VERSION < 3

        cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,

                                               frameSize, R1, T1, E, F,

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

                                               flags_stereo);

    #else

        cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,

                                               frameSize, R1, T1, E, F, flags_stereo,

                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));

    #endif

    cal.R1 = R1;

    cal.T1 = T1;

    cal.E = E;

    cal.F = F;

    // Print to log

    std::stringstream out;

    out << "## Camera Calibration ##" << std::endl

        << "No. images used for intrinsics of cam0: " << qc0.size() << std::endl

        << "No. images used for intrinsics of cam1: " << qc1.size() << std::endl

        << "No. images used for stereo calibration: " << qc0Stereo.size() << std::endl;

    cal.printCamera(out);

    out << std::endl << std::endl;

    emit logMessage(QString::fromStdString(out.str()));

    // save to (reentrant qsettings object)

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

    emit done();

}

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

    auto nSets = calibrationData.size();

    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;

    std::vector<cv::Point3f> Qi;

    QSettings settings;

    // Generate world object coordinates [mm]

    const cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(),settings.value("calibration/patternSizeY", 13).toInt()));

    const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();

    if(settings.value("calibration/method").toString() == "Charuco"){

        // Find id's which were detected in all images

        std::vector<int> ids;

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

            if(!calibrationData[i].selected)

                continue;

            if(ids.empty()){

                ids = calibrationData[i].qc0id;

            }

            std::vector<int> idsi0;

            std::set_intersection(ids.begin(), ids.end(), calibrationData[i].qc0id.begin(), calibrationData[i].qc0id.end(), std::back_inserter(idsi0));

            ids = idsi0;

            std::vector<int> idsi1;

            std::set_intersection(ids.begin(), ids.end(), calibrationData[i].qc1id.begin(), calibrationData[i].qc1id.end(), std::back_inserter(idsi1));

            ids = idsi1;

        }

        if(ids.size() < 2){

            std::cerr << "Not enough feature points present in all images for rotation axis determination!" << std::endl;

            emit logMessage("Not enough feature points present in all images for rotation axis determination! Deselect some calibration images.");

        }

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

            if(!calibrationData[i].selected)

                continue;

            std::vector<cv::Point2f> qc0StereoI, qc1StereoI;

            for(int j=0; j<ids.size(); j++){

                ptrdiff_t idx0 = std::find(calibrationData[i].qc0id.begin(), calibrationData[i].qc0id.end(), ids[j]) - calibrationData[i].qc0id.begin();

                ptrdiff_t idx1 = std::find(calibrationData[i].qc1id.begin(), calibrationData[i].qc1id.end(), ids[j]) - calibrationData[i].qc1id.begin();

                qc0StereoI.push_back(calibrationData[i].qc0[idx0]);

                qc1StereoI.push_back(calibrationData[i].qc1[idx1]);

            }

            qc0Stereo.push_back(qc0StereoI);

            qc1Stereo.push_back(qc1StereoI);

        }

        float markerLength = 0.8f*checkerSize;

        cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

        cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);

        for(int j=0; j<ids.size(); j++){

            Qi.push_back(board->chessboardCorners[ids[j]]);

        }

    } else {

        // Normal checkerboard with all corners

        for(auto i=0; i<nSets; i++){

            if(!calibrationData[i].selected)

                continue;

            if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){

                qc0Stereo.push_back(calibrationData[i].qc0);

                qc1Stereo.push_back(calibrationData[i].qc1);

            }

        }

        Qi = generateWorldCoords(checkerCount, checkerSize);

    }

    SMCalibrationParameters cal = settings.value("calibration/parameters").value<SMCalibrationParameters>();

    if(qc0Stereo.size() <= 2){

        std::cerr << "Not enough images for rotation axis determination!" << std::endl;

        emit logMessage("Not enough images for rotation axis determination!");