Rev 167 | Rev 195 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed
#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/patternSizeX", 22).toInt();
int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();
cv::Size checkerCount(checkerCountX, checkerCountY);
unsigned int nSets = calibrationData.size();
// 2D Points collected for OpenCV's calibration procedures
std::vector< std::vector<cv::Point2f> > qc0, qc1;
std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
std::vector<bool> success0(nSets), success1(nSets);
std::vector<float> angles;
// Loop through calibration sets
for(unsigned int i=0; i<nSets; i++){
SMCalibrationSet SMCalibrationSetI = calibrationData[i];
if(!SMCalibrationSetI.checked)
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);
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
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);
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
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);
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);
}
// Show progress
emit newSetProcessed(i);
}
size_t nValidSets = angles.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/squareSize", 10.0).toFloat(); // width and height of one checker square 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> > Q0, Q1, QStereo;
for(unsigned int i=0; i<qc0.size(); i++)
Q0.push_back(Qi);
for(unsigned int i=0; i<qc1.size(); i++)
Q1.push_back(Qi);
for(unsigned int i=0; i<nValidSets; i++)
QStereo.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_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;
// // 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,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
//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;
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);
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);
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);
}
cal.cam0_errors_per_view[i] = (float)err/n;
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){
if(success1[i]){
int n = (int)Q1[idx].size();
std::vector<cv::Point2f> qc_proj;
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++){
cv::Point2f d = qc1[idx][j] - qc_proj[j];
err += cv::sqrt(d.x*d.x + d.y*d.y);
}
cal.cam1_errors_per_view[i] = (float)err/n;
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(qc0[i], qc0i, cal.K0, cal.k0);
cv::undistortPoints(qc1[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::Vec3f axis, point;
cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);
// 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;
// Print to std::cout
cal.print();
// save to (reentrant qsettings object)
settings.setValue("calibration/parameters", QVariant::fromValue(cal));
emit done();
}