How to set Rajawali camera's rotation using translation matrix and rotation matrix from opencv pnpRansac() function?

184 Views Asked by At

I am working on an Android AR app. I'm using RAJAWALI library for AR rendering and openCV for the detection and tracking of targets. I am calculating rotation and translation matrix using opencv's PNPRansac function.

    cv::Mat rotation=cv::Mat::zeros(3, 1, CV_64FC1);
    cv::Mat translation=cv::Mat::zeros(3, 1, CV_64FC1);
    TranslationMatrix=cv::Mat::zeros(3, 1, CV_64FC1);
    RotationMatrix=cv::Mat::zeros(3, 3, CV_64FC1);
    if (!cv::solvePnPRansac(objectPoints_float,
                            scenePoints_float,
                            cameraParams.cameraMatrix,
                            cameraParams.distortionMatrix,
                            rotation,
                            translation,
                            false,
                            iterationsCount,
                            cameraParams.reprojectionError,
                            confidence)) {

        rotation.release();
        translation.release();
        return;
    }

    TranslationMatrix = translation;

    cv::Rodrigues(rotation,
                  RotationMatrix); // converts Rotation Vector to Matrix
    cv::Mat R_t, A;
    R_t = cv::Mat::zeros(3, 3, CV_64FC1);
    A = cv::Mat::zeros(3, 1, CV_64FC1);
    A.at<double>(0, 0) = 0;//[0]
    A.at<double>(1, 0) = 0;//[0]
    A.at<double>(2, 0) = 1;//[1]
    //transpose of rotation matrix
    R_t = RotationMatrix.t();

    // Reference for this formula https://en.wikipedia.org/wiki/Camera_resectioning
    cameraPosition = -1 * (R_t * TranslationMatrix);

    //CameraOrientation = R_t * A;
    //CameraOrientation = rot2euler(RotationMatrix).clone();

I'm trying to pass this rotation and position to Rajawali camera to achieve the AR experience. The position appears to be correct, but the rotation is not being set correctly. I have this in onRender of Rajawali scene:

    // Get camera position and rotation from C++ to Java using native method
    getCameraPositionRotation(cameraPosition.getNativeObjAddr(), cameraOrientation.getNativeObjAddr());

    double POSx = cameraPosition.get(0, 0)[0] / 1000;
    double POSy = cameraPosition.get(1, 0)[0] / 1000;
    double POSz = cameraPosition.get(2, 0)[0] / 1000;
    double ROTx = Math.toDegrees(cameraOrientation.get(0, 0)[0]);
    double ROTy = Math.toDegrees(cameraOrientation.get(1, 0)[0]);
    double ROTz = Math.toDegrees(cameraOrientation.get(2, 0)[0]);
    getCurrentCamera().setPosition(POSx, POSy, POSz);
    getCurrentCamera().setRotation(ROTx,ROTy,ROTz);

How can I correctly set the rotation of Rajawali camera?

0

There are 0 best solutions below