Having issues using Aruco Markers to render 3D objects

Hey.
I am trying to use Aruco markers to render 3D objects. Basic Marker-based AR stuff.
I have been having no issues detecting the markers and verifying that they were detected by drawing axis using the information emitted by the estimate pose method.
But, once I started implementing the onRender method to use the same information to render 3D objects instead of just axis, it all broke down pretty quickly.
Before I share code, I’ll try to explain the approach I took.

  • Firstly, I realize that estimate pose method emits a rodrigues vector and a translation vector. My plan is to use these two things, alongside the camera matrix, to calculate the model view matrix.
  • I convert the rodrigues vector into a rotation matrix, using Calib3d.Rodrigues
  • I construct a 4x4 view matrix Mat object, and I copy the rotation matrix into the first 3x3 “sub-matrix” (i.e. 4th row and 4th column are untouched)
  • I also copy the tvec into the 4th (last) column of that view Matrix.
  • I fill in the 4th row with 3 zeroes then a 1 (the 1 being in the same column as tvec)
  • I multiply the camera matrix with the view matrix
  • I construct a conversion matrix to convert between OpenCV’s coordinates and OpenGL’s coordinates, since OpenCV’s Y- and Z-Axis are inverted relative to OpenGl.
  • I multiply cvTOgl with the product of cameraMatrix * viewMatrix (stored in viewMatrix), store the result in GlViewMatrix
  • I transpose GlViewMatrix because OpenGL matrices are column major, while openCV matrices are row major.
  • I extract the components corresponding to the rotation
  • I extract the components corresponding to translation
  • I pass them to the .setRotation and .setPosition functions in Rajawali.
    What am I doing wrong? I would appreciate help a lot.

I don’t specifically care for the Rajawali library. If you have examples using just OpenGL or any other library in Java using OpenCV and Aruco Markers I’d love to take a look.
If not, I’d still like some feedback on what I am possibly doing wrong.

    @Override
    protected void onRender(long elapsedRealtime, double deltaTime) {
        super.onRender(elapsedRealtime, deltaTime);
        getCurrentScene().clearChildren();
        List<MarkerData> markerData = markerDetector.getDetectedMarkers();
        for (MarkerData marker : markerData) {
            try {
                int id = R.raw.monkey;
                LoaderOBJ parser = new LoaderOBJ(mContext.getResources(), mTextureManager, id);
                parser.parse();

                Object3D object = parser.getParsedObject();
                object.setMaterial(defaultMaterial);
                object.setScale(0.3);

                Mat rvec = marker.getRvec();
                Mat tvec = marker.getTvec();
                
                Mat rotationMatrix = new Mat(4, 4, CV_64F);
                Mat viewMatrix = new Mat(4, 4, CV_64F);
                Calib3d.Rodrigues(rvec, rotationMatrix);
                for(int row = 0; row < 3; row++)
                {
                    for(int col = 0; col < 3; col++)
                    {
                        viewMatrix.put(row, col, rotationMatrix.get(row, col)[0]);
                    }
                    viewMatrix.put(row, 3, tvec.get(row, 0)[0]);
                }
                viewMatrix.put(3, 0, 0);
                viewMatrix.put(3, 1, 0);
                viewMatrix.put(3, 2, 0);
                viewMatrix.put(3, 3, 1.0f);

                Mat cam4x4mat = new Mat(4, 4, CV_64F);
                for(int i = 0; i < 3; i++)
                {
                    for(int j = 0; j < 3; j++)
                    {
                        cam4x4mat.put(i, j, marker.getCameraMatrix().get(i, j)[0]);
                        cam4x4mat.put(3, j, 0);
                    }
                    cam4x4mat.put(i, 3, 0);
                }
                cam4x4mat.put(3, 3, 1);

                Mat tmpMat = new Mat();
                gemm(cam4x4mat, viewMatrix, 1, new Mat(), 0, tmpMat);
                tmpMat.copyTo(viewMatrix);

                // transfer matrix (openGL and openCV coordinates aren't inherently compatible, need to adjust for that first)
                // OpenCV's y-axis and z-axis are inverted w.r.t OpenGL
                // therefore, the transfer matrix is just the identity matrix, with the y and z components inverted
                Mat cvTOgl = new Mat(4, 4, CV_64F);
                cvTOgl.put(0, 0, 1.0f);
                cvTOgl.put(1, 1, -1.0f); // invert y
                cvTOgl.put(2, 2, -1.0f); // invert z
                cvTOgl.put(3, 3, 1.0f);
                Mat GlViewMatrix = new Mat();
                gemm(cvTOgl.t(), viewMatrix, 1, new Mat(), 0, GlViewMatrix); // cvTOgl * viewMatrix

                // OpenGL matrices are column major. OpenCV matrices are row major. Transpose view matrix.
                GlViewMatrix = GlViewMatrix.t();

                Mat R = new Mat(4, 4, CV_64F);
                Mat T = new Mat(4, 1, CV_64F);
                for(int row = 0; row < 3; row++)
                {
                    for(int col = 0; col < 3; col++)
                    {
                        R.put(row, col, GlViewMatrix.get(row, col)[0]);
                    }
                    T.put(row, 0, GlViewMatrix.get(row, 3)[0]);
                }

                Mat newRvec = new Mat();
                Calib3d.Rodrigues(R, newRvec);
                object.setRotation(new Vector3(newRvec.get(0, 0)[0], newRvec.get(1, 0)[0], newRvec.get(2, 0)[0]));
                object.setPosition(new Vector3(T.get(0, 0)[0], T.get(1,  0)[0], T.get(2, 0)[0]));
                getCurrentScene().addChild(object);
            } catch (ParsingException e) {
                e.printStackTrace();
            }
        }
    }