Subversion Repositories seema-scanner

// Function 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){

    // number of frames (points on each arch)

    int l = Qcam.size();

    // number of points in each frame

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

            float i = Qobj[idx].x+4;

            float j = Qobj[idx].y+4;

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

        }

    }

//std::cout << M << std::endl;

    //M *= 1.0/(l*mn);

    cv::completeSymm(M, false);

//std::cout << M << std::endl;

    // solve for axis

    std::vector<float> lambda;

    cv::Mat u;

    cv::eigen(M, lambda, u);

    float minLambda = abs(lambda[0]);

    int idx = 0;

    for(int i=1; i<lambda.size(); i++){

        if(abs(lambda[i]) < minLambda){

            minLambda = lambda[i];

            idx = i;

        }

    }

std::cout << "u:" <<  u << std::endl;

    axis = u.row(idx).colRange(0, 3);

    axis /= cv::norm(axis);

    float nx = u.at<float>(5, 0);

    float ny = u.at<float>(5, 1);

    float nz = u.at<float>(5, 2);

    float d  = u.at<float>(5, 3);

    float dh = u.at<float>(5, 4);

    float dv = u.at<float>(5, 5);

    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));

    cv::Mat b(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;

            b.at<float>(row, 0) = f + (2*z*d)/nz;

        }

    }

    // solve for point

    cv::Mat abe;

    cv::solve(A, b, abe, cv::DECOMP_SVD);

    point[0] = abe.at<float>(0, 0);

    point[1] = abe.at<float>(1, 0);

    point[2] = abe.at<float>(2, 0);

}