WebSVN – seema-scanner – Diff – /src/SMCalibrationWorker.cpp


#include "SMCalibrationWorker.h"
#include "SMCalibrationParameters.h"
 
#include "cvtools.h"
 
#include <QSettings>
 
#include <ceres/ceres.h>
 
 
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;
};
 
 
// 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 rotationAxisCalibration(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)
    int l = Qcam.size();
 
    // number of points in each frame
    size_t mn = Qobj.size();
 
    assert(mn == Qcam[0].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;
 
    // Non-linear optimization using Ceres
    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(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(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(int k=0; k<l; k++){
            meanDev += std::abs(dI[k] - mean);
        }
        meanDev /= l;
        error += meanDev;
    }
    error /= mn;
}
 
bool processCBCorners(const cv::Size & checkerCount,
                      const cv::Mat & SMCalibrationSetI_frameX,
                      cv::Mat & SMCalibrationSetI_frameXResult,
                      std::vector<cv::Point2f> & qciX){
    // Convert to grayscale
    cv::Mat gray;
    if(SMCalibrationSetI_frameX.channels() == 1)
        cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_BayerBG2GRAY);
    else
        cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_RGB2GRAY);
 
    // Extract checker corners
    bool success = cv::findChessboardCorners(gray, checkerCount, qciX, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
    if(success){
        cv::cornerSubPix(gray, qciX, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
        // Draw colored chessboard
        cv::Mat color;
        if(SMCalibrationSetI_frameX.channels() == 1)
            cv::cvtColor(SMCalibrationSetI_frameX, color, CV_BayerBG2RGB);
        else
            color = SMCalibrationSetI_frameX.clone();
 
        cvtools::drawChessboardCorners(color, checkerCount, qciX, success, 10);
        SMCalibrationSetI_frameXResult = color;
    }
    return success;
}
 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
 
    QSettings settings;
 
    // Number of saddle points on calibration pattern
    int checkerCountX = settings.value("calibration/patternSizeX", 22).toInt();
    int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();
    cv::Size checkerCount(checkerCountX, checkerCountY);
 
    unsigned int 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;
 
    std::vector<bool> success0(nSets), success1(nSets);
 
    std::vector<float> angles;
 
    // Loop through calibration sets
    for(unsigned int i=0; i<nSets; i++){
 
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
 
        if(!SMCalibrationSetI.checked)
            continue;
 
        // Camera 0
        std::vector<cv::Point2f> qci0;
 
        // Convert to grayscale
        cv::Mat gray;
        if(SMCalibrationSetI.frame0.channels() == 1)
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);
        else
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
 
        // Extract checker corners
        success0[i] = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success0[i]){
            cv::cornerSubPix(gray, qci0, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
            // Draw colored chessboard
            cv::Mat color;
            if(SMCalibrationSetI.frame0.channels() == 1)
                cv::cvtColor(SMCalibrationSetI.frame0, color, CV_BayerBG2RGB);
            else
                color = SMCalibrationSetI.frame0.clone();
 
            cvtools::drawChessboardCorners(color, checkerCount, qci0, success0[i], 10);
            SMCalibrationSetI.frame0Result = color;
        }
 
        emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);
 
        // Camera 1
        std::vector<cv::Point2f> qci1;
 
        // Convert to grayscale
        if(SMCalibrationSetI.frame1.channels() == 1)
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);
        else
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
 
        // Extract checker corners
        success1[i] = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
        if(success1[i]){
            cv::cornerSubPix(gray, qci1, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
            // Draw colored chessboard
            cv::Mat color;
            if(SMCalibrationSetI.frame1.channels() == 1)
                cv::cvtColor(SMCalibrationSetI.frame1, color, CV_BayerBG2RGB);
            else
                color = SMCalibrationSetI.frame1.clone();
 
            cvtools::drawChessboardCorners(color, checkerCount, qci1, success1[i], 10);
            SMCalibrationSetI.frame1Result = color;
        }
 
        emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);
 
        if(success0[i])
            qc0.push_back(qci0);
 
        if(success1[i])
            qc1.push_back(qci1);
 
        if(success0[i] && success1[i]){
            qc0Stereo.push_back(qci0);
            qc1Stereo.push_back(qci1);
            angles.push_back(SMCalibrationSetI.rotationAngle);
        }
 
        // Show progress
        emit newSetProcessed(i);
    }
 
    size_t nValidSets = angles.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/squareSize", 10.0).toFloat(); // width and height of one checker square 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> > Q0, Q1, QStereo;
    for(unsigned int i=0; i<qc0.size(); i++)
        Q0.push_back(Qi);
    for(unsigned int i=0; i<qc1.size(); i++)
        Q1.push_back(Qi);
    for(unsigned int i=0; i<nValidSets; i++)
        QStereo.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);
 
    // 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, flags,
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
//std::cout << cal.k0 << std::endl;
//    // refine extrinsics for camera 0
//    for(int i=0; i<Q.size(); i++)
//        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);
 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
//std::cout << cal.k1 << std::endl;
    // 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;
    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));
 
 
    cal.R1 = R1;
    cal.T1 = T1;
    cal.E = E;
    cal.F = F;
 
    // Determine per-view reprojection errors:
    cal.cam0_errors_per_view.resize(nSets);
    int idx = 0;
    for(unsigned int i = 0; i < nSets; ++i){
        if(success0[i]){
            int n = (int)Q0[idx].size();
            std::vector<cv::Point2f> qc_proj;
            cv::projectPoints(cv::Mat(Q0[idx]), cam_rvecs0[idx], cam_tvecs0[idx], cal.K0,  cal.k0, qc_proj);
            float err = 0;
            for(unsigned int j=0; j<qc_proj.size(); j++){
                cv::Point2f d = qc0[idx][j] - qc_proj[j];
                err += cv::sqrt(d.x*d.x + d.y*d.y);
            }
            cal.cam0_errors_per_view[i] = (float)err/n;
            idx++;
        } else
            cal.cam0_errors_per_view[i] = NAN;
    }
    cal.cam1_errors_per_view.resize(nSets);
    idx = 0;
    for(unsigned int i = 0; i < nSets; ++i){
        if(success1[i]){
            int n = (int)Q1[idx].size();
            std::vector<cv::Point2f> qc_proj;
            cv::projectPoints(cv::Mat(Q1[idx]), cam_rvecs1[idx], cam_tvecs1[idx], cal.K1,  cal.k1, qc_proj);
            float err = 0;
            for(unsigned int j=0; j<qc_proj.size(); j++){
                cv::Point2f d = qc1[idx][j] - qc_proj[j];
                err += cv::sqrt(d.x*d.x + d.y*d.y);
            }
            cal.cam1_errors_per_view[i] = (float)err/n;
            idx++;
       } else
            cal.cam1_errors_per_view[i] = NAN;
    }
 
//    // hand-eye calibration
//    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::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
 
//    for(int i=0; i<nValidSets-1; i++){
//        std::cout << i << std::endl;
 
//        cv::Mat Rci;
//        cv::Rodrigues(Rc[i], Rci);
//        std::cout << "Rc[i]" << Rci << std::endl;
//        std::cout << "Tc[i]" << Tc[i] << std::endl;
 
//        cv::Mat Rri;
//        cv::Rodrigues(Rr[i], Rri);
//        std::cout << "Rr[i]" << Rri << std::endl;
//        std::cout << "Tr[i]" << Tr[i] << std::endl;
 
//        cv::Mat Rcr = cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t());
//        cv::Rodrigues(Rcr, Rcr);
//        cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
//        std::cout << "Rcr[i]" << Rcr << std::endl;
//        std::cout << "Tcr[i]" << Tcr << std::endl;
//        std::cout << std::endl;
//    }
 
 
    // Direct rotation axis calibration //
    // full camera matrices
    cv::Matx34f P0 = cv::Matx34f::eye();
    cv::Mat RT1(3, 4, CV_32F);
    cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
    cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
    cv::Matx34f P1 = cv::Matx34f(RT1);
 
    // calibration points in camera 0 frame
    std::vector< std::vector<cv::Point3f> > Qcam;
 
    for(unsigned int i=0; i<nValidSets; i++){
        std::vector<cv::Point2f> qc0i, qc1i;
 
        cv::undistortPoints(qc0Stereo[i], qc0i, cal.K0, cal.k0);
        cv::undistortPoints(qc1Stereo[i], qc1i, cal.K1, cal.k1);
//        qc0i = qc0[i];
//        qc1i = qc1[i];
 
        cv::Mat Qhom, Qcami;
        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
        std::vector<cv::Point3f> QcamiPoints;
        cvtools::matToPoints3f(Qcami, QcamiPoints);
 
        Qcam.push_back(QcamiPoints);
    }
 
//    cv::Mat QcamMat(Qcam.size(), Qcam[0].size(), CV_32FC3);
//    for(int i=0; i<Qcam.size(); i++)
//        for(int j=0; j<Qcam[0].size(); j++)
//            QcamMat.at<cv::Point3f>(i,j) = Qcam[i][j];
//    cvtools::writeMat(QcamMat, "Qcam.mat", "Qcam");
 
    cv::Vec3f axis, point;
    float rot_axis_error;
    rotationAxisCalibration(Qcam, Qi, axis, point, rot_axis_error);
 
    // construct transformation matrix
    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
    ex = cv::normalize(ex);
    cv::Vec3f ez = ex.cross(axis);
    ez = cv::normalize(ez);
 
    cv::Mat RrMat(3, 3, CV_32F);
    cv::Mat(ex).copyTo(RrMat.col(0));
    cv::Mat(axis).copyTo(RrMat.col(1));
    cv::Mat(ez).copyTo(RrMat.col(2));
 
    cal.Rr = cv::Matx33f(RrMat).t();
    cal.Tr = -cv::Matx33f(RrMat).t()*point;
    cal.rot_axis_error = rot_axis_error;
 
    // Print to std::cout
    cal.print();
 
    // save to (reentrant qsettings object)
    settings.setValue("calibration/parameters", QVariant::fromValue(cal));
 
    emit done();
 
}
 

Rev 198	Rev 206
1	`#include "SMCalibrationWorker.h"`	1	`#include "SMCalibrationWorker.h"`
2	`#include "SMCalibrationParameters.h"`	2	`#include "SMCalibrationParameters.h"`
3		3
4	`#include "cvtools.h"`	4	`#include "cvtools.h"`
5		5
6	`#include <QSettings>`	6	`#include <QSettings>`
7		7
8	`#include <ceres/ceres.h>`	8	`#include <ceres/ceres.h>`
9		9
10		10
11	`struct CircleResidual {`	11	`struct CircleResidual {`
12	`CircleResidual(std::vector<cv::Point3f> _pointsOnArc)`	12	`CircleResidual(std::vector<cv::Point3f> _pointsOnArc)`
13	`: pointsOnArc(_pointsOnArc) {}`	13	`: pointsOnArc(_pointsOnArc) {}`
14		14
15	`template <typename T>`	15	`template <typename T>`
16	`bool operator()(const T* point, const T* axis, T* residual) const {`	16	`bool operator()(const T* point, const T* axis, T* residual) const {`
17		17
18	`T axisSqNorm = axis[0]axis[0] + axis[1]axis[1] + axis[2]*axis[2];`	18	`T axisSqNorm = axis[0]axis[0] + axis[1]axis[1] + axis[2]*axis[2];`
19		19
20	`unsigned int l = pointsOnArc.size();`	20	`unsigned int l = pointsOnArc.size();`
21	`std::vector<T> dI(l);`	21	`std::vector<T> dI(l);`
22		22
23	`// note, this is automatically initialized to 0`	23	`// note, this is automatically initialized to 0`
24	`T sum(0.0);`	24	`T sum(0.0);`
25		25
26	`for(unsigned int i=0; i<l; i++){`	26	`for(unsigned int i=0; i<l; i++){`
27	`cv::Point3d p = pointsOnArc[i];`	27	`cv::Point3d p = pointsOnArc[i];`
28	`//T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};`	28	`//T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};`
29		29
30	`// point to line distance`	30	`// point to line distance`
31	`T dotProd = (point[0]-p.x)axis[0] + (point[1]-p.y)axis[1] + (point[2]-p.z)*axis[2];`	31	`T dotProd = (point[0]-p.x)axis[0] + (point[1]-p.y)axis[1] + (point[2]-p.z)*axis[2];`
32	`T dIx = point[0] - p.x - (dotProd*axis[0]/axisSqNorm);`	32	`T dIx = point[0] - p.x - (dotProd*axis[0]/axisSqNorm);`
33	`T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);`	33	`T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);`
34	`T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);`	34	`T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);`
35	`dI[i] = ceres::sqrt(dIxdIx + dIydIy + dIz*dIz);`	35	`dI[i] = ceres::sqrt(dIxdIx + dIydIy + dIz*dIz);`
36		36
37	`sum += dI[i];`	37	`sum += dI[i];`
38	`}`	38	`}`
39		39
40	`T mean = sum / double(l);`	40	`T mean = sum / double(l);`
41		41
42	`for(unsigned int i=0; i<l; i++){`	42	`for(unsigned int i=0; i<l; i++){`
43	`residual[i] = dI[i] - mean;`	43	`residual[i] = dI[i] - mean;`
44	`}`	44	`}`
45		45
46	`return true;`	46	`return true;`
47	`}`	47	`}`
48		48
49	`private:`	49	`private:`
50		50
51	`// Observations for one checkerboard corner.`	51	`// Observations for one checkerboard corner.`
52	`const std::vector<cv::Point3f> pointsOnArc;`	52	`const std::vector<cv::Point3f> pointsOnArc;`
53	`};`	53	`};`
54		54
55		55
56	`// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points`	56	`// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points`
57	`// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014`	57	`// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014`
58	`// DTU, 2014, Jakob Wilm`	58	`// DTU, 2014, Jakob Wilm`
59	`static void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point, float &error){`	59	`static void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam,`
-		60	`const std::vector<cv::Point3f> Qobj,`
-		61	`cv::Vec3f &axis, cv::Vec3f &point, float &error){`
60		62
61	`// number of frames (points on each arch)`	63	`// number of frames (points on each arch)`
62	`int l = Qcam.size();`	64	`int l = Qcam.size();`
63		65
64	`// number of points in each frame`	66	`// number of points in each frame`
65	`size_t mn = Qobj.size();`	67	`size_t mn = Qobj.size();`
66		68
67	`assert(mn == Qcam[0].size());`	69	`assert(mn == Qcam[0].size());`
68		70
69	`// construct matrix for axis determination`	71	`// construct matrix for axis determination`
70	`cv::Mat M(6, 6, CV_32F, cv::Scalar(0));`	72	`cv::Mat M(6, 6, CV_32F, cv::Scalar(0));`
71		73
72	`for(int k=0; k<l; k++){`	74	`for(int k=0; k<l; k++){`
73	`for(unsigned int idx=0; idx<mn; idx++){`	75	`for(unsigned int idx=0; idx<mn; idx++){`
74		76
75	`// float i = Qobj[idx].x+4;`	77	`// float i = Qobj[idx].x+4;`
76	`// float j = Qobj[idx].y+4;`	78	`// float j = Qobj[idx].y+4;`
77	`float i = Qobj[idx].x;`	79	`float i = Qobj[idx].x;`
78	`float j = Qobj[idx].y;`	80	`float j = Qobj[idx].y;`
79		81
80	`float x = Qcam[k][idx].x;`	82	`float x = Qcam[k][idx].x;`
81	`float y = Qcam[k][idx].y;`	83	`float y = Qcam[k][idx].y;`
82	`float z = Qcam[k][idx].z;`	84	`float z = Qcam[k][idx].z;`
83		85
84	`M += (cv::Mat_<float>(6,6) << xx, xy, xz, x, ix, j*x,`	86	`M += (cv::Mat_<float>(6,6) << xx, xy, xz, x, ix, j*x,`
85	`0, yy, yz, y, iy, jy,`	87	`0, yy, yz, y, iy, jy,`
86	`0, 0, zz, z, iz, j*z,`	88	`0, 0, zz, z, iz, j*z,`
87	`0, 0, 0, 1, i, j,`	89	`0, 0, 0, 1, i, j,`
88	`0, 0, 0, 0, ii, ij,`	90	`0, 0, 0, 0, ii, ij,`
89	`0, 0, 0, 0, 0, j*j);`	91	`0, 0, 0, 0, 0, j*j);`
90		92
91	`}`	93	`}`
92	`}`	94	`}`
93		95
94	`cv::completeSymm(M, false);`	96	`cv::completeSymm(M, false);`
95		97
96	`// solve for axis`	98	`// solve for axis`
97	`std::vector<float> lambda;`	99	`std::vector<float> lambda;`
98	`cv::Mat u;`	100	`cv::Mat u;`
99	`cv::eigen(M, lambda, u);`	101	`cv::eigen(M, lambda, u);`
100		102
101	`float minLambda = std::abs(lambda[0]);`	103	`float minLambda = std::abs(lambda[0]);`
102	`int idx = 0;`	104	`int idx = 0;`
103	`for(unsigned int i=1; i<lambda.size(); i++){`	105	`for(unsigned int i=1; i<lambda.size(); i++){`
104	`if(abs(lambda[i]) < minLambda){`	106	`if(abs(lambda[i]) < minLambda){`
105	`minLambda = lambda[i];`	107	`minLambda = lambda[i];`
106	`idx = i;`	108	`idx = i;`
107	`}`	109	`}`
108	`}`	110	`}`
109		111
110	`axis = u.row(idx).colRange(0, 3);`	112	`axis = u.row(idx).colRange(0, 3);`
111	`axis = cv::normalize(axis);`	113	`axis = cv::normalize(axis);`
112		114
113	`float nx = u.at<float>(idx, 0);`	115	`float nx = u.at<float>(idx, 0);`
114	`float ny = u.at<float>(idx, 1);`	116	`float ny = u.at<float>(idx, 1);`
115	`float nz = u.at<float>(idx, 2);`	117	`float nz = u.at<float>(idx, 2);`
116	`//float d = u.at<float>(idx, 3);`	118	`//float d = u.at<float>(idx, 3);`
117	`float dh = u.at<float>(idx, 4);`	119	`float dh = u.at<float>(idx, 4);`
118	`float dv = u.at<float>(idx, 5);`	120	`float dv = u.at<float>(idx, 5);`
119		121
120	`// // Paper version: c is initially eliminated`	122	`// // Paper version: c is initially eliminated`
121	`// cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));`	123	`// cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));`
122	`// cv::Mat bb(l*mn, 1, CV_32F);`	124	`// cv::Mat bb(l*mn, 1, CV_32F);`
123		125
124	`// for(int k=0; k<l; k++){`	126	`// for(int k=0; k<l; k++){`
125	`// for(unsigned int idx=0; idx<mn; idx++){`	127	`// for(unsigned int idx=0; idx<mn; idx++){`
126		128
127	`// float i = Qobj[idx].x;`	129	`// float i = Qobj[idx].x;`
128	`// float j = Qobj[idx].y;`	130	`// float j = Qobj[idx].y;`
129		131
130	`// float x = Qcam[k][idx].x;`	132	`// float x = Qcam[k][idx].x;`
131	`// float y = Qcam[k][idx].y;`	133	`// float y = Qcam[k][idx].y;`
132	`// float z = Qcam[k][idx].z;`	134	`// float z = Qcam[k][idx].z;`
133		135
134	`// float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`	136	`// float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`
135		137
136	`// int row = k*mn+idx;`	138	`// int row = k*mn+idx;`
137	`// A.at<float>(row, 0) = 2x - (2z*nx)/nz;`	139	`// A.at<float>(row, 0) = 2x - (2z*nx)/nz;`
138	`// A.at<float>(row, 1) = 2y - (2z*ny)/nz;`	140	`// A.at<float>(row, 1) = 2y - (2z*ny)/nz;`
139	`// A.at<float>(row, idx+2) = 1.0;`	141	`// A.at<float>(row, idx+2) = 1.0;`
140		142
141	`// bb.at<float>(row, 0) = f + (2zd)/nz;`	143	`// bb.at<float>(row, 0) = f + (2zd)/nz;`
142	`// }`	144	`// }`
143	`// }`	145	`// }`
144		146
145	`// // solve for point`	147	`// // solve for point`
146	`// cv::Mat abe;`	148	`// cv::Mat abe;`
147	`// cv::solve(A, bb, abe, cv::DECOMP_SVD);`	149	`// cv::solve(A, bb, abe, cv::DECOMP_SVD);`
148		150
149	`// float a = abe.at<float>(0, 0);`	151	`// float a = abe.at<float>(0, 0);`
150	`// float b = abe.at<float>(1, 0);`	152	`// float b = abe.at<float>(1, 0);`
151	`// float c = -(nxa+nyb+d)/nz;`	153	`// float c = -(nxa+nyb+d)/nz;`
152		154
153	`// Our version: solve simultanously for a,b,c`	155	`// Our version: solve simultanously for a,b,c`
154	`cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));`	156	`cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));`
155	`cv::Mat bb(l*mn, 1, CV_32F);`	157	`cv::Mat bb(l*mn, 1, CV_32F);`
156		158
157	`for(int k=0; k<l; k++){`	159	`for(int k=0; k<l; k++){`
158	`for(unsigned int idx=0; idx<mn; idx++){`	160	`for(unsigned int idx=0; idx<mn; idx++){`
159		161
160	`float i = Qobj[idx].x;`	162	`float i = Qobj[idx].x;`
161	`float j = Qobj[idx].y;`	163	`float j = Qobj[idx].y;`
162		164
163	`float x = Qcam[k][idx].x;`	165	`float x = Qcam[k][idx].x;`
164	`float y = Qcam[k][idx].y;`	166	`float y = Qcam[k][idx].y;`
165	`float z = Qcam[k][idx].z;`	167	`float z = Qcam[k][idx].z;`
166		168
167	`float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`	169	`float f = xx + yy + zz + (2xnx + 2yny + 2znz)(idh + jdv);`
168		170
169	`int row = k*mn+idx;`	171	`int row = k*mn+idx;`
170	`A.at<float>(row, 0) = 2*x;`	172	`A.at<float>(row, 0) = 2*x;`
171	`A.at<float>(row, 1) = 2*y;`	173	`A.at<float>(row, 1) = 2*y;`
172	`A.at<float>(row, 2) = 2*z;`	174	`A.at<float>(row, 2) = 2*z;`
173	`A.at<float>(row, idx+3) = 1.0;`	175	`A.at<float>(row, idx+3) = 1.0;`
174		176
175	`bb.at<float>(row, 0) = f;`	177	`bb.at<float>(row, 0) = f;`
176	`}`	178	`}`
177	`}`	179	`}`
178		180
179	`// solve for point`	181	`// solve for point`
180	`cv::Mat abe;`	182	`cv::Mat abe;`
181	`cv::solve(A, bb, abe, cv::DECOMP_SVD);`	183	`cv::solve(A, bb, abe, cv::DECOMP_SVD);`
182		184
183	`float a = abe.at<float>(0, 0);`	185	`float a = abe.at<float>(0, 0);`
184	`float b = abe.at<float>(1, 0);`	186	`float b = abe.at<float>(1, 0);`
185	`float c = abe.at<float>(2, 0);`	187	`float c = abe.at<float>(2, 0);`
186		188
187	`point[0] = a;`	189	`point[0] = a;`
188	`point[1] = b;`	190	`point[1] = b;`
189	`point[2] = c;`	191	`point[2] = c;`
190		192
191	`// Non-linear optimization using Ceres`	193	`// Non-linear optimization using Ceres`
192	`double pointArray[] = {point[0], point[1], point[2]};`	194	`double pointArray[] = {point[0], point[1], point[2]};`
193	`double axisArray[] = {axis[0], axis[1], axis[2]};`	195	`double axisArray[] = {axis[0], axis[1], axis[2]};`
194		196
195	`ceres::Problem problem;`	197	`ceres::Problem problem;`
196	`// loop through saddle points`	198	`// loop through saddle points`
197	`for(unsigned int idx=0; idx<mn; idx++){`	199	`for(unsigned int idx=0; idx<mn; idx++){`
198	`std::vector<cv::Point3f> pointsOnArch(l);`	200	`std::vector<cv::Point3f> pointsOnArch(l);`
199	`for(int k=0; k<l; k++){`	201	`for(int k=0; k<l; k++){`
200	`pointsOnArch[k] = Qcam[k][idx];`	202	`pointsOnArch[k] = Qcam[k][idx];`
201	`}`	203	`}`
202	`ceres::CostFunction* cost_function =`	204	`ceres::CostFunction* cost_function =`
203	`new ceres::AutoDiffCostFunction<CircleResidual, ceres::DYNAMIC, 3, 3>(`	205	`new ceres::AutoDiffCostFunction<CircleResidual, ceres::DYNAMIC, 3, 3>(`
204	`new CircleResidual(pointsOnArch), l);`	206	`new CircleResidual(pointsOnArch), l);`
205	`problem.AddResidualBlock(cost_function, NULL, pointArray, axisArray);`	207	`problem.AddResidualBlock(cost_function, NULL, pointArray, axisArray);`
206	`}`	208	`}`
207		209
208	`// Run the solver!`	210	`// Run the solver!`
209	`ceres::Solver::Options options;`	211	`ceres::Solver::Options options;`
210	`options.linear_solver_type = ceres::DENSE_QR;`	212	`options.linear_solver_type = ceres::DENSE_QR;`
211	`options.minimizer_progress_to_stdout = true;`	213	`options.minimizer_progress_to_stdout = true;`
212	`ceres::Solver::Summary summary;`	214	`ceres::Solver::Summary summary;`
213	`ceres::Solve(options, &problem, &summary);`	215	`ceres::Solve(options, &problem, &summary);`
214		216
215	`std::cout << summary.BriefReport() << "\n";`	217	`std::cout << summary.BriefReport() << "\n";`
216		218
217	`point = cv::Vec3f(pointArray[0], pointArray[1], pointArray[2]);`	219	`point = cv::Vec3f(pointArray[0], pointArray[1], pointArray[2]);`
218	`axis = cv::Vec3f(axisArray[0], axisArray[1], axisArray[2]);`	220	`axis = cv::Vec3f(axisArray[0], axisArray[1], axisArray[2]);`
219	`axis /= cv::norm(axis);`	221	`axis /= cv::norm(axis);`
220		222
221		223
222	`// Error estimate (sum of squared differences)`	224	`// Error estimate (sum of squared differences)`
223	`error = 0;`	225	`error = 0;`
224	`// loop through saddle points`	226	`// loop through saddle points`
225	`for(unsigned int idx=0; idx<mn; idx++){`	227	`for(unsigned int idx=0; idx<mn; idx++){`
226		228
227	`// vector of distances from rotation axis`	229	`// vector of distances from rotation axis`
228	`std::vector<float> dI(l);`	230	`std::vector<float> dI(l);`
229	`// loop through angular positions`	231	`// loop through angular positions`
230	`for(int k=0; k<l; k++){`	232	`for(int k=0; k<l; k++){`
231	`cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);`	233	`cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);`
232	`// point to line distance`	234	`// point to line distance`
233	`dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);`	235	`dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);`
234	`}`	236	`}`
235	`float sum = std::accumulate(dI.begin(), dI.end(), 0.0);`	237	`float sum = std::accumulate(dI.begin(), dI.end(), 0.0);`
236	`float mean = sum / dI.size();`	238	`float mean = sum / dI.size();`
237	`float meanDev = 0;`	239	`float meanDev = 0;`
238	`for(int k=0; k<l; k++){`	240	`for(int k=0; k<l; k++){`
239	`meanDev += std::abs(dI[k] - mean);`	241	`meanDev += std::abs(dI[k] - mean);`
240	`}`	242	`}`
241	`meanDev /= l;`	243	`meanDev /= l;`
242	`error += meanDev;`	244	`error += meanDev;`
243	`}`	245	`}`
244	`error /= mn;`	246	`error /= mn;`
245	`}`	247	`}`
246		248
-		249	`bool processCBCorners(const cv::Size & checkerCount,`
-		250	`const cv::Mat & SMCalibrationSetI_frameX,`
-		251	`cv::Mat & SMCalibrationSetI_frameXResult,`
-		252	`std::vector<cv::Point2f> & qciX){`
-		253	`// Convert to grayscale`
-		254	`cv::Mat gray;`
-		255	`if(SMCalibrationSetI_frameX.channels() == 1)`
-		256	`cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_BayerBG2GRAY);`
-		257	`else`
-		258	`cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_RGB2GRAY);`
-		259
-		260	`// Extract checker corners`
-		261	`bool success = cv::findChessboardCorners(gray, checkerCount, qciX, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`
-		262	`if(success){`
-		263	`cv::cornerSubPix(gray, qciX, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));`
-		264	`// Draw colored chessboard`
-		265	`cv::Mat color;`
-		266	`if(SMCalibrationSetI_frameX.channels() == 1)`
-		267	`cv::cvtColor(SMCalibrationSetI_frameX, color, CV_BayerBG2RGB);`
-		268	`else`
-		269	`color = SMCalibrationSetI_frameX.clone();`
-		270
-		271	`cvtools::drawChessboardCorners(color, checkerCount, qciX, success, 10);`
-		272	`SMCalibrationSetI_frameXResult = color;`
-		273	`}`
-		274	`return success;`
-		275	`}`
-		276
247	`void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){`	277	`void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){`
248		278
249	`QSettings settings;`	279	`QSettings settings;`
250		280
251	`// Number of saddle points on calibration pattern`	281	`// Number of saddle points on calibration pattern`
252	`int checkerCountX = settings.value("calibration/patternSizeX", 22).toInt();`	282	`int checkerCountX = settings.value("calibration/patternSizeX", 22).toInt();`
253	`int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();`	283	`int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();`
254	`cv::Size checkerCount(checkerCountX, checkerCountY);`	284	`cv::Size checkerCount(checkerCountX, checkerCountY);`
255		285
256	`unsigned int nSets = calibrationData.size();`	286	`unsigned int nSets = calibrationData.size();`
257		287
258	`// 2D Points collected for OpenCV's calibration procedures`	288	`// 2D Points collected for OpenCV's calibration procedures`
259	`std::vector< std::vector<cv::Point2f> > qc0, qc1;`	289	`std::vector< std::vector<cv::Point2f> > qc0, qc1;`
260	`std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;`	290	`std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;`
261		291
262	`std::vector<bool> success0(nSets), success1(nSets);`	292	`std::vector<bool> success0(nSets), success1(nSets);`
263		293
264	`std::vector<float> angles;`	294	`std::vector<float> angles;`
265		295
266	`// Loop through calibration sets`	296	`// Loop through calibration sets`
267	`for(unsigned int i=0; i<nSets; i++){`	297	`for(unsigned int i=0; i<nSets; i++){`
268		298
269	`SMCalibrationSet SMCalibrationSetI = calibrationData[i];`	299	`SMCalibrationSet SMCalibrationSetI = calibrationData[i];`
270		300
271	`if(!SMCalibrationSetI.checked)`	301	`if(!SMCalibrationSetI.checked)`
272	`continue;`	302	`continue;`
273		303
274	`// Camera 0`	304	`// Camera 0`
275	`std::vector<cv::Point2f> qci0;`	305	`std::vector<cv::Point2f> qci0;`
276		306
277	`// Convert to grayscale`	307	`// Convert to grayscale`
278	`cv::Mat gray;`	308	`cv::Mat gray;`
279	`if(SMCalibrationSetI.frame0.channels() == 1)`	309	`if(SMCalibrationSetI.frame0.channels() == 1)`
280	`cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);`	310	`cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);`
281	`else`	311	`else`
282	`cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);`	312	`cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);`
283		313
284	`// Extract checker corners`	314	`// Extract checker corners`
285	`success0[i] = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`	315	`success0[i] = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`
286	`if(success0[i]){`	316	`if(success0[i]){`
287	`cv::cornerSubPix(gray, qci0, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));`	317	`cv::cornerSubPix(gray, qci0, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));`
288	`// Draw colored chessboard`	318	`// Draw colored chessboard`
289	`cv::Mat color;`	319	`cv::Mat color;`
290	`if(SMCalibrationSetI.frame0.channels() == 1)`	320	`if(SMCalibrationSetI.frame0.channels() == 1)`
291	`cv::cvtColor(SMCalibrationSetI.frame0, color, CV_BayerBG2RGB);`	321	`cv::cvtColor(SMCalibrationSetI.frame0, color, CV_BayerBG2RGB);`
292	`else`	322	`else`
293	`color = SMCalibrationSetI.frame0.clone();`	323	`color = SMCalibrationSetI.frame0.clone();`
294		324
295	`cvtools::drawChessboardCorners(color, checkerCount, qci0, success0[i], 10);`	325	`cvtools::drawChessboardCorners(color, checkerCount, qci0, success0[i], 10);`
296	`SMCalibrationSetI.frame0Result = color;`	326	`SMCalibrationSetI.frame0Result = color;`
297	`}`	327	`}`
298		328
299	`emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);`	329	`emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);`
300		330
301	`// Camera 1`	331	`// Camera 1`
302	`std::vector<cv::Point2f> qci1;`	332	`std::vector<cv::Point2f> qci1;`
303		333
304	`// Convert to grayscale`	334	`// Convert to grayscale`
305	`if(SMCalibrationSetI.frame1.channels() == 1)`	335	`if(SMCalibrationSetI.frame1.channels() == 1)`
306	`cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);`	336	`cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);`
307	`else`	337	`else`
308	`cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);`	338	`cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);`
309		339
310	`// Extract checker corners`	340	`// Extract checker corners`
311	`success1[i] = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`	341	`success1[i] = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);`
312	`if(success1[i]){`	342	`if(success1[i]){`
313	`cv::cornerSubPix(gray, qci1, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));`	343	`cv::cornerSubPix(gray, qci1, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));`
314	`// Draw colored chessboard`	344	`// Draw colored chessboard`
315	`cv::Mat color;`	345	`cv::Mat color;`
316	`if(SMCalibrationSetI.frame1.channels() == 1)`	346	`if(SMCalibrationSetI.frame1.channels() == 1)`
317	`cv::cvtColor(SMCalibrationSetI.frame1, color, CV_BayerBG2RGB);`	347	`cv::cvtColor(SMCalibrationSetI.frame1, color, CV_BayerBG2RGB);`
318	`else`	348	`else`
319	`color = SMCalibrationSetI.frame1.clone();`	349	`color = SMCalibrationSetI.frame1.clone();`
320		350
321	`cvtools::drawChessboardCorners(color, checkerCount, qci1, success1[i], 10);`	351	`cvtools::drawChessboardCorners(color, checkerCount, qci1, success1[i], 10);`
322	`SMCalibrationSetI.frame1Result = color;`	352	`SMCalibrationSetI.frame1Result = color;`
323	`}`	353	`}`
324		354
325	`emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);`	355	`emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);`
326		356
327	`if(success0[i])`	357	`if(success0[i])`
328	`qc0.push_back(qci0);`	358	`qc0.push_back(qci0);`
329		359
330	`if(success1[i])`	360	`if(success1[i])`
331	`qc1.push_back(qci1);`	361	`qc1.push_back(qci1);`
332		362
333	`if(success0[i] && success1[i]){`	363	`if(success0[i] && success1[i]){`
334	`qc0Stereo.push_back(qci0);`	364	`qc0Stereo.push_back(qci0);`
335	`qc1Stereo.push_back(qci1);`	365	`qc1Stereo.push_back(qci1);`
336	`angles.push_back(SMCalibrationSetI.rotationAngle);`	366	`angles.push_back(SMCalibrationSetI.rotationAngle);`
337	`}`	367	`}`
338		368
339	`// Show progress`	369	`// Show progress`
340	`emit newSetProcessed(i);`	370	`emit newSetProcessed(i);`
341	`}`	371	`}`
342		372
343	`size_t nValidSets = angles.size();`	373	`size_t nValidSets = angles.size();`
344	`if(nValidSets < 2){`	374	`if(nValidSets < 2){`
345	`std::cerr << "Not enough valid calibration sequences!" << std::endl;`	375	`std::cerr << "Not enough valid calibration sequences!" << std::endl;`
346	`emit done();`	376	`emit done();`
347	`return;`	377	`return;`
348	`}`	378	`}`
349		379
350	`// Generate world object coordinates [mm]`	380	`// Generate world object coordinates [mm]`
351	`float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat(); // width and height of one checker square in mm`	381	`float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat(); // width and height of one checker square in mm`
352	`std::vector<cv::Point3f> Qi;`	382	`std::vector<cv::Point3f> Qi;`
353	`for (int h=0; h<checkerCount.height; h++)`	383	`for (int h=0; h<checkerCount.height; h++)`
354	`for (int w=0; w<checkerCount.width; w++)`	384	`for (int w=0; w<checkerCount.width; w++)`
355	`Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));`	385	`Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));`
356		386
357	`std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;`	387	`std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;`
358	`for(unsigned int i=0; i<qc0.size(); i++)`	388	`for(unsigned int i=0; i<qc0.size(); i++)`
359	`Q0.push_back(Qi);`	389	`Q0.push_back(Qi);`
360	`for(unsigned int i=0; i<qc1.size(); i++)`	390	`for(unsigned int i=0; i<qc1.size(); i++)`
361	`Q1.push_back(Qi);`	391	`Q1.push_back(Qi);`
362	`for(unsigned int i=0; i<nValidSets; i++)`	392	`for(unsigned int i=0; i<nValidSets; i++)`
363	`QStereo.push_back(Qi);`	393	`QStereo.push_back(Qi);`
364		394
365	`// calibrate the cameras`	395	`// calibrate the cameras`
366	`SMCalibrationParameters cal;`	396	`SMCalibrationParameters cal;`
367	`cal.frameWidth = calibrationData[0].frame0.cols;`	397	`cal.frameWidth = calibrationData[0].frame0.cols;`
368	`cal.frameHeight = calibrationData[0].frame0.rows;`	398	`cal.frameHeight = calibrationData[0].frame0.rows;`
369	`cv::Size frameSize(cal.frameWidth, cal.frameHeight);`	399	`cv::Size frameSize(cal.frameWidth, cal.frameHeight);`
370		400
371	`// determine only k1, k2 for lens distortion`	401	`// determine only k1, k2 for lens distortion`
372	`int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;`	402	`int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;`
373	`// Note: several of the output arguments below must be cv::Mat, otherwise segfault`	403	`// Note: several of the output arguments below must be cv::Mat, otherwise segfault`
374	`std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;`	404	`std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;`
375	`cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags,`	405	`cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags,`
376	`cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));`	406	`cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));`
377	`//std::cout << cal.k0 << std::endl;`	407	`//std::cout << cal.k0 << std::endl;`
378	`// // refine extrinsics for camera 0`	408	`// // refine extrinsics for camera 0`
379	`// for(int i=0; i<Q.size(); i++)`	409	`// for(int i=0; i<Q.size(); i++)`
380	`// 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);`	410	`// 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);`
381		411
382	`std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;`	412	`std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;`
383	`cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,`	413	`cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,`
384	`cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));`	414	`cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));`
385	`//std::cout << cal.k1 << std::endl;`	415	`//std::cout << cal.k1 << std::endl;`
386	`// stereo calibration`	416	`// stereo calibration`
387	`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;`	417	`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;`
388	`cv::Mat E, F, R1, T1;`	418	`cv::Mat E, F, R1, T1;`
389	`cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,`	419	`cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,`
390	`frameSize, R1, T1, E, F,`	420	`frameSize, R1, T1, E, F, flags_stereo,`
391	`cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON),`	421	`cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));`
392	`flags_stereo);`	-
393		422
394	`cal.R1 = R1;`	423	`cal.R1 = R1;`
395	`cal.T1 = T1;`	424	`cal.T1 = T1;`
396	`cal.E = E;`	425	`cal.E = E;`
397	`cal.F = F;`	426	`cal.F = F;`
398		427
399	`// Determine per-view reprojection errors:`	428	`// Determine per-view reprojection errors:`
400	`cal.cam0_errors_per_view.resize(nSets);`	429	`cal.cam0_errors_per_view.resize(nSets);`
401	`int idx = 0;`	430	`int idx = 0;`
402	`for(unsigned int i = 0; i < nSets; ++i){`	431	`for(unsigned int i = 0; i < nSets; ++i){`
403	`if(success0[i]){`	432	`if(success0[i]){`
404	`int n = (int)Q0[idx].size();`	433	`int n = (int)Q0[idx].size();`
405	`std::vector<cv::Point2f> qc_proj;`	434	`std::vector<cv::Point2f> qc_proj;`
406	`cv::projectPoints(cv::Mat(Q0[idx]), cam_rvecs0[idx], cam_tvecs0[idx], cal.K0, cal.k0, qc_proj);`	435	`cv::projectPoints(cv::Mat(Q0[idx]), cam_rvecs0[idx], cam_tvecs0[idx], cal.K0, cal.k0, qc_proj);`
407	`float err = 0;`	436	`float err = 0;`
408	`for(unsigned int j=0; j<qc_proj.size(); j++){`	437	`for(unsigned int j=0; j<qc_proj.size(); j++){`
409	`cv::Point2f d = qc0[idx][j] - qc_proj[j];`	438	`cv::Point2f d = qc0[idx][j] - qc_proj[j];`
410	`err += cv::sqrt(d.xd.x + d.yd.y);`	439	`err += cv::sqrt(d.xd.x + d.yd.y);`
411	`}`	440	`}`
412	`cal.cam0_errors_per_view[i] = (float)err/n;`	441	`cal.cam0_errors_per_view[i] = (float)err/n;`
413	`idx++;`	442	`idx++;`
414	`} else`	443	`} else`
415	`cal.cam0_errors_per_view[i] = NAN;`	444	`cal.cam0_errors_per_view[i] = NAN;`
416	`}`	445	`}`
417	`cal.cam1_errors_per_view.resize(nSets);`	446	`cal.cam1_errors_per_view.resize(nSets);`
418	`idx = 0;`	447	`idx = 0;`
419	`for(unsigned int i = 0; i < nSets; ++i){`	448	`for(unsigned int i = 0; i < nSets; ++i){`
420	`if(success1[i]){`	449	`if(success1[i]){`
421	`int n = (int)Q1[idx].size();`	450	`int n = (int)Q1[idx].size();`
422	`std::vector<cv::Point2f> qc_proj;`	451	`std::vector<cv::Point2f> qc_proj;`
423	`cv::projectPoints(cv::Mat(Q1[idx]), cam_rvecs1[idx], cam_tvecs1[idx], cal.K1, cal.k1, qc_proj);`	452	`cv::projectPoints(cv::Mat(Q1[idx]), cam_rvecs1[idx], cam_tvecs1[idx], cal.K1, cal.k1, qc_proj);`
424	`float err = 0;`	453	`float err = 0;`
425	`for(unsigned int j=0; j<qc_proj.size(); j++){`	454	`for(unsigned int j=0; j<qc_proj.size(); j++){`
426	`cv::Point2f d = qc1[idx][j] - qc_proj[j];`	455	`cv::Point2f d = qc1[idx][j] - qc_proj[j];`
427	`err += cv::sqrt(d.xd.x + d.yd.y);`	456	`err += cv::sqrt(d.xd.x + d.yd.y);`
428	`}`	457	`}`
429	`cal.cam1_errors_per_view[i] = (float)err/n;`	458	`cal.cam1_errors_per_view[i] = (float)err/n;`
430	`idx++;`	459	`idx++;`
431	`} else`	460	`} else`
432	`cal.cam1_errors_per_view[i] = NAN;`	461	`cal.cam1_errors_per_view[i] = NAN;`
433	`}`	462	`}`
434		463
435	`// // hand-eye calibration`	464	`// // hand-eye calibration`
436	`// std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame`	465	`// std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame`
437	`// std::vector<cv::Vec3f> Tc(nValidSets - 1);`	466	`// std::vector<cv::Vec3f> Tc(nValidSets - 1);`
438	`// std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame`	467	`// std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame`
439	`// std::vector<cv::Vec3f> Tr(nValidSets - 1);`	468	`// std::vector<cv::Vec3f> Tr(nValidSets - 1);`
440	`// for(int i=0; i<nValidSets-1; i++){`	469	`// for(int i=0; i<nValidSets-1; i++){`
441	`// // relative transformations in camera`	470	`// // relative transformations in camera`
442	`// cv::Mat cRw1, cRw2;`	471	`// cv::Mat cRw1, cRw2;`
443	`// cv::Rodrigues(cam_rvecs0[i], cRw1);`	472	`// cv::Rodrigues(cam_rvecs0[i], cRw1);`
444	`// cv::Rodrigues(cam_rvecs0[i+1], cRw2);`	473	`// cv::Rodrigues(cam_rvecs0[i+1], cRw2);`
445	`// cv::Mat cTw1 = cam_tvecs0[i];`	474	`// cv::Mat cTw1 = cam_tvecs0[i];`
446	`// cv::Mat cTw2 = cam_tvecs0[i+1];`	475	`// cv::Mat cTw2 = cam_tvecs0[i+1];`
447	`// cv::Mat w1Rc = cRw1.t();`	476	`// cv::Mat w1Rc = cRw1.t();`
448	`// cv::Mat w1Tc = -cRw1.t()*cTw1;`	477	`// cv::Mat w1Tc = -cRw1.t()*cTw1;`
449	`// Rc[i] = cv::Mat(cRw2*w1Rc);`	478	`// Rc[i] = cv::Mat(cRw2*w1Rc);`
450	`// Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);`	479	`// Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);`
451		480
452	`// // relative transformations in rotation stage`	481	`// // relative transformations in rotation stage`
453	`// // we define the rotation axis to be in origo, pointing in positive y direction`	482	`// // we define the rotation axis to be in origo, pointing in positive y direction`
454	`// float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;`	483	`// float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;`
455	`// cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`	484	`// cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`
456	`// cv::Mat Rri;`	485	`// cv::Mat Rri;`
457	`// cv::Rodrigues(rot_rvec, Rri);`	486	`// cv::Rodrigues(rot_rvec, Rri);`
458	`// Rr[i] = Rri;`	487	`// Rr[i] = Rri;`
459	`// Tr[i] = 0.0;`	488	`// Tr[i] = 0.0;`
460		489
461	`//// std::cout << i << std::endl;`	490	`//// std::cout << i << std::endl;`
462	`//// std::cout << "cTw1" << cTw1 << std::endl;`	491	`//// std::cout << "cTw1" << cTw1 << std::endl;`
463	`//// std::cout << "cTw2" << cTw2 << std::endl;`	492	`//// std::cout << "cTw2" << cTw2 << std::endl;`
464	`//// std::cout << "w2Rc" << w2Rc << std::endl;`	493	`//// std::cout << "w2Rc" << w2Rc << std::endl;`
465	`//// std::cout << "w2Tc" << w2Tc << std::endl;`	494	`//// std::cout << "w2Tc" << w2Tc << std::endl;`
466		495
467	`//// std::cout << "w2Rc" << w2Rc << std::endl;`	496	`//// std::cout << "w2Rc" << w2Rc << std::endl;`
468	`//// std::cout << "w2Tc" << w2Tc << std::endl;`	497	`//// std::cout << "w2Tc" << w2Tc << std::endl;`
469		498
470	`//// cv::Mat Rci;`	499	`//// cv::Mat Rci;`

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 198 → 206