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

 #include "SMCalibrationWorker.h"
 #include "SMCalibrationParameters.h"
 #include "cvtools.h"
 #include <QSettings>
 void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
     QSettings settings;
     // Number of saddle points on calibration pattern
     int checkerCountX = settings.value("calibration/checkerCountX", 22).toInt();
     int checkerCountY = settings.value("calibration/checkerCountY", 13).toInt();
     cv::Size checkerCount(checkerCountX, checkerCountY);
     int nSets = calibrationData.size();
     std::vector< std::vector<cv::Point2f> > qc0, qc1;
     std::vector<float> angles;
     // Loop through calibration sets
     for(int i=0; i<nSets; i++){
         SMCalibrationSet SMCalibrationSetI = calibrationData[i];
         if(!SMCalibrationSetI.checked)
             continue;
         // Camera 0
         std::vector<cv::Point2f> qci0;
         // Extract checker corners
         bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
         if(success0){
             cv::Mat gray;
             cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
             cv::cornerSubPix(gray, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
             // Draw colored chessboard
             SMCalibrationSetI.frame0Result = SMCalibrationSetI.frame0.clone();
             cvtools::drawChessboardCorners(SMCalibrationSetI.frame0Result, checkerCount, qci0, success0, 10);
+        }
         emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
         // Camera 1
         std::vector<cv::Point2f> qci1;
         // Extract checker corners
         bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
         if(success1){
             cv::Mat gray;
             cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
             cv::cornerSubPix(gray, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.001));
             // Draw colored chessboard
             SMCalibrationSetI.frame1Result = SMCalibrationSetI.frame1.clone();
             cvtools::drawChessboardCorners(SMCalibrationSetI.frame1Result, checkerCount, qci1, success1, 10);
+        }
         emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
         SMCalibrationSetI.success = success0 && success1;
         // Add to whole set
         if(SMCalibrationSetI.success){
             qc0.push_back(qci0);
             qc1.push_back(qci1);
             angles.push_back(SMCalibrationSetI.rotationAngle);
+        }
         // Show progress
         emit newSetProcessed(i);
+    }
     int nValidSets = qc0.size();
     if(nValidSets < 2){
         std::cerr << "Not enough valid calibration sequences!" << std::endl;
         emit done();
         return;
+    }
     // Generate world object coordinates [mm]
     float checkerSize = settings.value("calibration/checkerSize", 15.0).toFloat(); // width and height of one field in mm
     std::vector<cv::Point3f> Qi;
     for (int h=0; h<checkerCount.height; h++)
         for (int w=0; w<checkerCount.width; w++)
             Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
     std::vector< std::vector<cv::Point3f> > Q;
     for(int i=0; i<qc0.size(); i++)
         Q.push_back(Qi);
     // calibrate the cameras
     SMCalibrationParameters cal;
     cal.frameWidth = calibrationData[0].frame0.cols;
     cal.frameHeight = calibrationData[0].frame0.rows;
     cv::Size frameSize(cal.frameWidth, cal.frameHeight);
     // determine only k1, k2 for lens distortion
     int flags = cv::CALIB_FIX_K3;
     // 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(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
     // 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(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
     // stereo calibration (fix K0, K1, k0, k1)
     int flags_stereo = cv::CALIB_FIX_INTRINSIC;
     cv::Mat E, F, R1, T1;
     cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
                                               frameSize, R1, T1, E, F,
                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
                                               flags_stereo);
     cal.R1 = R1;
     cal.T1 = T1;
     cal.E = E;
     cal.F = F;
-    // calibrate rotation axis 1
+//    // hand-eye calibration
-    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame
+//    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::Vec3f> Tc(nValidSets - 1);
-    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
+//    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
-    std::vector<cv::Vec3f> Tr(nValidSets - 1);
+//    std::vector<cv::Vec3f> Tr(nValidSets - 1);
-    for(int i=0; i<nValidSets-1; i++){
+//    for(int i=0; i<nValidSets-1; i++){
-        // relative transformations in camera
+//        // relative transformations in camera
-        cv::Mat cRw1, cRw2;
+//        cv::Mat cRw1, cRw2;
-        cv::Rodrigues(cam_rvecs0[i], cRw1);
+//        cv::Rodrigues(cam_rvecs0[i], cRw1);
-        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
+//        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
-        cv::Mat cTw1 = cam_tvecs0[i];
+//        cv::Mat cTw1 = cam_tvecs0[i];
-        cv::Mat cTw2 = cam_tvecs0[i+1];
+//        cv::Mat cTw2 = cam_tvecs0[i+1];
-        cv::Mat w1Rc = cRw1.t();
+//        cv::Mat w1Rc = cRw1.t();
-        cv::Mat w1Tc = -cRw1.t()*cTw1;
+//        cv::Mat w1Tc = -cRw1.t()*cTw1;
-        Rc[i] = cv::Mat(cRw2*w1Rc);
+//        Rc[i] = cv::Mat(cRw2*w1Rc);
-        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);
+//        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);
-        // relative transformations in rotation stage
+//        // relative transformations in rotation stage
-        // we define the rotation axis to be in origo, pointing in positive y direction
+//        // 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;
+//        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;
-        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
+//        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
-        cv::Mat Rri;
+//        cv::Mat Rri;
-        cv::Rodrigues(rot_rvec, Rri);
+//        cv::Rodrigues(rot_rvec, Rri);
-        Rr[i] = Rri;
+//        Rr[i] = Rri;
-        Tr[i] = 0.0;
+//        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;
+//    }
-//        std::cout << i << std::endl;
-//        std::cout << "cTw1" << cTw1 << std::endl;
-//        std::cout << "cTw2" << cTw2 << std::endl;
+//    // determine the transformation from rotation stage to camera 0
-//        std::cout << "w2Rc" << w2Rc << std::endl;
+//    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
-//        std::cout << "w2Tc" << w2Tc << std::endl;
-//        std::cout << "w2Rc" << w2Rc << std::endl;
+//    for(int i=0; i<nValidSets-1; i++){
-//        std::cout << "w2Tc" << w2Tc << std::endl;
+//        std::cout << i << std::endl;
 //        cv::Mat Rci;
 //        cv::Rodrigues(Rc[i], Rci);
-//        std::cout << "Rci" << Rci << std::endl;
+//        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 << "rot_rvec" << rot_rvec << std::endl;
+//        std::cout << "Rr[i]" << Rri << 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);
+//        cv::Mat Rcr = cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t());
-    for(int i=0; i<nValidSets-1; i++){
+//        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 << i << std::endl;
+//        std::cout << 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;
-    }
-std::cout << "Rr: " << cal.Rr << std::endl;
-std::cout << "Tr: " << cal.Tr << std::endl;
-//    //* Using rotation axis calibration *//
+    // Direct rotation axis calibration //
-//    // full camera matrices
+    // full camera matrices
-//    cv::Matx34f P0 = cal.K0*cv::Matx34f::eye();
+    cv::Matx34f P0 = cv::Matx34f::eye();
-//    cv::Mat RT1(3, 4, CV_32F);
+    cv::Mat RT1(3, 4, CV_32F);
-//    cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
+    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::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
-//    cv::Matx34f P1 = cal.K1*cv::Matx34f(RT1);
+    cv::Matx34f P1 = cv::Matx34f(RT1);
-//    // calibration points in camera 0 frame
+    // calibration points in camera 0 frame
-//    std::vector< std::vector<cv::Point3f> > Qcam;
+    std::vector< std::vector<cv::Point3f> > Qcam;
-//    for(int i=0; i<nValidSets; i++){
+    for(int i=0; i<nValidSets; i++){
-//        std::vector<cv::Point2f> qc0i, qc1i;
+        std::vector<cv::Point2f> qc0i, qc1i;
-////        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
+        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);
-////        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);
+        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);
 //        qc0i = qc0[i];
 //        qc1i = qc1[i];
-//        cv::Mat Qhom, Qcami;
+        cv::Mat Qhom, Qcami;
-//        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
+        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
-//        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
+        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
-//        std::vector<cv::Point3f> QcamiPoints;
+        std::vector<cv::Point3f> QcamiPoints;
-//        cvtools::matToPoints3f(Qcami, QcamiPoints);
+        cvtools::matToPoints3f(Qcami, QcamiPoints);
-//        Qcam.push_back(QcamiPoints);
+        Qcam.push_back(QcamiPoints);
-//    }
+    }
-//    cv::Vec3f axis, point;
+    cv::Vec3f axis, point;
-//    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);
+    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);
-//    // construct transformation matrix
+    // construct transformation matrix
-//    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,-1.0));
+    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
+    ex = cv::normalize(ex);
-//    cv::Vec3f ez = ex.cross(axis);
+    cv::Vec3f ez = ex.cross(axis);
+    ez = cv::normalize(ez);
-//    cv::Mat RrMat(3, 3, CV_32F);
+    cv::Mat RrMat(3, 3, CV_32F);
-//    cv::Mat(ex).copyTo(RrMat.col(0));
+    cv::Mat(ex).copyTo(RrMat.col(0));
-//    cv::Mat(axis).copyTo(RrMat.col(1));
+    cv::Mat(axis).copyTo(RrMat.col(1));
-//    cv::Mat(ez).copyTo(RrMat.col(2));
+    cv::Mat(ez).copyTo(RrMat.col(2));
-//    cal.Rr = cv::Mat(RrMat.t());
+    cal.Rr = cv::Matx33f(RrMat).t();
-//    cal.Tr = point;
+    cal.Tr = -cv::Matx33f(RrMat).t()*point;
     // Print to std::cout
     cal.print();
     // save to (reentrant qsettings object)
     settings.setValue("calibration/parameters", QVariant::fromValue(cal));
     emit done();
+}

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 88 → 91