Subversion Repositories seema-scanner

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

void rotationAxisCalibration(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)

    int l = Qcam.size();

    // number of points in each frame

    size_t mn = Qobj.size();

    assert(mn == Qcam[0].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(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;

    // Error estimate (mean 3D point deviations from circles around rotation axis)

    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;

        //std::cout << meanDev << std::endl;

        error += meanDev;

    }

    error /= mn;

}