Revision history [back]

I am using aruco markers and solvePnp to return A camera pose. I run PnP, then I use the following function to get the camera pose as a quaternion rotation from the rvec and tvec:

void ArucoFunctions::GetCameraPoseEigen(cv::Vec3d tvecV, cv::Vec3d rvecV, Eigen::Vector3d &Translate, Eigen::Quaterniond &quats)
{
    Mat R;
    Mat tvec, rvec;

    tvec = DoubleMatFromVec3b(tvecV);
    rvec = DoubleMatFromVec3b(rvecV);

    cv::Rodrigues(rvec, R); // R is 3x3
    R = R.t();                 // rotation of inverse
    tvec = -R*tvec;           // translation of inverse

    Eigen::Matrix3d mat;

    mat(0, 0) = R.at<double>(0, 0);
    mat(0, 1) = R.at<double>(0, 1);
    mat(0, 2) = R.at<double>(0, 2);
    mat(1, 0) = R.at<double>(1, 0);
    mat(1, 1) = R.at<double>(1, 1);
    mat(1, 2) = R.at<double>(1, 2);
    mat(2, 0) = R.at<double>(2, 0);
    mat(2, 1) = R.at<double>(2, 1);
    mat(2, 2) = R.at<double>(2, 2);


    Eigen::Quaterniond EigenQuat(mat);

    quats = EigenQuat;


    double x_t = tvec.at<double>(0, 0);
    double y_t = tvec.at<double>(1, 0);
    double z_t = tvec.at<double>(2, 0);

    Translate.x() = x_t * 10;
    Translate.y() = y_t * 10;
    Translate.z() = z_t * 10;


}

This works, but i am still seeing huge flipping / spasming in the rotation values at some angles.

I am running the DrawAxis to check the pose in my camera frame, and this looks solid, yet my converted values do not. Where am i going wrong? Is there a mistake in the above function?

I am using aruco markers and solvePnp to return A camera pose. I run PnP, then I use the following function to get the camera pose as a quaternion rotation from the rvec and tvec:

void ArucoFunctions::GetCameraPoseEigen(cv::Vec3d GetCameraPoseEigen(cv::Vec3d tvecV, cv::Vec3d rvecV, Eigen::Vector3d &Translate, Eigen::Quaterniond &quats)
{
    Mat R;
    Mat tvec, rvec;

    tvec = DoubleMatFromVec3b(tvecV);
    rvec = DoubleMatFromVec3b(rvecV);

    cv::Rodrigues(rvec, R); // R is 3x3
    R = R.t();                 // rotation of inverse
    tvec = -R*tvec;           // translation of inverse

    Eigen::Matrix3d mat;

    mat(0, 0) = R.at<double>(0, 0);
    mat(0, 1) = R.at<double>(0, 1);
    mat(0, 2) = R.at<double>(0, 2);
    mat(1, 0) = R.at<double>(1, 0);
    mat(1, 1) = R.at<double>(1, 1);
    mat(1, 2) = R.at<double>(1, 2);
    mat(2, 0) = R.at<double>(2, 0);
    mat(2, 1) = R.at<double>(2, 1);
    mat(2, 2) = R.at<double>(2, 2);

    cv2eigen(R, mat);

    Eigen::Quaterniond EigenQuat(mat);

    quats = EigenQuat;


    double x_t = tvec.at<double>(0, 0);
    double y_t = tvec.at<double>(1, 0);
    double z_t = tvec.at<double>(2, 0);

    Translate.x() = x_t * 10;
    Translate.y() = y_t * 10;
    Translate.z() = z_t * 10;
10;   

}

This works, but i am still seeing huge flipping / spasming in the rotation values at some angles. My pose flips 180 degrees, and then back.

I am running the DrawAxis to check the pose in my camera frame, and this looks solid, yet my converted values do not. Where am i going wrong? Is there a mistake in the above function?