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

 #include "SMCalibrationParameters.h"
 #include "cvtools.h"
 #include <QSettings>
+#include <QTextStream>
 #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,
+static void rotationAxisEstimation(const std::vector< std::vector<cv::Point3f> > Qcam,
-                                    const std::vector<cv::Point3f> Qobj,
+                                   const std::vector<cv::Point3f> Qobj,
-                                    cv::Vec3f &axis, cv::Vec3f &point, float &error){
+                                   cv::Vec3f &axis, cv::Vec3f &point){
     assert(Qobj.size() == Qcam[0].size());
     // number of frames (points on each arch)
     int l = Qcam.size();
     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();
-    // Non-linear optimization using Ceres
+    // 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(int k=0; k<l; k++){
+        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);
     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++){
+        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(int k=0; k<l; k++){
+        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;
+}
-bool processCBCorners(const cv::Size & checkerCount,
+static bool detectCheckerBoardCorners(const cv::Size & checkerCount,
-                      const cv::Mat & SMCalibrationSetI_frameX,
+                                    const cv::Mat & frame,
-                      cv::Mat & SMCalibrationSetI_frameXResult,
+                                    cv::Mat & frameResult,
-                      std::vector<cv::Point2f> & qciX){
+                                    std::vector<cv::Point2f> & qc){
     // Convert to grayscale
     cv::Mat gray;
-    if(SMCalibrationSetI_frameX.channels() == 1)
+    if(frame.channels() == 1)
-        cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_BayerBG2GRAY);
+        cv::cvtColor(frame, gray, CV_BayerBG2GRAY);
     else
-        cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_RGB2GRAY);
+        cv::cvtColor(frame, gray, CV_RGB2GRAY);
     // Extract checker corners
-    bool success = cv::findChessboardCorners(gray, checkerCount, qciX, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
+    bool success = cv::findChessboardCorners(gray, checkerCount, qc, 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));
+        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(SMCalibrationSetI_frameX.channels() == 1)
+        if(frame.channels() == 1)
-            cv::cvtColor(SMCalibrationSetI_frameX, SMCalibrationSetI_frameXResult, CV_BayerBG2RGB);
+            cv::cvtColor(frame, frameResult, CV_BayerBG2RGB);
         else
-            SMCalibrationSetI_frameXResult = SMCalibrationSetI_frameX.clone();
+            frameResult = frame.clone();
-        cvtools::drawChessboardCorners(SMCalibrationSetI_frameXResult, checkerCount, qciX, success, 10);
+        cvtools::drawChessboardCorners(frameResult, checkerCount, qc, success, 10);
+    }
     return success;
+}
-std::vector<cv::Point3f> SMCalibrationWorker::generateObjCoordsInWorldCS(const cv::Size checkerCount, const float checkerSize)
+void SMCalibrationWorker::checkerboardDetection(SMCalibrationSet calibrationSet){
-{
-    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;
+    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)
+        emit logMessage(QString("Could not detect checkerboard on set %1 camera0").arg(calibrationSet.id));
+    bool success1 = detectCheckerBoardCorners(checkerCount, calibrationSet.frame1, calibrationSet.frame1Result, calibrationSet.qc1);
+    if(!success1)
+        emit logMessage(QString("Could not detect checkerboard on set %1 camera1").arg(calibrationSet.id));
+    emit newCheckerboardResult(calibrationSet.id, calibrationSet);
-void SMCalibrationWorker::perViewReprojError(const std::vector<bool> &success0,
-                                             const std::vector< std::vector<cv::Point3f> > &Q0,
-                                             const std::vector<cv::Mat> &cam_tvecs0,
-                                             const std::vector<cv::Mat> &cam_rvecs0,
-                                             const std::vector< std::vector<cv::Point2f> > &qc0,
-                                             const cv::Matx33f &K0,
-                                             const cv::Vec<float, 5> &k0,
-                                             std::vector<float> &camX_errors_per_view)
-{
-    unsigned int nSets = success0.size();
-    camX_errors_per_view.resize(nSets);
-    int idx = 0;
-    //#pragma omp parallel for
-    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], K0,  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);
-            }
-            camX_errors_per_view[i] = (float)err/n;
-            idx++;
-        } else
-            camX_errors_per_view[i] = NAN;
-    }
+}
-void SMCalibrationWorker::performCameraCalibration(std::vector<SMCalibrationSet> calibrationData){
+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()));
+    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
-#define OLD_WAYZ 0
-#if OLD_WAYZ
     std::vector< std::vector<cv::Point2f> > qc0, qc1, qc0Stereo, qc1Stereo;
-    std::vector<bool> fwdToStageEstimation;
-    std::vector<float> angles;
-    std::vector<unsigned int> TESTER;
-#else
-    std::vector< std::vector<cv::Point2f> > qc0(nSets), qc1(nSets), qc0Stereo(nSets), qc1Stereo(nSets);
-    std::vector<bool> fwdToStageEstimation(nSets);
-    std::vector<float> angles(nSets);
-    std::vector<unsigned int> TESTER(nSets);
-#endif
-    std::vector<bool> success0(nSets, false), success1(nSets, false);
-    // Loop through calibration sets
-#if !OLD_WAYZ
-#pragma omp parallel for
-#endif
-    for(unsigned int i=0; i<nSets; i++){
-        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
+    for(int i=0; i<nSets; i++){
-        // TODO this changes semantics of the checkboxes in the GUI
-        /*if(!SMCalibrationSetI.checked)
+        if(!calibrationData[i].selected)
-            continue;*/
+            continue;
-        // Camera 0
-        std::vector<cv::Point2f> qci0;
-        success0[i] = processCBCorners(checkerCount, SMCalibrationSetI.frame0, SMCalibrationSetI.frame0Result, qci0);
-#pragma omp critical (CBCALCupdateUI1)
-{
-        emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);
+        // Note: avoiding push_back has only minor theoretical value
-}
-        // Camera 1
-        std::vector<cv::Point2f> qci1;
+        if(!calibrationData[i].qc0.empty())
-        success1[i] = processCBCorners(checkerCount, SMCalibrationSetI.frame1, SMCalibrationSetI.frame1Result, qci1);
-#pragma omp critical (CBCALCupdateUI2)
-{
-        emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);
-}
-        // store results
-#if OLD_WAYZ
-        if(success0[i])
-            qc0.push_back(qci0);
+            qc0.push_back(calibrationData[i].qc0);
-        if(success1[i])
+        if(!calibrationData[i].qc1.empty())
-            qc1.push_back(qci1);
+            qc1.push_back(calibrationData[i].qc1);
-        if(success0[i] && success1[i]){
+        if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){
-            qc0Stereo.push_back(qci0);
+            qc0Stereo.push_back(calibrationData[i].qc0);
-            qc1Stereo.push_back(qci1);
+            qc1Stereo.push_back(calibrationData[i].qc1);
-            angles.push_back(SMCalibrationSetI.rotationAngle);
-            fwdToStageEstimation.push_back(SMCalibrationSetI.checked);
-            TESTER.push_back(i);
-        }
-#else
-        qc0[i] = qci0;
-        qc1[i] = qci1;
-        qc0Stereo[i] = qci0;
-        qc1Stereo[i] = qci1;
-        angles[i] = SMCalibrationSetI.rotationAngle;
-        fwdToStageEstimation[i] = SMCalibrationSetI.checked;
-        TESTER[i] = (i);
-#endif
-        // Show progress
-        #pragma omp critical (CBCALCupdateUI3)
-        {
-            emit newSetProcessed(i);
+        }
+    }
-#if !OLD_WAYZ
-    auto s0it = success0.cbegin();
-    qc0.erase(std::remove_if( qc0.begin(), qc0.end(), [&s0it](std::vector<cv::Point2f>){return !*s0it++;}),
-            qc0.end());
-    auto s1it = success1.cbegin();
-    qc1.erase(std::remove_if( qc1.begin(), qc1.end(), [&s1it](std::vector<cv::Point2f>){return !*s1it++;}),
-            qc1.end());
-    s0it = success0.cbegin(); s1it = success1.cbegin();
-    qc0Stereo.erase(std::remove_if( qc0Stereo.begin(), qc0Stereo.end(), [&s0it,&s1it](std::vector<cv::Point2f>){return !((*s0it++)&(*s1it++));}),
-            qc0Stereo.end());
-    s0it = success0.cbegin(); s1it = success1.cbegin();
-    qc1Stereo.erase(std::remove_if( qc1Stereo.begin(), qc1Stereo.end(), [&s0it,&s1it](std::vector<cv::Point2f>){return !((*s0it++)&(*s1it++));}),
-            qc1Stereo.end());
-    s0it = success0.cbegin(); s1it = success1.cbegin();
-    angles.erase(std::remove_if( angles.begin(), angles.end(), [&s0it,&s1it](float){return !((*s0it++)&(*s1it++));}),
-            angles.end());
-    s0it = success0.cbegin(); s1it = success1.cbegin();
-    fwdToStageEstimation.erase(std::remove_if( fwdToStageEstimation.begin(), fwdToStageEstimation.end(), [&s0it,&s1it](bool){return !((*s0it++)&(*s1it++));}),
-            fwdToStageEstimation.end());
-    s0it = success0.cbegin(); s1it = success1.cbegin();
-    TESTER.erase(std::remove_if( TESTER.begin(), TESTER.end(), [&s0it,&s1it](bool){return !((*s0it++)&(*s1it++));}),
-            TESTER.end());
-    /*for(unsigned int i=0; i<nSets; i++){
-        if(~success0[i])
-            std::remove(qc0.begin(),qc0.end(),i);
-        if(~success1[i])
-            std::remove(qc1.begin(),qc1.end(),i);
-        if(~success0[i] || ~success1[i]){
-            std::remove(qc0Stereo.begin(),qc0Stereo.end(),i);
-            std::remove(qc1Stereo.begin(),qc1Stereo.end(),i);
-            std::remove(angles.begin(),angles.end(),i);
-            std::remove(fwdToStageEstimation.begin(),fwdToStageEstimation.end(),i);
-        }
-    }*/
-#endif
-    std::copy(TESTER.begin(), TESTER.end(), std::ostream_iterator<unsigned int>(std::cout, " "));
-    std::cout << std::endl;
-    const size_t nValidStereoSets = angles.size();
-    if(nValidStereoSets < 2){
-        std::cerr << "Not enough valid calibration sequences!" << std::endl;
-        emit done();
-        return;
-    }
     // Generate world object coordinates [mm]
-    std::vector<cv::Point3f> Qi = generateObjCoordsInWorldCS(checkerCount,
-                                                             settings.value("calibration/squareSize", 10.0).toFloat());
+    std::vector<cv::Point3f> Qi = generateWorldCoords(checkerCount, settings.value("calibration/squareSize", 10.0).toFloat());
-    std::vector< std::vector<cv::Point3f> >
+    std::vector< std::vector<cv::Point3f> > Q0(qc0.size(), Qi), Q1(qc1.size(), Qi), QStereo(qc0Stereo.size(), Qi);
-            Q0(qc0.size(), Qi),
-            Q1(qc1.size(), Qi),
-            QStereo(nValidStereoSets, Qi);
-    // calibrate the cameras
+    // 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,
+    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::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,
+    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));
-    //std::cout << cal.k1 << std::endl;
-    // stereo calibration
+    // 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
+    #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
+    #else
         cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,
-                                               frameSize, R1, T1, E, F,flags_stereo,
+                                               frameSize, R1, T1, E, F, flags_stereo,
                                                cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));
-#endif
+    #endif
     cal.R1 = R1;
     cal.T1 = T1;
     cal.E = E;
     cal.F = F;
-    // Determine per-view reprojection errors:
+    // Print to log
-    perViewReprojError(success0, Q0, cam_tvecs0, cam_rvecs0, qc0, cal.K0, cal.k0, cal.cam0_errors_per_view);
-    perViewReprojError(success1, Q1, cam_tvecs1, cam_rvecs1, qc1, cal.K1, cal.k1, cal.cam1_errors_per_view);
-    // This would be so much nicer with range adaptors
-    std::vector< std::vector<cv::Point2f> > qc0StereoFwd2Stage;
-    std::vector< std::vector<cv::Point2f> > qc1StereoFwd2Stage;
-    for(size_t i=0; i<qc0Stereo.size(); i++ )
-        if(fwdToStageEstimation[i])
-            qc0StereoFwd2Stage.push_back(qc0Stereo[i]);
-    for(size_t i=0; i<qc1Stereo.size(); i++ )
-        if(fwdToStageEstimation[i])
-            qc1StereoFwd2Stage.push_back(qc1Stereo[i]);
-    performStageCalibration(qc0StereoFwd2Stage,qc1StereoFwd2Stage,cal);
-    // Print to std::cout
+    std::stringstream out;
-    std::cout << std::endl << "========== BEGIN Calibration info ================================================" << std::endl;
+    out << "## Camera Calibration ##" << std::endl
-    std::cout << "Num. Images used for intrinsics of cam0: " << qc0.size() << std::endl;
+        << "No. images used for intrinsics of cam0: " << qc0.size() << std::endl
-    std::cout << "Num. Images used for intrinsics of cam1: " << qc1.size() << std::endl;
+        << "No. images used for intrinsics of cam1: " << qc1.size() << std::endl
-    std::cout << "Num. Images used for extrinsocs of cam1: " << nValidStereoSets << std::endl;
+        << "No. images used for stereo calibration: " << qc0Stereo.size() << std::endl;
-    std::cout << "Num. Images used for rotation stage axis estim.: " << qc0StereoFwd2Stage.size() << std::endl << std::endl;
-    cal.print();
+    cal.printCamera(out);
-    std::cout << "========== END   Calibration info ================================================" << std::endl << std::endl;
+    out << std::endl << std::endl;
+    emit logMessage(QString::fromStdString(out.str()));
-    // save to (reentrant qsettings object)
+    // save to (reentrant) qsettings object
     settings.setValue("calibration/parameters", QVariant::fromValue(cal));
     emit done();
+}
-void SMCalibrationWorker::performStageCalibration(
+void SMCalibrationWorker::rotationStageCalibration(std::vector<SMCalibrationSet> calibrationData){
-        const std::vector< std::vector<cv::Point2f> > &qc0Stereo,
+    int nSets = calibrationData.size();
-        const std::vector< std::vector<cv::Point2f> > &qc1Stereo,
+    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
+    for(int i=0; i<nSets; i++){
-        SMCalibrationParameters& cal){
+        if(!calibrationData[i].selected)
+            continue;
+        // Note: avoiding push_back has only minor theoretical value
-    assert(qc0Stereo.size()==qc1Stereo.size());
+        if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){
-    if(qc0Stereo.size()>1 && qc1Stereo.size()>1){
+            qc0Stereo.push_back(calibrationData[i].qc0);
-        // save to (reentrant qsettings object)
+            qc1Stereo.push_back(calibrationData[i].qc1);
+        }
+    }
-        QSettings settings;
+    QSettings settings;
+    SMCalibrationParameters cal = settings.value("calibration/parameters").value<SMCalibrationParameters>();
+    if(qc0Stereo.size() > 2){
         // Generate world object coordinates [mm]
-        std::vector<cv::Point3f> Qi = generateObjCoordsInWorldCS(
-                    cv::Size(settings.value("calibration/patternSizeX", 22).toInt(),settings.value("calibration/patternSizeY", 13).toInt()),
+        const cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(),settings.value("calibration/patternSizeY", 13).toInt()));
-                    settings.value("calibration/squareSize", 10.0).toFloat());
+        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.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
+        #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);
             Qcam[i] = QcamiPoints;
+        }
         cv::Vec3f axis, point;
         float rot_axis_error;
+        rotationAxisEstimation(Qcam, Qi, axis, point);
-        rotationAxisCalibration(Qcam, Qi, axis, point, rot_axis_error);
+        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);
         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{
+    } else {
         cal.Rr = cv::Matx33f::eye();
         cal.Tr = cv::Vec3f(0,0,0);
         cal.rot_axis_error = -1;
+        return;
+    }
+    // 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 207 → 225