Rev 249 | Blame | Compare with Previous | Last modification | View Log | RSS feed
#include "SMCalibrationWorker.h"
#include "SMCalibrationParameters.h"
#include "cvtools.h"
#include <opencv2/aruco/charuco.hpp>
#include <QSettings>
#include <QTextStream>
#include <ceres/ceres.h>
// 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 rotationAxisEstimation(const std::vector< std::vector<cv::Point3f> > Qcam,
const std::vector<cv::Point3f> Qobj,
cv::Vec3f &axis, cv::Vec3f &point){
assert(Qobj.size() == Qcam[0].size());
// number of frames (points on each arch)
auto l = Qcam.size();
// number of points in each frame
size_t mn = Qobj.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 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,
0, y*y, y*z, y, i*y, j*y,
0, 0, z*z, z, i*z, j*z,
0, 0, 0, 1, i, j,
0, 0, 0, 0, i*i, i*j,
0, 0, 0, 0, 0, j*j);
}
}
cv::completeSymm(M, false);
// solve for axis
std::vector<float> lambda;
cv::Mat u;
cv::eigen(M, lambda, u);
float minLambda = std::abs(lambda[0]);
int idx = 0;
for(unsigned int i=1; i<lambda.size(); i++){
if(abs(lambda[i]) < minLambda){
minLambda = lambda[i];
idx = i;
}
}
axis = u.row(idx).colRange(0, 3);
axis = cv::normalize(axis);
float nx = u.at<float>(idx, 0);
float ny = u.at<float>(idx, 1);
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
/*cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
cv::Mat bb(l*mn, 1, CV_32F);
for(int k=0; k<l; k++){
for(unsigned int idx=0; idx<mn; idx++){
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;
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;
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, idx+2) = 1.0;
bb.at<float>(row, 0) = f + (2*z*d)/nz;
}
}
// solve for point
cv::Mat abe;
cv::solve(A, bb, abe, cv::DECOMP_SVD);
float a = abe.at<float>(0, 0);
float b = abe.at<float>(1, 0);
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);
for(int k=0; k<l; k++){
for(unsigned int idx=0; idx<mn; idx++){
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;
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;
A.at<float>(row, 0) = 2*x;
A.at<float>(row, 1) = 2*y;
A.at<float>(row, 2) = 2*z;
A.at<float>(row, idx+3) = 1.0;
bb.at<float>(row, 0) = f;
}
}
// solve for point
cv::Mat abe;
cv::solve(A, bb, abe, cv::DECOMP_SVD);
float a = abe.at<float>(0, 0);
float b = abe.at<float>(1, 0);
float c = abe.at<float>(2, 0);
point[0] = a;
point[1] = b;
point[2] = c;
}
struct CircleResidual {
CircleResidual(std::vector<cv::Point3f> _pointsOnArc)
: pointsOnArc(_pointsOnArc) {}
template <typename T>
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];
unsigned int l = pointsOnArc.size();
std::vector<T> dI(l);
// note, this is automatically initialized to 0
T sum(0.0);
for(unsigned int i=0; i<l; i++){
cv::Point3d p = pointsOnArc[i];
//T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};
// 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 dIx = point[0] - p.x - (dotProd*axis[0]/axisSqNorm);
T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);
T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);
dI[i] = ceres::sqrt(dIx*dIx + dIy*dIy + dIz*dIz);
sum += dI[i];
}
T mean = sum / double(l);
for(unsigned int i=0; i<l; i++){
residual[i] = dI[i] - mean;
}
return true;
}
private:
// Observations for one checkerboard corner.
const std::vector<cv::Point3f> pointsOnArc;
};
static void rotationAxisOptimization(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point, float &error){
// number of frames (points on each arch)
size_t l = Qcam.size();
// number of points in each frame
size_t mn = Qobj.size();
// read initial guess
double pointArray[] = {point[0], point[1], point[2]};
double axisArray[] = {axis[0], axis[1], axis[2]};
ceres::Problem problem;
// loop through saddle points
for(unsigned int idx=0; idx<mn; idx++){
std::vector<cv::Point3f> pointsOnArch(l);
for(unsigned int k=0; k<l; k++){
pointsOnArch[k] = Qcam[k][idx];
}
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<CircleResidual, ceres::DYNAMIC, 3, 3>(
new CircleResidual(pointsOnArch), l);
problem.AddResidualBlock(cost_function, NULL, pointArray, axisArray);
}
// Run the solver!
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
point = cv::Vec3f(pointArray[0], pointArray[1], pointArray[2]);
axis = cv::Vec3f(axisArray[0], axisArray[1], axisArray[2]);
axis /= cv::norm(axis);
// Error estimate (sum of squared differences)
error = 0;
// loop through saddle points
for(unsigned int idx=0; idx<mn; idx++){
// vector of distances from rotation axis
std::vector<float> dI(l);
// loop through angular positions
for(unsigned int k=0; k<l; k++){
cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);
// point to line distance
dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);
}
float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
float mean = sum / dI.size();
float meanDev = 0;
for(unsigned int k=0; k<l; k++){
meanDev += std::abs(dI[k] - mean);
}
meanDev /= l;
error += meanDev;
}
error /= mn;
}
static std::vector<cv::Point3f> generateWorldCoords(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;
}
static bool detectCheckerBoardCorners(const cv::Size & checkerCount,
const cv::Mat & frame,
cv::Mat & frameResult,
std::vector<cv::Point2f> & qc){
// Convert to grayscale
cv::Mat gray;
if(frame.channels() == 1)
cv::cvtColor(frame, gray, CV_BayerBG2GRAY);
else
cv::cvtColor(frame, gray, CV_RGB2GRAY);
// Extract checker corners
bool success = cv::findChessboardCorners(gray, checkerCount, qc, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
if(success){
cv::cornerSubPix(gray, qc, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
// Draw colored chessboard
if(frame.channels() == 1)
cv::cvtColor(frame, frameResult, CV_BayerBG2RGB);
else
frameResult = frame.clone();
cvtools::drawChessboardCorners(frameResult, checkerCount, qc, success, 10);
} else {
qc.clear();
}
return success;
}
void SMCalibrationWorker::checkerboardDetection(SMCalibrationSet calibrationSet){
QSettings settings;
cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));
bool success0 = detectCheckerBoardCorners(checkerCount, calibrationSet.frame0, calibrationSet.frame0Result, calibrationSet.qc0);
if(!success0){
// calibrationSet.qc0.clear();
emit logMessage(QString("Could not detect checkerboard on set %1 camera0").arg(calibrationSet.id));
std::cerr << "Could not detect checkerboard on set " << calibrationSet.id << " camera0" << std::endl;
}
bool success1 = detectCheckerBoardCorners(checkerCount, calibrationSet.frame1, calibrationSet.frame1Result, calibrationSet.qc1);
if(!success1){
// calibrationSet.qc1.clear();
emit logMessage(QString("Could not detect checkerboard on set %1 camera1").arg(calibrationSet.id));
std::cerr << "Could not detect checkerboard on set " << calibrationSet.id << " camera1" << std::endl;
}
emit newCheckerboardResult(calibrationSet.id, calibrationSet);
}
void SMCalibrationWorker::cameraCalibration(std::vector<SMCalibrationSet> calibrationData){
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, qc0Stereo, qc1Stereo;
for(unsigned int i=0; i<nSets; i++){
if(!calibrationData[i].selected)
continue;
// Note: avoiding push_back has only minor theoretical value
if(!calibrationData[i].qc0.empty())
qc0.push_back(calibrationData[i].qc0);
if(!calibrationData[i].qc1.empty())
qc1.push_back(calibrationData[i].qc1);
if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){
qc0Stereo.push_back(calibrationData[i].qc0);
qc1Stereo.push_back(calibrationData[i].qc1);
}
}
// Generate world object coordinates [mm]
std::vector<cv::Point3f> Qi = generateWorldCoords(checkerCount, settings.value("calibration/squareSize", 10.0).toFloat());
std::vector< std::vector<cv::Point3f> > Q0(qc0.size(), Qi), Q1(qc1.size(), Qi), QStereo(qc0Stereo.size(), 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_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;
std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, cal.cam0_intrinsic_std, cal.cam0_extrinsic_std, cal.cam0_errors_per_view, flags,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, cal.cam1_intrinsic_std, cal.cam1_extrinsic_std, cal.cam1_errors_per_view, flags,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
// 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,
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,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));
#endif
cal.R1 = R1;
cal.T1 = T1;
cal.E = E;
cal.F = F;
// Print to log
std::stringstream out;
out << "## Camera Calibration ##" << std::endl
<< "No. images used for intrinsics of cam0: " << qc0.size() << std::endl
<< "No. images used for intrinsics of cam1: " << qc1.size() << std::endl
<< "No. images used for stereo calibration: " << qc0Stereo.size() << std::endl;
cal.printCamera(out);
out << std::endl << std::endl;
emit logMessage(QString::fromStdString(out.str()));
// save to (reentrant) qsettings object
settings.setValue("calibration/parameters", QVariant::fromValue(cal));
emit done();
}
static void drawDetectedMarkersCharuco(cv::InputOutputArray _image, cv::InputArrayOfArrays _corners,
cv::InputArray _ids, cv::Scalar borderColor) {
using namespace cv;
using namespace std;
CV_Assert(_image.getMat().total() != 0 &&
(_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0);
// calculate colors
Scalar textColor, cornerColor;
textColor = cornerColor = borderColor;
swap(textColor.val[0], textColor.val[1]); // text color just sawp G and R
swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B
int nMarkers = (int)_corners.total();
for(int i = 0; i < nMarkers; i++) {
Mat currentMarker = _corners.getMat(i);
CV_Assert(currentMarker.total() == 4 && currentMarker.type() == CV_32FC2);
// draw marker sides
for(int j = 0; j < 4; j++) {
Point2f p0, p1;
p0 = currentMarker.ptr< Point2f >(0)[j];
p1 = currentMarker.ptr< Point2f >(0)[(j + 1) % 4];
line(_image, p0, p1, borderColor, 2, LINE_AA);
}
// draw first corner mark
rectangle(_image, currentMarker.ptr< Point2f >(0)[0] - Point2f(3, 3),
currentMarker.ptr< Point2f >(0)[0] + Point2f(3, 3), cornerColor, 1, LINE_AA);
// draw ID
if(_ids.total() != 0) {
Point2f cent(0, 0);
for(int p = 0; p < 4; p++)
cent += currentMarker.ptr< Point2f >(0)[p];
cent = cent / 4.;
stringstream s;
s << _ids.getMat().ptr< int >(0)[i];
putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 1, textColor, 2, LINE_AA);
}
}
}
static void drawDetectedCornersCharuco(cv::InputOutputArray _image, cv::InputArray _charucoCorners,
cv::InputArray _charucoIds, cv::Scalar cornerColor) {
using namespace cv;
using namespace std;
CV_Assert(_image.getMat().total() != 0 &&
(_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()) ||
_charucoIds.getMat().total() == 0);
unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();
for(unsigned int i = 0; i < nCorners; i++) {
Point2f corner = _charucoCorners.getMat().at< Point2f >(i);
// draw first corner mark
rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 2, LINE_AA);
// draw ID
if(_charucoIds.total() != 0) {
int id = _charucoIds.getMat().at< int >(i);
stringstream s;
s << id;
putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 1.0,
cornerColor, 4, cv::LINE_AA);
}
}
}
static bool detectCheckerBoardCornersCharuco(const cv::Mat & frame,
const cv::Ptr<cv::aruco::Dictionary> dict,
const cv::Ptr<cv::aruco::CharucoBoard> board,
const cv::Ptr<cv::aruco::DetectorParameters> parameters,
cv::Mat& frameResult,
std::vector<cv::Point2f>& qc,
std::vector<int>& qci){
// Convert to grayscale
cv::Mat gray;
if(frame.channels() == 1)
cv::cvtColor(frame, gray, CV_BayerBG2GRAY);
else
cv::cvtColor(frame, gray, CV_RGB2GRAY);
std::vector<std::vector<cv::Point2f>> mc;
std::vector<int> mi;
cv::Mat grayBlurred;
cv::GaussianBlur(gray, grayBlurred, cv::Size(0, 0), 3);
// Extract checker corners
cv::aruco::detectMarkers(grayBlurred, dict, mc, mi, parameters);
//std::vector<std::vector<cv::Point2f>> mc, rejecti;
//cv::aruco::refineDetectedMarkers(gray0, board, mc0, mi0, rejecti0);
//cv::Mat cci, cidsi;
cv::aruco::interpolateCornersCharuco(mc, mi, gray, board, qc, qci, cv::noArray(), cv::noArray(), 1);
bool success = (qc.size() >= 4);
if(success){
cv::cornerSubPix(gray, qc, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
// Draw colored chessboard
if(frame.channels() == 1)
cv::cvtColor(frame, frameResult, CV_BayerBG2RGB);
else
frameResult = frame.clone();
// Draw colored chessboard
drawDetectedMarkersCharuco(frameResult, mc, mi, cv::Scalar(0, 255, 0));
drawDetectedCornersCharuco(frameResult, qc, qci, cv::Scalar(0, 255, 0));
} else {
qc.clear();
}
return success;
}
void SMCalibrationWorker::checkerboardDetectionCharuco(SMCalibrationSet calibrationSet){
QSettings settings;
cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));
const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();
float markerLength = 0.8f*checkerSize;
// Charuco board and dictionary
cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);
// cv::Mat boardImg;
// board->draw(cv::Size((saddlePointCountX+1)*100, (saddlePointCountY+1)*100), boardImg, 0, 1);
// cv::Mat boardImgRes;
// cv::resize(boardImg, boardImgRes, cv::Size((saddlePointCountX+1)*10, (saddlePointCountY+1)*10), 0, 0, cv::INTER_NEAREST);
// cv::imwrite("boardImg.png", boardImg);
// cv::imwrite("boardImgRes.png", boardImgRes);
cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
// parameters->adaptiveThreshWinSizeMin = 3;
// parameters->adaptiveThreshWinSizeMax = 153;
// parameters->adaptiveThreshWinSizeStep = 5;
// parameters->adaptiveThreshConstant = 20;
// parameters->minMarkerPerimeterRate = 0.01;
parameters->maxMarkerPerimeterRate = 5.0;
// parameters->minCornerDistanceRate = 0.10;
// parameters->perspectiveRemovePixelPerCell = 6;
// parameters->maxErroneousBitsInBorderRate = 0.8;
// parameters->minOtsuStdDev = 1;
// parameters->errorCorrectionRate = 0.1;
parameters->cornerRefinementWinSize = 10;
bool success0 = detectCheckerBoardCornersCharuco(calibrationSet.frame0, dict, board, parameters, calibrationSet.frame0Result, calibrationSet.qc0, calibrationSet.qc0id);
if(!success0){
emit logMessage(QString("Could not detect Charuco corners on set %1 camera0").arg(calibrationSet.id));
std::cerr << "Could not detect Charuco corners on set " << calibrationSet.id << " camera0" << std::endl;
}
bool success1 = detectCheckerBoardCornersCharuco(calibrationSet.frame1, dict, board, parameters, calibrationSet.frame1Result, calibrationSet.qc1, calibrationSet.qc1id);
if(!success1){
emit logMessage(QString("Could not detect Charuco corners on set %1 camera1").arg(calibrationSet.id));
std::cerr << "Could not detect Charuco corners on set " << calibrationSet.id << " camera1" << std::endl;
}
emit newCheckerboardResult(calibrationSet.id, calibrationSet);
}
void SMCalibrationWorker::cameraCalibrationCharuco(std::vector<SMCalibrationSet> calibrationData){
QSettings settings;
// Number of saddle points on calibration pattern
cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));
const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();
float markerLength = 0.8f*checkerSize;
cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);
auto 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;
// 3D object points
std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
// Loop through calibration sets
for(unsigned int i=0; i<nSets; i++){
if(!calibrationData[i].selected)
continue;
bool success0 = !calibrationData[i].qc0.empty();
bool success1 = !calibrationData[i].qc1.empty();
if(success0){
std::vector<cv::Point3f> Q0i;
for(std::size_t j=0; j<calibrationData[i].qc0id.size(); j++){
int id = calibrationData[i].qc0id[j];
Q0i.push_back(board->chessboardCorners[id]);
}
qc0.push_back(calibrationData[i].qc0);
Q0.push_back(Q0i);
}
if(success1){
std::vector<cv::Point3f> Q1i;
for(std::size_t j=0; j<calibrationData[i].qc1id.size(); j++){
int id = calibrationData[i].qc1id[j];
Q1i.push_back(board->chessboardCorners[id]);
}
qc1.push_back(calibrationData[i].qc1);
Q1.push_back(Q1i);
}
if(success0 && success1){
std::vector<cv::Point2f> qc0iStereo, qc1iStereo;
std::vector<cv::Point3f> QiStereo;
int j0 = 0;
int j1 = 0;
while(j0<calibrationData[i].qc0.size() && j1<calibrationData[i].qc1.size()){
int id0 = calibrationData[i].qc0id[j0];
int id1 = calibrationData[i].qc1id[j1];
if(id0 < id1)
j0++;
else if (id1 < id0)
j1++;
else{
assert(id0 == id1);
qc0iStereo.push_back(calibrationData[i].qc0[j0]);
qc1iStereo.push_back(calibrationData[i].qc1[j1]);
QiStereo.push_back(board->chessboardCorners[id0]);
j0++;
j1++;
}
}
if(QiStereo.size() > 0){
qc0Stereo.push_back(qc0iStereo);
qc1Stereo.push_back(qc1iStereo);
QStereo.push_back(QiStereo);
}
}
}
size_t nValidSets = qc0Stereo.size();
if(nValidSets < 2){
std::cerr << "Not enough valid calibration sequences!" << std::endl;
emit done();
return;
}
// 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, cal.cam0_intrinsic_std, cal.cam0_extrinsic_std, cal.cam0_errors_per_view, flags,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, cal.cam1_intrinsic_std, cal.cam1_extrinsic_std, cal.cam1_errors_per_view, flags,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
// 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,
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,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));
#endif
cal.R1 = R1;
cal.T1 = T1;
cal.E = E;
cal.F = F;
// Print to log
std::stringstream out;
out << "## Camera Calibration ##" << std::endl
<< "No. images used for intrinsics of cam0: " << qc0.size() << std::endl
<< "No. images used for intrinsics of cam1: " << qc1.size() << std::endl
<< "No. images used for stereo calibration: " << qc0Stereo.size() << std::endl;
cal.printCamera(out);
out << std::endl << std::endl;
emit logMessage(QString::fromStdString(out.str()));
// save to (reentrant qsettings object)
settings.setValue("calibration/parameters", QVariant::fromValue(cal));
emit done();
}
void SMCalibrationWorker::rotationStageCalibration(std::vector<SMCalibrationSet> calibrationData){
auto nSets = calibrationData.size();
std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
std::vector<cv::Point3f> Qi;
QSettings settings;
// Generate world object coordinates [mm]
const cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(),settings.value("calibration/patternSizeY", 13).toInt()));
const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();
if(settings.value("calibration/method").toString() == "Charuco"){
// Find id's which were detected in all images
std::vector<int> ids;
for(int i=0; i<calibrationData.size(); i++){
if(!calibrationData[i].selected)
continue;
if(ids.empty()){
ids = calibrationData[i].qc0id;
}
std::vector<int> idsi0;
std::set_intersection(ids.begin(), ids.end(), calibrationData[i].qc0id.begin(), calibrationData[i].qc0id.end(), std::back_inserter(idsi0));
ids = idsi0;
std::vector<int> idsi1;
std::set_intersection(ids.begin(), ids.end(), calibrationData[i].qc1id.begin(), calibrationData[i].qc1id.end(), std::back_inserter(idsi1));
ids = idsi1;
}
if(ids.size() < 2){
std::cerr << "Not enough feature points present in all images for rotation axis determination!" << std::endl;
emit logMessage("Not enough feature points present in all images for rotation axis determination! Deselect some calibration images.");
}
for(int i=0; i<calibrationData.size(); i++){
if(!calibrationData[i].selected)
continue;
std::vector<cv::Point2f> qc0StereoI, qc1StereoI;
for(int j=0; j<ids.size(); j++){
ptrdiff_t idx0 = std::find(calibrationData[i].qc0id.begin(), calibrationData[i].qc0id.end(), ids[j]) - calibrationData[i].qc0id.begin();
ptrdiff_t idx1 = std::find(calibrationData[i].qc1id.begin(), calibrationData[i].qc1id.end(), ids[j]) - calibrationData[i].qc1id.begin();
qc0StereoI.push_back(calibrationData[i].qc0[idx0]);
qc1StereoI.push_back(calibrationData[i].qc1[idx1]);
}
qc0Stereo.push_back(qc0StereoI);
qc1Stereo.push_back(qc1StereoI);
}
float markerLength = 0.8f*checkerSize;
cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);
for(int j=0; j<ids.size(); j++){
Qi.push_back(board->chessboardCorners[ids[j]]);
}
} else {
// Normal checkerboard with all corners
for(auto i=0; i<nSets; i++){
if(!calibrationData[i].selected)
continue;
if(!calibrationData[i].qc0.empty() && !calibrationData[i].qc1.empty()){
qc0Stereo.push_back(calibrationData[i].qc0);
qc1Stereo.push_back(calibrationData[i].qc1);
}
}
Qi = generateWorldCoords(checkerCount, checkerSize);
}
SMCalibrationParameters cal = settings.value("calibration/parameters").value<SMCalibrationParameters>();
if(qc0Stereo.size() <= 2){
std::cerr << "Not enough images for rotation axis determination!" << std::endl;
emit logMessage("Not enough images for rotation axis determination!");
cal.Rr = cv::Matx33f::eye();
cal.Tr = cv::Vec3f(0,0,0);
cal.rot_axis_error = -1;
emit done();
return;
}
// 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;
rotationAxisEstimation(Qcam, Qi, axis, point);
rotationAxisOptimization(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;
// Print to log
std::stringstream out;
out << "## Rotation Stage Calibration ##" << std::endl
<< "No. images used for calibration: " << qc0Stereo.size() << std::endl;
cal.printRotationStage(out);
out << std::endl << std::endl;
emit logMessage(QString::fromStdString(out.str()));
// Save to (reentrant) qsettings object
settings.setValue("calibration/parameters", QVariant::fromValue(cal));
emit done();
}