Rotation and Translation order issue in openCV with SolvePnP

Question

I have an openGL 3D rendering and a camera looking at a chessboard. The 3D rendering displays a small cross-hair on a 3D rendered chessboard, pointed at by a 3D rendered camera. When the real life camera moves, the cross-hair and 3D camera should move identically. My translations work perfectly. But my rotations always rotate around the origin of the chessboard, instead of around its current position. I know this means there is a matrix multiplication order that is wrong. But I don't know where it is. The procedure I have followed should work as far as I know.

This is the basic process I follow:

• SolvePnP -> to get rvec and tvec
• rodrigues -> rvec to RMatrix
• transpose RMatrix (-RMatrix * tvec) -> to get TVector
• CVpose=

[RMatrix TVector(0) ... TVector(1) ... TVector(2) 0, 0, 0, 1 ]

• transpose(CVpose) -> to get CVpose'
• assign CVpose' element by element to GLpose

My actual code for this section:

solvePnP(Mat(boardPoints), Mat(imagePoints),intrinsics, distortion,rvec, tvec, false);
//calculate camera pose
Rodrigues(rvec, rotM); // rotM is camera rotation matrix
RTMat = rotM.t();

//create full transform matrix
for(int row = 0; row < 3; row++){
    for(int col = 0; col < 3; col++){
        CVpose.at<double>(row,col) = RTMat.at<double>(row,col);
    }
    CVpose.at<double>(row, 3) = (tvec.at<double>(row, 0));
}
CVpose.at<double>(3, 3) = 1.0f;

// invert axes for openGL
cvToGl.at<double>(0, 0) = 1.0f; // Invert the x axis
cvToGl.at<double>(1, 1) = -1.0f; // Invert the y axis
cvToGl.at<double>(2, 2) = 1.0f; // invert the z axis
cvToGl.at<double>(3, 3) = 1.0f;

CVpose =  cvToGl * CVpose;

//assign CVpose to GLpose with col major and row major notation taken into account
for(int row = 0; row < 4; row++){
    for(int col = 0; col < 4; col++){
        GLpose[col][row] = GLpose.at<double>(row,col);
    }
}

//rotate camera to face chessboard in GL
GLpose= glm::rotate(GLpose, 180.0f, vec3(1.0f, 0.0f, 0.0f))  ;`

Have I missed something? I will give more info if needed.

Answer 1

I got around this issue with the following procedure:

1. SolvePnP() -> to get rvec and tvec
2. rodrigues() -> rvec to RMatrix
3. transpose RMatrix -> RTMatrix
4. (-RTMatrix * tvec) -> to get TVector
5. create a 4x4 openGL identity matirx -> (I called it GLtransform)
6. GLtransform = glm::translate() -> translate by TVector
7. GLtransform = glm::rotate() -> rotate by rvec
8. GLtransform = glm::rotate() -> rotate to fix compatibility between CV and GL axis
9. Draw GLtransform