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


#include "SMCalibrationWorker.h"
#include "SMCalibrationParameters.h"
 
#include "cvtools.h"
 
#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)
    int 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();
}
 
 
void SMCalibrationWorker::rotationStageCalibration(std::vector<SMCalibrationSet> calibrationData){
 
    int nSets = calibrationData.size();
 
    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
    for(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() && !calibrationData[i].qc1.empty()){
            qc0Stereo.push_back(calibrationData[i].qc0);
            qc1Stereo.push_back(calibrationData[i].qc1);
        }
    }
 
    QSettings settings;
    SMCalibrationParameters cal = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 
    if(qc0Stereo.size() > 2){
        // 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();
        std::vector<cv::Point3f> Qi = generateWorldCoords(checkerCount, checkerSize);
 
        // 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;
    } else {
        cal.Rr = cv::Matx33f::eye();
        cal.Tr = cv::Vec3f(0,0,0);
        cal.rot_axis_error = -1;
        return;
    }
 
    // 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();
}
 

Subversion Repositories seema-scanner

(root)/src/SMCalibrationWorker.cpp – Rev 236 → 242

Rev 236		Rev 242
1	`#include "SMCalibrationWorker.h"`	1	`#include "SMCalibrationWorker.h"`
2	`#include "SMCalibrationParameters.h"`	2	`#include "SMCalibrationParameters.h"`
3		3
4	`#include "cvtools.h"`	4	`#include "cvtools.h"`
5		5
6	`#include <QSettings>`	6	`#include <QSettings>`
7	`#include <QTextStream>`	7	`#include <QTextStream>`
8		8
9	`#include <ceres/ceres.h>`	9	`#include <ceres/ceres.h>`
10		10
11	`// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points`	11	`// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points`
12	`// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014`	12	`// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014`
13	`// DTU, 2014, Jakob Wilm`	13	`// DTU, 2014, Jakob Wilm`
14	`static void rotationAxisEstimation(const std::vector< std::vector<cv::Point3f> > Qcam,`	14	`static void rotationAxisEstimation(const std::vector< std::vector<cv::Point3f> > Qcam,`
15	`const std::vector<cv::Point3f> Qobj,`	15	`const std::vector<cv::Point3f> Qobj,`
16	`cv::Vec3f &axis, cv::Vec3f &point){`	16	`cv::Vec3f &axis, cv::Vec3f &point){`
17	`assert(Qobj.size() == Qcam[0].size());`	17	`assert(Qobj.size() == Qcam[0].size());`
18		18
19	`// number of frames (points on each arch)`	19	`// number of frames (points on each arch)`
20	`int l = Qcam.size();`	20	`int l = Qcam.size();`
21		21
22	`// number of points in each frame`	22	`// number of points in each frame`
23	`size_t mn = Qobj.size();`	23	`size_t mn = Qobj.size();`
24		24
25	`// construct matrix for axis determination`	25	`// construct matrix for axis determination`
26	`cv::Mat M(6, 6, CV_32F, cv::Scalar(0));`	26	`cv::Mat M(6, 6, CV_32F, cv::Scalar(0));`
27		27
28	`for(int k=0; k<l; k++){`	28	`for(int k=0; k<l; k++){`
29	`for(unsigned int idx=0; idx<mn; idx++){`	29	`for(unsigned int idx=0; idx<mn; idx++){`
30		30
31	`// float i = Qobj[idx].x+4;`	31	`// float i = Qobj[idx].x+4;`
32	`// float j = Qobj[idx].y+4;`	32	`// float j = Qobj[idx].y+4;`
33	`float i = Qobj[idx].x;`	33	`float i = Qobj[idx].x;`
34	`float j = Qobj[idx].y;`	34	`float j = Qobj[idx].y;`
35		35
36	`float x = Qcam[k][idx].x;`	36	`float x = Qcam[k][idx].x;`
37	`float y = Qcam[k][idx].y;`	37	`float y = Qcam[k][idx].y;`
38	`float z = Qcam[k][idx].z;`	38	`float z = Qcam[k][idx].z;`
39		39
40	`M += (cv::Mat_<float>(6,6) << xx, xy, xz, x, ix, j*x,`	40	`M += (cv::Mat_<float>(6,6) << xx, xy, xz, x, ix, j*x,`