Проблема порядка вращения и перевода в openCV с SolvePnP

У меня есть 3D рендеринг openGL и камера, смотрящая на шахматную доску. 3D-рендеринг отображает небольшое перекрестие на 3D-рендеринговой шахматной доске, на которую указывает 3D-рендеринг. Когда движется реальная камера, перекрестие и 3D-камера должны двигаться одинаково. Мои переводы работают отлично. Но мои вращения всегда вращаются вокруг начала шахматной доски, а не вокруг ее текущего положения. Я знаю, это означает, что существует неправильный порядок умножения матриц. Но я не знаю, где это. Процедура, которой я следовал, должна работать, насколько я знаю.

Это основной процесс, которому я следую:

• SolvePnP -> получить rvec и tvec
• родригес -> рвец в RMatrix
• транспонировать RMatrix (-RMatrix * tvec) -> чтобы получить TVector
• CVpose =

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

• transpose (CVpose) -> получить CVpose ‘
• присваивать элемент CVpose элемент за элементом GLpose

Мой фактический код для этого раздела:

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))  ;`

Я что-то пропустил? Я дам больше информации, если это необходимо.