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

 #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)
-    int l = Qcam.size();
+    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,
 , y*y, y*z, y, i*y, j*y,
 ,   0, z*z, z, i*z, j*z,
 ,   0,   0, 1,   i,   j,
 ,   0,   0, 0, i*i, i*j,
 ,   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);
+        }
+    }
+}
-void SMCalibrationWorker::cameraCalibrationCharuco(std::vector<SMCalibrationSet> calibrationData){
+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,
-    QSettings settings;
+                                            std::vector<int>& qci){
-    // Number of saddle points on calibration pattern
+    // Convert to grayscale
+    cv::Mat gray;
-    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));
+    if(frame.channels() == 1)
-    const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();
+        cv::cvtColor(frame, gray, CV_BayerBG2GRAY);
+    else
-    float markerLength = 0.8*checkerSize;
+        cv::cvtColor(frame, gray, CV_RGB2GRAY);
-    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>> mc;
-    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
+    std::vector<int> mi;
-    // 3D object points
+    cv::Mat grayBlurred;
-    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
+    cv::GaussianBlur(gray, grayBlurred, cv::Size(0, 0), 3);
-    // Charuco board and dictionary
+    // Extract checker corners
+    cv::aruco::detectMarkers(grayBlurred, dict, mc, mi, parameters);
+    //std::vector<std::vector<cv::Point2f>> mc, rejecti;
-    cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
+    //cv::aruco::refineDetectedMarkers(gray0, board, mc0, mi0, rejecti0);
-//    // The dictionary from the standalone Aruco library used to generate the original chessboards
+    //cv::Mat cci, cidsi;
-//    std::vector<uint64_t> ARUCO_MIP_36h12 = {0xd2b63a09dUL,0x6001134e5UL,0x1206fbe72UL,0xff8ad6cb4UL,0x85da9bc49UL,0xb461afe9cUL,0x6db51fe13UL,0x5248c541fUL,0x8f34503UL,0x8ea462eceUL,0xeac2be76dUL,0x1af615c44UL,0xb48a49f27UL,0x2e4e1283bUL,0x78b1f2fa8UL,0x27d34f57eUL,0x89222fff1UL,0x4c1669406UL,0xbf49b3511UL,0xdc191cd5dUL,0x11d7c3f85UL,0x16a130e35UL,0xe29f27effUL,0x428d8ae0cUL,0x90d548477UL,0x2319cbc93UL,0xc3b0c3dfcUL,0x424bccc9UL,0x2a081d630UL,0x762743d96UL,0xd0645bf19UL,0xf38d7fd60UL,0xc6cbf9a10UL,0x3c1be7c65UL,0x276f75e63UL,0x4490a3f63UL,0xda60acd52UL,0x3cc68df59UL,0xab46f9daeUL,0x88d533d78UL,0xb6d62ec21UL,0xb3c02b646UL,0x22e56d408UL,0xac5f5770aUL,0xaaa993f66UL,0x4caa07c8dUL,0x5c9b4f7b0UL,0xaa9ef0e05UL,0x705c5750UL,0xac81f545eUL,0x735b91e74UL,0x8cc35cee4UL,0xe44694d04UL,0xb5e121de0UL,0x261017d0fUL,0xf1d439eb5UL,0xa1a33ac96UL,0x174c62c02UL,0x1ee27f716UL,0x8b1c5ece9UL,0x6a05b0c6aUL,0xd0568dfcUL,0x192d25e5fUL,0x1adbeccc8UL,0xcfec87f00UL,0xd0b9dde7aUL,0x88dcef81eUL,0x445681cb9UL,0xdbb2ffc83UL,0xa48d96df1UL,0xb72cc2e7dUL,0xc295b53fUL,0xf49832704UL,0x9968edc29UL,0x9e4e1af85UL,0x8683e2d1bUL,0x810b45c04UL,0x6ac44bfe2UL,0x645346615UL,0x3990bd598UL,0x1c9ed0f6aUL,0xc26729d65UL,0x83993f795UL,0x3ac05ac5dUL,0x357adff3bUL,0xd5c05565UL,0x2f547ef44UL,0x86c115041UL,0x640fd9e5fUL,0xce08bbcf7UL,0x109bb343eUL,0xc21435c92UL,0x35b4dfce4UL,0x459752cf2UL,0xec915b82cUL,0x51881eed0UL,0x2dda7dc97UL,0x2e0142144UL,0x42e890f99UL,0x9a8856527UL,0x8e80d9d80UL,0x891cbcf34UL,0x25dd82410UL,0x239551d34UL,0x8fe8f0c70UL,0x94106a970UL,0x82609b40cUL,0xfc9caf36UL,0x688181d11UL,0x718613c08UL,0xf1ab7629UL,0xa357bfc18UL,0x4c03b7a46UL,0x204dedce6UL,0xad6300d37UL,0x84cc4cd09UL,0x42160e5c4UL,0x87d2adfa8UL,0x7850e7749UL,0x4e750fc7cUL,0xbf2e5dfdaUL,0xd88324da5UL,0x234b52f80UL,0x378204514UL,0xabdf2ad53UL,0x365e78ef9UL,0x49caa6ca2UL,0x3c39ddf3UL,0xc68c5385dUL,0x5bfcbbf67UL,0x623241e21UL,0xabc90d5ccUL,0x388c6fe85UL,0xda0e2d62dUL,0x10855dfe9UL,0x4d46efd6bUL,0x76ea12d61UL,0x9db377d3dUL,0xeed0efa71UL,0xe6ec3ae2fUL,0x441faee83UL,0xba19c8ff5UL,0x313035eabUL,0x6ce8f7625UL,0x880dab58dUL,0x8d3409e0dUL,0x2be92ee21UL,0xd60302c6cUL,0x469ffc724UL,0x87eebeed3UL,0x42587ef7aUL,0x7a8cc4e52UL,0x76a437650UL,0x999e41ef4UL,0x7d0969e42UL,0xc02baf46bUL,0x9259f3e47UL,0x2116a1dc0UL,0x9f2de4d84UL,0xeffac29UL,0x7b371ff8cUL,0x668339da9UL,0xd010aee3fUL,0x1cd00b4c0UL,0x95070fc3bUL,0xf84c9a770UL,0x38f863d76UL,0x3646ff045UL,0xce1b96412UL,0x7a5d45da8UL,0x14e00ef6cUL,0x5e95abfd8UL,0xb2e9cb729UL,0x36c47dd7UL,0xb8ee97c6bUL,0xe9e8f657UL,0xd4ad2ef1aUL,0x8811c7f32UL,0x47bde7c31UL,0x3adadfb64UL,0x6e5b28574UL,0x33e67cd91UL,0x2ab9fdd2dUL,0x8afa67f2bUL,0xe6a28fc5eUL,0x72049cdbdUL,0xae65dac12UL,0x1251a4526UL,0x1089ab841UL,0xe2f096ee0UL,0xb0caee573UL,0xfd6677e86UL,0x444b3f518UL,0xbe8b3a56aUL,0x680a75cfcUL,0xac02baea8UL,0x97d815e1cUL,0x1d4386e08UL,0x1a14f5b0eUL,0xe658a8d81UL,0xa3868efa7UL,0x3668a9673UL,0xe8fc53d85UL,0x2e2b7edd5UL,0x8b2470f13UL,0xf69795f32UL,0x4589ffc8eUL,0x2e2080c9cUL,0x64265f7dUL,0x3d714dd10UL,0x1692c6ef1UL,0x3e67f2f49UL,0x5041dad63UL,0x1a1503415UL,0x64c18c742UL,0xa72eec35UL,0x1f0f9dc60UL,0xa9559bc67UL,0xf32911d0dUL,0x21c0d4ffcUL,0xe01cef5b0UL,0x4e23a3520UL,0xaa4f04e49UL,0xe1c4fcc43UL,0x208e8f6e8UL,0x8486774a5UL,0x9e98c7558UL,0x2c59fb7dcUL,0x9446a4613UL,0x8292dcc2eUL,0x4d61631UL,0xd05527809UL,0xa0163852dUL,0x8f657f639UL,0xcca6c3e37UL,0xcb136bc7aUL,0xfc5a83e53UL,0x9aa44fc30UL,0xbdec1bd3cUL,0xe020b9f7cUL,0x4b8f35fb0UL,0xb8165f637UL,0x33dc88d69UL,0x10a2f7e4dUL,0xc8cb5ff53UL,0xde259ff6bUL,0x46d070dd4UL,0x32d3b9741UL,0x7075f1c04UL,0x4d58dbea0UL};
+    cv::aruco::interpolateCornersCharuco(mc, mi, gray, board, qc, qci, cv::noArray(), cv::noArray(), 1);
-//    int tau_MIP_3612 = 12;
-//    // shuffle according to aruco_create_markermap code
+    bool success = (qc.size() >= 4);
-//    std::srand(0);
+    if(success){
-//    std::random_shuffle(ARUCO_MIP_36h12.begin(),ARUCO_MIP_36h12.end());
+        cv::cornerSubPix(gray, qc, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
-//    cv::Mat bytesList(250, 5, CV_8UC4);
-//    for(int i=0; i<250; i++){
-//        cv::Mat bits(6, 6, CV_8UC1);
+        // Draw colored chessboard
-//        for(int b=0; b<36; b++){
+        if(frame.channels() == 1)
-//            bits.at<char>(b) = getBit(ARUCO_MIP_36h12[i], b+1);
+            cv::cvtColor(frame, frameResult, CV_BayerBG2RGB);
-//        }
+        else
-//        //std::cout << bits << std::endl;
-//        cv::Mat bytes = cv::aruco::Dictionary::getByteListFromBits(bits);
-//        bytes.copyTo(bytesList.row(i));
+            frameResult = frame.clone();
-//    }
-//    cv::Ptr<cv::aruco::Dictionary> dict = cv::makePtr<cv::aruco::Dictionary>(new cv::aruco::Dictionary(bytesList, 6, (tau_MIP_3612-1)/2));
-//    for(int i=0; i<250; i++){
-//        cv::Mat img;
-//        dict->drawMarker(i, 100, img, 1);
-//        cv::imwrite(QString("id%1.png").arg(i).toStdString(), img);
-//    }
+        // 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;
+    //    cv::Mat boardImg;
-//    board->draw(cv::Size((saddlePointCountX+1)*100, (saddlePointCountY+1)*100), boardImg, 0, 1);
+    //    board->draw(cv::Size((saddlePointCountX+1)*100, (saddlePointCountY+1)*100), boardImg, 0, 1);
-//    cv::Mat boardImgRes;
+    //    cv::Mat boardImgRes;
-//    cv::resize(boardImg, boardImgRes, cv::Size((saddlePointCountX+1)*10, (saddlePointCountY+1)*10), 0, 0, cv::INTER_NEAREST);
+    //    cv::resize(boardImg, boardImgRes, cv::Size((saddlePointCountX+1)*10, (saddlePointCountY+1)*10), 0, 0, cv::INTER_NEAREST);
-//    cv::imwrite("boardImg.png", boardImg);
+    //    cv::imwrite("boardImg.png", boardImg);
-//    cv::imwrite("boardImgRes.png", boardImgRes);
+    //    cv::imwrite("boardImgRes.png", boardImgRes);
     cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
-    parameters->adaptiveThreshWinSizeMin = 3;
+//    parameters->adaptiveThreshWinSizeMin = 3;
-    parameters->adaptiveThreshWinSizeMax = 153;
+//    parameters->adaptiveThreshWinSizeMax = 153;
-    parameters->adaptiveThreshWinSizeStep = 3;
+//    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;
-    // Loop through calibration sets
-    for(unsigned int i=0; i<nSets; i++){
-        SMCalibrationSet calibrationSetI = calibrationData[i];
+    bool success0 = detectCheckerBoardCornersCharuco(calibrationSet.frame0, dict, board, parameters, calibrationSet.frame0Result, calibrationSet.qc0, calibrationSet.qc0id);
-        if(!calibrationSetI.selected)
-            continue;
-        // Camera 0
+    if(!success0){
-        std::vector<std::vector<cv::Point2f>> mc0, rejecti0;
+        emit logMessage(QString("Could not detect  Charuco corners on set %1 camera0").arg(calibrationSet.id));
-        std::vector<int> mi0;
-        cv::Mat gray0;
-        cv::cvtColor(calibrationSetI.frame0, gray0, CV_BayerBG2GRAY);
+        std::cerr << "Could not detect Charuco corners on set " << calibrationSet.id << " camera0" << std::endl;
+    }
-        // Extract checker corners
-        cv::aruco::detectMarkers(gray0, dict, mc0, mi0, parameters);
-        cv::aruco::refineDetectedMarkers(gray0, board, mc0, mi0, rejecti0);
+    bool success1 = detectCheckerBoardCornersCharuco(calibrationSet.frame1, dict, board, parameters, calibrationSet.frame1Result, calibrationSet.qc1, calibrationSet.qc1id);
-        cv::Mat cci0, cidsi0;
+    if(!success1){
-        cv::aruco::interpolateCornersCharuco(mc0, mi0, gray0, board, cci0, cidsi0);
+        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;
+    }
-        bool success0 = (cci0.rows >= 4);
-        if(success0){
-            cv::Mat color0;
-            cv::cvtColor(calibrationSetI.frame0, color0, CV_BayerBG2RGB);
+    emit newCheckerboardResult(calibrationSet.id, calibrationSet);
-            // Draw colored chessboard
-            drawDetectedMarkersCharuco(color0, mc0, mi0, cv::Scalar(0, 255, 0));
-            drawDetectedCornersCharuco(color0, cci0, cidsi0, cv::Scalar(0, 255, 0));
+}
-            calibrationSetI.frame0Result = color0;
-        } else {
-            std::cerr << "Could not detect Charuco in camera 0, id " << calibrationSetI.id << std::endl;
+void SMCalibrationWorker::cameraCalibrationCharuco(std::vector<SMCalibrationSet> calibrationData){
-        }
+    QSettings settings;
-        // Camera 1
+    // Number of saddle points on calibration pattern
+    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));
-        std::vector<std::vector<cv::Point2f>> mc1, rejecti1;
+    const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();
-        std::vector<int> mi1;
+    float markerLength = 0.8f*checkerSize;
-        cv::Mat gray1;
+    cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
-        cv::cvtColor(calibrationSetI.frame1, gray1, CV_BayerBG2GRAY);
+    cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);
-        // Extract checker corners
+    auto nSets = calibrationData.size();
-        cv::aruco::detectMarkers(gray1, dict, mc1, mi1, parameters);
-        cv::aruco::refineDetectedMarkers(gray1, board, mc1, mi1, rejecti1);
+    // 2D Points collected for OpenCV's calibration procedures
-        cv::Mat cci1, cidsi1;
+    std::vector< std::vector<cv::Point2f> > qc0, qc1;
-        cv::aruco::interpolateCornersCharuco(mc1, mi1, gray1, board, cci1, cidsi1);
+    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
-        bool success1 = (cci1.rows >= 4);
-        if(success1){
-            cv::Mat color1;
+    // 3D object points
-            cv::cvtColor(calibrationSetI.frame1, color1, CV_BayerBG2RGB);
+    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
-            // Draw colored chessboard
+    // Loop through calibration sets
-            drawDetectedMarkersCharuco(color1, mc1, mi1, cv::Scalar(0, 255, 0));
-            drawDetectedCornersCharuco(color1, cci1, cidsi1, cv::Scalar(0, 255, 0));
+    for(unsigned int i=0; i<nSets; i++){
-            calibrationSetI.frame1Result = color1;
+        if(!calibrationData[i].selected)
-        } else {
+            continue;
-            std::cerr << "Could not detect Charuco in camera 1, id " << calibrationSetI.id << std::endl;
-        }
+        bool success0 = !calibrationData[i].qc0.empty();
-        emit newCheckerboardResult(calibrationSetI.id, calibrationSetI);
+        bool success1 = !calibrationData[i].qc1.empty();
         if(success0){
-            std::vector<cv::Point2f> qc0i;
             std::vector<cv::Point3f> Q0i;
-            for(int j=0; j<cci0.rows; j++){
+            for(std::size_t j=0; j<calibrationData[i].qc0id.size(); j++){
-                qc0i.push_back(cci0.at<cv::Point2f>(j));
-                int id = cidsi0.at<int>(j);
+                int id = calibrationData[i].qc0id[j];
                 Q0i.push_back(board->chessboardCorners[id]);
+            }
-            qc0.push_back(qc0i);
+            qc0.push_back(calibrationData[i].qc0);
             Q0.push_back(Q0i);
+        }
         if(success1){
-            std::vector<cv::Point2f> qc1i;
             std::vector<cv::Point3f> Q1i;
-            for(int j=0; j<cci1.rows; j++){
+            for(std::size_t j=0; j<calibrationData[i].qc1id.size(); j++){
-                qc1i.push_back(cci1.at<cv::Point2f>(j));
-                int id = cidsi1.at<int>(j);
+                int id = calibrationData[i].qc1id[j];
                 Q1i.push_back(board->chessboardCorners[id]);
+            }
-            qc1.push_back(qc1i);
+            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<cci0.rows && j1<cci1.rows){
+            while(j0<calibrationData[i].qc0.size() && j1<calibrationData[i].qc1.size()){
-                int id0 = cidsi0.at<int>(j0);
+                int id0 = calibrationData[i].qc0id[j0];
-                int id1 = cidsi1.at<int>(j1);
+                int id1 = calibrationData[i].qc1id[j1];
                 if(id0 < id1)
                     j0++;
                 else if (id1 < id0)
                     j1++;
                 else{
                     assert(id0 == id1);
-                    qc0iStereo.push_back(cci0.at<cv::Point2f>(j0));
+                    qc0iStereo.push_back(calibrationData[i].qc0[j0]);
-                    qc1iStereo.push_back(cci1.at<cv::Point2f>(j1));
+                    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_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;
+    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){
-    int nSets = calibrationData.size();
+    auto nSets = calibrationData.size();
     std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
-    for(int i=0; i<nSets; i++){
+    std::vector<cv::Point3f> Qi;
-        if(!calibrationData[i].selected)
+    QSettings settings;
-            continue;
+    // 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"){
-        // Note: avoiding push_back has only minor theoretical value
+        // Find id's which were detected in all images
+        std::vector<int> ids;
-        if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){
+        for(int i=0; i<calibrationData.size(); i++){
-            qc0Stereo.push_back(calibrationData[i].qc0);
+            if(!calibrationData[i].selected)
+                continue;
+            if(ids.empty()){
-            qc1Stereo.push_back(calibrationData[i].qc1);
+                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)
-    QSettings settings;
+                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();
-    SMCalibrationParameters cal = settings.value("calibration/parameters").value<SMCalibrationParameters>();
+                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]]);
+        }
-    if(qc0Stereo.size() > 2){
-        // 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();
-        std::vector<cv::Point3f> Qi = generateWorldCoords(checkerCount, checkerSize);
-        // 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(qc0Stereo.size());
-        #pragma omp parallel for
-        for(unsigned int i=0; i<qc0Stereo.size(); 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);
-            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[i] = QcamiPoints;
-        }
-        cv::Vec3f axis, point;
-        float rot_axis_error;
-        rotationAxisEstimation(Qcam, Qi, axis, point);
-        rotationAxisOptimization(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;
     } else {
+        // Normal checkerboard with all corners
-        cal.Rr = cv::Matx33f::eye();
+        for(auto i=0; i<nSets; i++){
-        cal.Tr = cv::Vec3f(0,0,0);
+            if(!calibrationData[i].selected)
-        cal.rot_axis_error = -1;
+                continue;
+            if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){
+                qc0Stereo.push_back(calibrationData[i].qc0);
+                qc1Stereo.push_back(calibrationData[i].qc1);
-        return;
+            }
+        }
+        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!");
+       cal.Rr = cv::Matx33f::eye();
+       cal.Tr = cv::Vec3f(0,0,0);
+       cal.rot_axis_error = -1;
+       emit done();
+       return;
+    }
+    // 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(qc0Stereo.size());
+    #pragma omp parallel for
+    for(unsigned int i=0; i<qc0Stereo.size(); 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);
+        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[i] = QcamiPoints;
+    }
+    cv::Vec3f axis, point;
+    float rot_axis_error;
+    rotationAxisEstimation(Qcam, Qi, axis, point);
+    rotationAxisOptimization(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 log
     std::stringstream out;
     out << "## Rotation Stage Calibration ##" << std::endl
         << "No. images used for calibration: " << qc0Stereo.size() << std::endl;
     cal.printRotationStage(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();
+}

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 249 → 250