Subversion Repositories seema-scanner

#include "SMCalibrationWorker.h"

#include "SMCalibrationParameters.h"

#include "cvtools.h"

#include <QSettings>

#include <ceres/ceres.h>

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;

};

// 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 rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam,

                                    const std::vector<cv::Point3f> Qobj,

                                    cv::Vec3f &axis, cv::Vec3f &point, float &error){

    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;

    // Non-linear optimization using Ceres

    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(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(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(int k=0; k<l; k++){

            meanDev += std::abs(dI[k] - mean);

        }

        meanDev /= l;

        error += meanDev;

    }

    error /= mn;

}

bool processCBCorners(const cv::Size & checkerCount,

                      const cv::Mat & SMCalibrationSetI_frameX,

                      cv::Mat & SMCalibrationSetI_frameXResult,

                      std::vector<cv::Point2f> & qciX){

    // Convert to grayscale

    cv::Mat gray;

    if(SMCalibrationSetI_frameX.channels() == 1)

        cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_BayerBG2GRAY);

    else

        cv::cvtColor(SMCalibrationSetI_frameX, gray, CV_RGB2GRAY);

    // Extract checker corners

    bool success = cv::findChessboardCorners(gray, checkerCount, qciX, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);

    if(success){

        cv::cornerSubPix(gray, qciX, cv::Size(6, 6), cv::Size(1, 1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));

        // Draw colored chessboard

        if(SMCalibrationSetI_frameX.channels() == 1)

            cv::cvtColor(SMCalibrationSetI_frameX, SMCalibrationSetI_frameXResult, CV_BayerBG2RGB);

        else

            SMCalibrationSetI_frameXResult = SMCalibrationSetI_frameX.clone();

        cvtools::drawChessboardCorners(SMCalibrationSetI_frameXResult, checkerCount, qciX, success, 10);

    }

    return success;

}

std::vector<cv::Point3f> SMCalibrationWorker::generateObjCoordsInWorldCS(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;

}

void SMCalibrationWorker::perViewReprojError(const std::vector<bool> &success0,

                                             const std::vector< std::vector<cv::Point3f> > &Q0,

                                             const std::vector<cv::Mat> &cam_tvecs0,

                                             const std::vector<cv::Mat> &cam_rvecs0,

                                             const std::vector< std::vector<cv::Point2f> > &qc0,

                                             const cv::Matx33f &K0,

                                             const cv::Vec<float, 5> &k0,

                                             std::vector<float> &camX_errors_per_view)

{

    unsigned int nSets = success0.size();

    camX_errors_per_view.resize(nSets);

    int idx = 0;

    //#pragma omp parallel for

    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], K0,  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);

            }

            camX_errors_per_view[i] = (float)err/n;

            idx++;

        } else

            camX_errors_per_view[i] = NAN;

    }

}

void SMCalibrationWorker::performCameraCalibration(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

#define OLD_WAYZ 0

#if OLD_WAYZ

    std::vector< std::vector<cv::Point2f> > qc0, qc1, qc0Stereo, qc1Stereo;

    std::vector<bool> fwdToStageEstimation;

    std::vector<float> angles;

    std::vector<unsigned int> TESTER;

#else

    std::vector< std::vector<cv::Point2f> > qc0(nSets), qc1(nSets), qc0Stereo(nSets), qc1Stereo(nSets);

    std::vector<bool> fwdToStageEstimation(nSets);

    std::vector<float> angles(nSets);

    std::vector<unsigned int> TESTER(nSets);

#endif

    std::vector<bool> success0(nSets, false), success1(nSets, false);

    // Loop through calibration sets

#if !OLD_WAYZ

#pragma omp parallel for

#endif

    for(unsigned int i=0; i<nSets; i++){

        SMCalibrationSet SMCalibrationSetI = calibrationData[i];

        // TODO this changes semantics of the checkboxes in the GUI

        /*if(!SMCalibrationSetI.checked)

            continue;*/

        // Camera 0

        std::vector<cv::Point2f> qci0;

        success0[i] = processCBCorners(checkerCount, SMCalibrationSetI.frame0, SMCalibrationSetI.frame0Result, qci0);

#pragma omp critical (CBCALCupdateUI1)

{

        emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);

}

        // Camera 1

        std::vector<cv::Point2f> qci1;

        success1[i] = processCBCorners(checkerCount, SMCalibrationSetI.frame1, SMCalibrationSetI.frame1Result, qci1);

#pragma omp critical (CBCALCupdateUI2)

{

        emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);

}

        // store results

#if OLD_WAYZ

        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);

            fwdToStageEstimation.push_back(SMCalibrationSetI.checked);

            TESTER.push_back(i);

        }

#else

        qc0[i] = qci0;

        qc1[i] = qci1;

        qc0Stereo[i] = qci0;

        qc1Stereo[i] = qci1;

        angles[i] = SMCalibrationSetI.rotationAngle;

        fwdToStageEstimation[i] = SMCalibrationSetI.checked;

        TESTER[i] = (i);

#endif

        // Show progress

        #pragma omp critical (CBCALCupdateUI3)

        {

            emit newSetProcessed(i);

        }

    }

#if !OLD_WAYZ

    auto s0it = success0.cbegin();

    qc0.erase(std::remove_if( qc0.begin(), qc0.end(), [&s0it](std::vector<cv::Point2f>){return !*s0it++;}),

            qc0.end());

    auto s1it = success1.cbegin();

    qc1.erase(std::remove_if( qc1.begin(), qc1.end(), [&s1it](std::vector<cv::Point2f>){return !*s1it++;}),

            qc1.end());

    s0it = success0.cbegin(); s1it = success1.cbegin();

    qc0Stereo.erase(std::remove_if( qc0Stereo.begin(), qc0Stereo.end(), [&s0it,&s1it](std::vector<cv::Point2f>){return !((*s0it++)&(*s1it++));}),

            qc0Stereo.end());

    s0it = success0.cbegin(); s1it = success1.cbegin();

    qc1Stereo.erase(std::remove_if( qc1Stereo.begin(), qc1Stereo.end(), [&s0it,&s1it](std::vector<cv::Point2f>){return !((*s0it++)&(*s1it++));}),

            qc1Stereo.end());

    s0it = success0.cbegin(); s1it = success1.cbegin();

    angles.erase(std::remove_if( angles.begin(), angles.end(), [&s0it,&s1it](float){return !((*s0it++)&(*s1it++));}),

            angles.end());

    s0it = success0.cbegin(); s1it = success1.cbegin();

    fwdToStageEstimation.erase(std::remove_if( fwdToStageEstimation.begin(), fwdToStageEstimation.end(), [&s0it,&s1it](bool){return !((*s0it++)&(*s1it++));}),

            fwdToStageEstimation.end());

    s0it = success0.cbegin(); s1it = success1.cbegin();

    TESTER.erase(std::remove_if( TESTER.begin(), TESTER.end(), [&s0it,&s1it](bool){return !((*s0it++)&(*s1it++));}),

            TESTER.end());

    /*for(unsigned int i=0; i<nSets; i++){

        if(~success0[i])

            std::remove(qc0.begin(),qc0.end(),i);

        if(~success1[i])

            std::remove(qc1.begin(),qc1.end(),i);

        if(~success0[i] || ~success1[i]){

            std::remove(qc0Stereo.begin(),qc0Stereo.end(),i);

            std::remove(qc1Stereo.begin(),qc1Stereo.end(),i);

            std::remove(angles.begin(),angles.end(),i);

            std::remove(fwdToStageEstimation.begin(),fwdToStageEstimation.end(),i);

        }

    }*/

#endif

    std::copy(TESTER.begin(), TESTER.end(), std::ostream_iterator<unsigned int>(std::cout, " "));

    std::cout << std::endl;

    const size_t nValidStereoSets = angles.size();

    if(nValidStereoSets < 2){

        std::cerr << "Not enough valid calibration sequences!" << std::endl;

        emit done();

        return;

    }

    // Generate world object coordinates [mm]

    std::vector<cv::Point3f> Qi = generateObjCoordsInWorldCS(checkerCount,

                                                             settings.value("calibration/squareSize", 10.0).toFloat());

    std::vector< std::vector<cv::Point3f> >

            Q0(qc0.size(), Qi),

            Q1(qc1.size(), Qi),

            QStereo(nValidStereoSets, 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;

#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;

    // Determine per-view reprojection errors:

    perViewReprojError(success0, Q0, cam_tvecs0, cam_rvecs0, qc0, cal.K0, cal.k0, cal.cam0_errors_per_view);

    perViewReprojError(success1, Q1, cam_tvecs1, cam_rvecs1, qc1, cal.K1, cal.k1, cal.cam1_errors_per_view);

    // This would be so much nicer with range adaptors

    std::vector< std::vector<cv::Point2f> > qc0StereoFwd2Stage;

    std::vector< std::vector<cv::Point2f> > qc1StereoFwd2Stage;

    for(size_t i=0; i<qc0Stereo.size(); i++ )

        if(fwdToStageEstimation[i])

            qc0StereoFwd2Stage.push_back(qc0Stereo[i]);

    for(size_t i=0; i<qc1Stereo.size(); i++ )

        if(fwdToStageEstimation[i])

            qc1StereoFwd2Stage.push_back(qc1Stereo[i]);

    performStageCalibration(qc0StereoFwd2Stage,qc1StereoFwd2Stage,cal);

    // Print to std::cout

    std::cout << std::endl << "========== BEGIN Calibration info ================================================" << std::endl;

    std::cout << "Num. Images used for intrinsics of cam0: " << qc0.size() << std::endl;

    std::cout << "Num. Images used for intrinsics of cam1: " << qc1.size() << std::endl;

    std::cout << "Num. Images used for extrinsocs of cam1: " << nValidStereoSets << std::endl;

    std::cout << "Num. Images used for rotation stage axis estim.: " << qc0StereoFwd2Stage.size() << std::endl << std::endl;

    cal.print();

    std::cout << "========== END   Calibration info ================================================" << std::endl << std::endl;

    // save to (reentrant qsettings object)

    settings.setValue("calibration/parameters", QVariant::fromValue(cal));

    emit done();

}

void SMCalibrationWorker::performStageCalibration(

        const std::vector< std::vector<cv::Point2f> > &qc0Stereo,

        const std::vector< std::vector<cv::Point2f> > &qc1Stereo,

        SMCalibrationParameters& cal){

    assert(qc0Stereo.size()==qc1Stereo.size());

    if(qc0Stereo.size()>1 && qc1Stereo.size()>1){

        // save to (reentrant qsettings object)

        QSettings settings;

        // Generate world object coordinates [mm]

        std::vector<cv::Point3f> Qi = generateObjCoordsInWorldCS(

                    cv::Size(settings.value("calibration/patternSizeX", 22).toInt(),settings.value("calibration/patternSizeY", 13).toInt()),

                    settings.value("calibration/squareSize", 10.0).toFloat());

        // 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;

        rotationAxisCalibration(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;

    }

}