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){
     // Number of saddle points on calibration pattern
     cv::Size patternSize(10, 9);
     int nSets = calibrationData.size();
-    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
+    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);
         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);
+        }
         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);
         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);
+        }
         emit newFrameResult(i, 1, success1, SMCalibrationSetI.frame1Result);
         SMCalibrationSetI.success = success0 && success1;
         // Add to whole set
         if(SMCalibrationSetI.success){
-            qc[0].push_back(qci0);
+            qc0.push_back(qci0);
-            qc[1].push_back(qci1);
+            qc1.push_back(qci1);
+            angles.push_back(SMCalibrationSetI.rotationAngle);
+        }
         // Show progress
         emit newSetProcessed(i);
+    }
-    int nValidSets = qc[0].size();
+    int nValidSets = qc0.size();
     if(nValidSets < 2){
         std::cerr << "Not enough valid calibration sequences!" << std::endl;
         emit done();
         return;
+    }
     // Generate world object coordinates [mm]
     std::vector<cv::Point3f> Qi;
     for (int h=0; h<patternSize.height; h++)
         for (int w=0; w<patternSize.width; w++)
             Qi.push_back(cv::Point3f(5 * w, 5* h, 0.0)); // 5mm chess field size
     std::vector< std::vector<cv::Point3f> > Q;
-    for(int i=0; i<qc[0].size(); i++)
+    for(int i=0; i<qc0.size(); i++)
         Q.push_back(Qi);
     // calibrate the cameras
-    cv::Size frameSize(calibrationData[0].frame0.cols, calibrationData[0].frame0.rows);
-//    cv::Size frameSize(640, 480);
-    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
-    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
-    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
     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;
-    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
+    std::vector<cv::Vec3f> cam_rvecs0, cam_tvecs0;
-    cv::Mat K0, k0;
-    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0);
+    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags);
-    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
+    std::vector<cv::Vec3f> cam_rvecs1, cam_tvecs1;
-    cv::Mat K1, k1;
-    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1);
+    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags);
-    // stereo calibration (don't change K0, K1, k0, k1)
+    // stereo calibration (fix K0, K1, k0, k1)
-    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
+    int flags_stereo = cv::CALIB_FIX_INTRINSIC;
-    cv::Mat E, F, R1, T1;
-    cal.stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], K0, k0, K1, k1,
+    cal.stereo_error = cv::stereoCalibrate(Q, qc0, qc1, cal.K0, cal.k0, cal.K1, cal.k1,
-                                              frameSize, R1, T1, E, F,
+                                              frameSize, cal.R1, cal.T1, cal.E, cal.F,
                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
                                               flags_stereo);
+    // 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::Rodrigues(cam_rvecs0[i], cRw1);
+        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
+        cv::Vec3f cTw1 = cam_tvecs0[i];
+        cv::Vec3f cTw2 = cam_tvecs0[i+1];
+        cv::composeRT(cRw1, cTw1, cRw2.t(), -cRw2.t()*cTw2, Rc[i], Tc[i]);
+        // 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;
+        cv::Vec3f rot_rvec(0.0, angleRadians, 0.0);
+        cv::Rodrigues(rot_rvec, Rr[i]);
+        Tr[i] = 0.0;
+    }
+    cvtools::fitSixDofData(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
     // Print to std::cout
     cal.print();
     // save to (reentrant qsettings object)
     QSettings settings;
     settings.setValue("calibration", QVariant::fromValue(cal));
     emit done();
+}

Subversion Repositories seema-scanner

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