Subversion Repositories seema-scanner

    // calibrate rotation axis 1

//    // hand-eye calibration

    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame

//    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame

    std::vector<cv::Vec3f> Tc(nValidSets - 1);

//    std::vector<cv::Vec3f> Tc(nValidSets - 1);

    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame

//    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame

    std::vector<cv::Vec3f> Tr(nValidSets - 1);

//    std::vector<cv::Vec3f> Tr(nValidSets - 1);

    for(int i=0; i<nValidSets-1; i++){

//    for(int i=0; i<nValidSets-1; i++){

        // relative transformations in camera

//        // relative transformations in camera

        cv::Mat cRw1, cRw2;

//        cv::Mat cRw1, cRw2;

        cv::Rodrigues(cam_rvecs0[i], cRw1);

//        cv::Rodrigues(cam_rvecs0[i], cRw1);

        cv::Rodrigues(cam_rvecs0[i+1], cRw2);

//        cv::Rodrigues(cam_rvecs0[i+1], cRw2);

        cv::Mat cTw1 = cam_tvecs0[i];

//        cv::Mat cTw1 = cam_tvecs0[i];

        cv::Mat cTw2 = cam_tvecs0[i+1];

//        cv::Mat cTw2 = cam_tvecs0[i+1];

        cv::Mat w1Rc = cRw1.t();

//        cv::Mat w1Rc = cRw1.t();

        cv::Mat w1Tc = -cRw1.t()*cTw1;

//        cv::Mat w1Tc = -cRw1.t()*cTw1;

        Rc[i] = cv::Mat(cRw2*w1Rc);

//        Rc[i] = cv::Mat(cRw2*w1Rc);

        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);

//        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);

        // relative transformations in rotation stage

//        // relative transformations in rotation stage

        // we define the rotation axis to be in origo, pointing in positive y direction

//        // we define the rotation axis to be in origo, pointing in positive y direction

        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;

//        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;

        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);

//        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);

        cv::Mat Rri;

//        cv::Mat Rri;

        cv::Rodrigues(rot_rvec, Rri);

//        cv::Rodrigues(rot_rvec, Rri);

        Rr[i] = Rri;

//        Rr[i] = Rri;

        Tr[i] = 0.0;

//        Tr[i] = 0.0;

//        std::cout << "cTw2" << cTw2 << std::endl;

//    // determine the transformation from rotation stage to camera 0

//        std::cout << "w2Rc" << w2Rc << std::endl;

//    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);

//        std::cout << "w2Rc" << w2Rc << std::endl;

//    for(int i=0; i<nValidSets-1; i++){

//        std::cout << "w2Tc" << w2Tc << std::endl;

//        std::cout << i << std::endl;

//        std::cout << "Rci" << Rci << std::endl;

//        std::cout << "Rc[i]" << Rci << std::endl;

//        std::cout << "rot_rvec" << rot_rvec << std::endl;

//        std::cout << "Rr[i]" << Rri << std::endl;

    for(int i=0; i<nValidSets-1; i++){

//        cv::Rodrigues(Rcr, Rcr);

        std::cout << i << std::endl;

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

//    //* Using rotation axis calibration *//

    // Direct rotation axis calibration //

//    // full camera matrices

    // full camera matrices

//    cv::Matx34f P0 = cal.K0*cv::Matx34f::eye();

    cv::Matx34f P0 = cv::Matx34f::eye();

//    cv::Mat RT1(3, 4, CV_32F);

    cv::Mat RT1(3, 4, CV_32F);

//    cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));

    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::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));

//    cv::Matx34f P1 = cal.K1*cv::Matx34f(RT1);

    cv::Matx34f P1 = cv::Matx34f(RT1);

//    // calibration points in camera 0 frame

    // calibration points in camera 0 frame

//    std::vector< std::vector<cv::Point3f> > Qcam;

    std::vector< std::vector<cv::Point3f> > Qcam;

//    for(int i=0; i<nValidSets; i++){

    for(int i=0; i<nValidSets; i++){

//        std::vector<cv::Point2f> qc0i, qc1i;

        std::vector<cv::Point2f> qc0i, qc1i;

////        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);

        cv::undistortPoints(qc0[i], qc0i, cal.K0, cal.k0);

////        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);

        cv::undistortPoints(qc1[i], qc1i, cal.K1, cal.k1);

//        cv::Mat Qhom, Qcami;

        cv::Mat Qhom, Qcami;

//        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);

        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);

//        cvtools::convertMatFromHomogeneous(Qhom, Qcami);

        cvtools::convertMatFromHomogeneous(Qhom, Qcami);

//        std::vector<cv::Point3f> QcamiPoints;

        std::vector<cv::Point3f> QcamiPoints;

//        cvtools::matToPoints3f(Qcami, QcamiPoints);

        cvtools::matToPoints3f(Qcami, QcamiPoints);

//        Qcam.push_back(QcamiPoints);

        Qcam.push_back(QcamiPoints);

//    }

    }

//    cv::Vec3f axis, point;

    cv::Vec3f axis, point;

//    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);

    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point);

//    // construct transformation matrix

    // construct transformation matrix

//    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,-1.0));

    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));

//    cv::Vec3f ez = ex.cross(axis);

    cv::Vec3f ez = ex.cross(axis);

//    cv::Mat RrMat(3, 3, CV_32F);

    cv::Mat RrMat(3, 3, CV_32F);

//    cv::Mat(ex).copyTo(RrMat.col(0));

    cv::Mat(ex).copyTo(RrMat.col(0));

//    cv::Mat(axis).copyTo(RrMat.col(1));

    cv::Mat(axis).copyTo(RrMat.col(1));

//    cv::Mat(ez).copyTo(RrMat.col(2));

    cv::Mat(ez).copyTo(RrMat.col(2));

//    cal.Rr = cv::Mat(RrMat.t());

    cal.Rr = cv::Matx33f(RrMat).t();

//    cal.Tr = point;

    cal.Tr = -cv::Matx33f(RrMat).t()*point;