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

 #include <ceres/ceres.h>
 struct CircleResidual {
-  CircleResidual(std::vector<cv::Point3f> _pointsOnArc)
+    CircleResidual(std::vector<cv::Point3f> _pointsOnArc)
-      : pointsOnArc(_pointsOnArc) {}
+        : pointsOnArc(_pointsOnArc) {}
-  template <typename T>
+    template <typename T>
-  bool operator()(const T* point, const T* axis, T* residual) const {
+    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];
+        T axisSqNorm = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
-    unsigned int l = pointsOnArc.size();
+        unsigned int l = pointsOnArc.size();
-    std::vector<T> dI(l);
+        std::vector<T> dI(l);
-    // note, this is automatically initialized to 0
+        // note, this is automatically initialized to 0
-    T sum(0.0);
+        T sum(0.0);
-    for(unsigned int i=0; i<l; i++){
+        for(unsigned int i=0; i<l; i++){
-      cv::Point3d p = pointsOnArc[i];
+            cv::Point3d p = pointsOnArc[i];
-      //T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};
+            //T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};
-      // point to line distance
+            // 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 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 dIx = point[0] - p.x - (dotProd*axis[0]/axisSqNorm);
-      T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);
+            T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);
-      T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);
+            T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);
-      dI[i] = ceres::sqrt(dIx*dIx + dIy*dIy + dIz*dIz);
+            dI[i] = ceres::sqrt(dIx*dIx + dIy*dIy + dIz*dIz);
-      sum += dI[i];
+            sum += dI[i];
-    }
+        }
-    T mean = sum / double(l);
+        T mean = sum / double(l);
-    for(unsigned int i=0; i<l; i++){
+        for(unsigned int i=0; i<l; i++){
-        residual[i] = dI[i] - mean;
+            residual[i] = dI[i] - mean;
-    }
+        }
-    return true;
+        return true;
-  }
+    }
- private:
+private:
-  // Observations for one checkerboard corner.
+    // Observations for one checkerboard corner.
-  const std::vector<cv::Point3f> pointsOnArc;
+    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){
+    assert(Qobj.size() == Qcam[0].size());
     // 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 i = Qobj[idx].x+4;
-//            float j = Qobj[idx].y+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);
     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
+    // Paper version: c is initially eliminated
-//    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
+    /*cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
-//    cv::Mat bb(l*mn, 1, CV_32F);
+        cv::Mat bb(l*mn, 1, CV_32F);
-//    for(int k=0; k<l; k++){
+        for(int k=0; k<l; k++){
-//        for(unsigned int idx=0; idx<mn; idx++){
+            for(unsigned int idx=0; idx<mn; idx++){
-//            float i = Qobj[idx].x;
+                float i = Qobj[idx].x;
-//            float j = Qobj[idx].y;
+                float j = Qobj[idx].y;
-//            float x = Qcam[k][idx].x;
+                float x = Qcam[k][idx].x;
-//            float y = Qcam[k][idx].y;
+                float y = Qcam[k][idx].y;
-//            float z = Qcam[k][idx].z;
+                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);
+                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;
+                int row = k*mn+idx;
-//            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
+                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, 1) = 2*y - (2*z*ny)/nz;
-//            A.at<float>(row, idx+2) = 1.0;
+                A.at<float>(row, idx+2) = 1.0;
-//            bb.at<float>(row, 0) = f + (2*z*d)/nz;
+                bb.at<float>(row, 0) = f + (2*z*d)/nz;
-//        }
+            }
-//    }
+        }
-//    // solve for point
+        // solve for point
-//    cv::Mat abe;
+        cv::Mat abe;
-//    cv::solve(A, bb, abe, cv::DECOMP_SVD);
+        cv::solve(A, bb, abe, cv::DECOMP_SVD);
-//    float a = abe.at<float>(0, 0);
+        float a = abe.at<float>(0, 0);
-//    float b = abe.at<float>(1, 0);
+        float b = abe.at<float>(1, 0);
-//    float c = -(nx*a+ny*b+d)/nz;
+        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);
         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 ceres::AutoDiffCostFunction<CircleResidual, ceres::DYNAMIC, 3, 3>(
-               new CircleResidual(pointsOnArch), l);
+                    new CircleResidual(pointsOnArch), l);
         problem.AddResidualBlock(cost_function, NULL, pointArray, axisArray);
+    }
     // Run the solver!
     ceres::Solver::Options options;
     // 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);
+            cv::cvtColor(SMCalibrationSetI_frameX, SMCalibrationSetI_frameXResult, CV_BayerBG2RGB);
         else
-            color = SMCalibrationSetI_frameX.clone();
+            SMCalibrationSetI_frameXResult = SMCalibrationSetI_frameX.clone();
-        cvtools::drawChessboardCorners(color, checkerCount, qciX, success, 10);
+        cvtools::drawChessboardCorners(SMCalibrationSetI_frameXResult, checkerCount, qciX, success, 10);
-        SMCalibrationSetI_frameXResult = color;
+    }
     return success;
+}
-void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
+std::vector<cv::Point3f> SMCalibrationWorker::generateObjCoordsInWorldCS(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;
+}
+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);
-    QSettings settings;
+    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;
+    }
+}
-    // Number of saddle points on calibration pattern
-    int checkerCountX = settings.value("calibration/patternSizeX", 22).toInt();
-    int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();
+void SMCalibrationWorker::performCameraCalibration(std::vector<SMCalibrationSet> calibrationData){
-    cv::Size checkerCount(checkerCountX, checkerCountY);
+    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;
+#define OLD_WAYZ 0
+#if OLD_WAYZ
-    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
+    std::vector< std::vector<cv::Point2f> > qc0, qc1, qc0Stereo, qc1Stereo;
-    std::vector<bool> success0(nSets), success1(nSets);
+    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];
+        // TODO this changes semantics of the checkboxes in the GUI
-        if(!SMCalibrationSetI.checked)
+        /*if(!SMCalibrationSetI.checked)
-            continue;
+            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);
+        success0[i] = processCBCorners(checkerCount, SMCalibrationSetI.frame0, SMCalibrationSetI.frame0Result, qci0);
-        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
+#pragma omp critical (CBCALCupdateUI1)
-            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);
+        success1[i] = processCBCorners(checkerCount, SMCalibrationSetI.frame1, SMCalibrationSetI.frame1Result, qci1);
-        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
+#pragma omp critical (CBCALCupdateUI2)
-            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);
+}
+        // store results
+#if OLD_WAYZ
         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);
+            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);
+            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;
-    size_t nValidSets = angles.size();
+    const size_t nValidStereoSets = angles.size();
-    if(nValidSets < 2){
+    if(nValidStereoSets < 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;
+    std::vector<cv::Point3f> Qi = generateObjCoordsInWorldCS(checkerCount,
-    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));
+                                                             settings.value("calibration/squareSize", 10.0).toFloat());
-    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
+    std::vector< std::vector<cv::Point3f> >
-    for(unsigned int i=0; i<qc0.size(); i++)
-        Q0.push_back(Qi);
+            Q0(qc0.size(), Qi),
-    for(unsigned int i=0; i<qc1.size(); i++)
-        Q1.push_back(Qi);
+            Q1(qc1.size(), Qi),
-    for(unsigned int i=0; i<nValidSets; i++)
-        QStereo.push_back(Qi);
+            QStereo(nValidStereoSets, Qi);
     // calibrate the cameras
     SMCalibrationParameters cal;
     cal.frameWidth = calibrationData[0].frame0.cols;
     cal.frameHeight = calibrationData[0].frame0.rows;
     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;
+    //std::cout << cal.k0 << std::endl;
-//    // refine extrinsics for camera 0
+    //    // refine extrinsics for camera 0
-//    for(int i=0; i<Q.size(); i++)
+    //    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);
+    //        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;
+    //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;
+#if CV_MAJOR_VERSION < 3
-    cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,
+        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,
+                                               frameSize, R1, T1, E, F,flags_stereo,
-                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));
+                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));
+#endif
     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);
+    perViewReprojError(success0, Q0, cam_tvecs0, cam_rvecs0, qc0, cal.K0, cal.k0, cal.cam0_errors_per_view);
-            float err = 0;
-            for(unsigned int j=0; j<qc_proj.size(); j++){
+    perViewReprojError(success1, Q1, cam_tvecs1, cam_rvecs1, qc1, cal.K1, cal.k1, cal.cam1_errors_per_view);
-                cv::Point2f d = qc0[idx][j] - qc_proj[j];
+    // This would be so much nicer with range adaptors
-                err += cv::sqrt(d.x*d.x + d.y*d.y);
+    std::vector< std::vector<cv::Point2f> > qc0StereoFwd2Stage;
-            }
-            cal.cam0_errors_per_view[i] = (float)err/n;
+    std::vector< std::vector<cv::Point2f> > qc1StereoFwd2Stage;
-            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){
+    for(size_t i=0; i<qc0Stereo.size(); i++ )
-        if(success1[i]){
+        if(fwdToStageEstimation[i])
-            int n = (int)Q1[idx].size();
-            std::vector<cv::Point2f> qc_proj;
+            qc0StereoFwd2Stage.push_back(qc0Stereo[i]);
-            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++){
+    for(size_t i=0; i<qc1Stereo.size(); i++ )
-                cv::Point2f d = qc1[idx][j] - qc_proj[j];
-                err += cv::sqrt(d.x*d.x + d.y*d.y);
+        if(fwdToStageEstimation[i])
-            }
-            cal.cam1_errors_per_view[i] = (float)err/n;
+            qc1StereoFwd2Stage.push_back(qc1Stereo[i]);
-            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);
+    performStageCalibration(qc0StereoFwd2Stage,qc1StereoFwd2Stage,cal);
-    // 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
+    std::cout << std::endl << "========== BEGIN Calibration info ================================================" << std::endl;
+    std::cout << "Num. Images used for intrinsics of cam0: " << qc0.size() << std::endl;
+    std::cout << "Num. Images used for intrinsics of cam1: " << qc1.size() << std::endl;
+    std::cout << "Num. Images used for extrinsocs of cam1: " << nValidStereoSets << std::endl;
+    std::cout << "Num. Images used for rotation stage axis estim.: " << qc0StereoFwd2Stage.size() << std::endl << std::endl;
     cal.print();
+    std::cout << "========== END   Calibration info ================================================" << std::endl << std::endl;
     // save to (reentrant qsettings object)
     settings.setValue("calibration/parameters", QVariant::fromValue(cal));
     emit done();
+}
+void SMCalibrationWorker::performStageCalibration(
+        const std::vector< std::vector<cv::Point2f> > &qc0Stereo,
+        const std::vector< std::vector<cv::Point2f> > &qc1Stereo,
+        SMCalibrationParameters& cal){
+    assert(qc0Stereo.size()==qc1Stereo.size());
+    if(qc0Stereo.size()>1 && qc1Stereo.size()>1){
+        // save to (reentrant qsettings object)
+        QSettings settings;
+        // 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()),
+                    settings.value("calibration/squareSize", 10.0).toFloat());
+        // 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;
+        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;
+    }else{
+        cal.Rr = cv::Matx33f::eye();
+        cal.Tr = cv::Vec3f(0,0,0);
+        cal.rot_axis_error = -1;
+    }
+}

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp @ 225 – Rev 206 → 207