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", 10).toInt();
+    int checkerCountY = settings.value("calibration/checkerCountY", 9).toInt();
-    cv::Size patternSize(10, 9);
+    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, patternSize, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
+        bool success0 = cv::findChessboardCorners(SMCalibrationSetI.frame0, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
         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();
-            cv::drawChessboardCorners(SMCalibrationSetI.frame0Result, patternSize, qci0, success0);
+            cv::drawChessboardCorners(SMCalibrationSetI.frame0Result, checkerCount, qci0, success0);
+        }
         emit newFrameResult(i, 0, success0, SMCalibrationSetI.frame0Result);
         // Camera 1
         std::vector<cv::Point2f> qci1;
         // Extract checker corners
-        bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, patternSize, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
+        bool success1 = cv::findChessboardCorners(SMCalibrationSetI.frame1, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
         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();
-            cv::drawChessboardCorners(SMCalibrationSetI.frame1Result, patternSize, qci1, success1);
+            cv::drawChessboardCorners(SMCalibrationSetI.frame1Result, checkerCount, qci1, success1);
+        }
         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<patternSize.height; h++)
+    for (int h=0; h<checkerCount.height; h++)
-        for (int w=0; w<patternSize.width; w++)
+        for (int w=0; w<checkerCount.width; w++)
-            Qi.push_back(cv::Point3f(5 * w, 5* h, 0.0)); // 5mm chess field size
+            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);
     int flags = 0; //cv::CALIB_FIX_K3;
+    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
-    std::vector<cv::Vec3f> cam_rvecs0, cam_tvecs0;
+    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);
-    std::vector<cv::Vec3f> cam_rvecs1, cam_tvecs1;
+    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, cal.R1, cal.T1, cal.E, cal.F,
+                                              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
     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::Matx33f cRw1, cRw2;
+        cv::Mat cRw1, cRw2;
         cv::Rodrigues(cam_rvecs0[i], cRw1);
         cv::Rodrigues(cam_rvecs0[i+1], cRw2);
-        cv::Vec3f cTw1 = cam_tvecs0[i];
+        cv::Mat cTw1 = cam_tvecs0[i];
-        cv::Vec3f cTw2 = cam_tvecs0[i+1];
+        cv::Mat cTw2 = cam_tvecs0[i+1];
+        cv::Mat w1Rc = cRw1.t();
-        cv::composeRT(cRw1, cTw1, cRw2.t(), -cRw2.t()*cTw2, Rc[i], Tc[i]);
+        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]/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::Rodrigues(rot_rvec, Rr[i]);
+        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::fitSixDofData(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
+    cvtools::fitSixDofData(Rr, Tr, Rc, Tc, cal.Rr, cal.Tr);
+    for(int i=0; i<Rc.size(); i++){
+//        cv::Matx33f a = cal.Rr*Rc[i];
+//        cv::Matx33f b = Rr[i];
+//        std::cout << "a:" << a << std::endl;
+//        std::cout << "b:" << b << std::endl;
+        cv::Vec3f a = cal.Rr*Tr[i]+cal.Tr;
+        cv::Vec3f b = Tc[i];
+        std::cout << "a:" << a << std::endl;
+        std::cout << "b:" << b << std::endl;
+    }
+    cv::Mat rrvec;
+    cv::Rodrigues(cal.Rr, rrvec);
+    std::cout << "rrvec:" << rrvec << std::endl;
     // Print to std::cout
     cal.print();
     // save to (reentrant qsettings object)
-    QSettings settings;
-    settings.setValue("calibration", QVariant::fromValue(cal));
+    settings.setValue("calibration/parameters", QVariant::fromValue(cal));
     emit done();
+}

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 31 → 33