使用SolvePnP在openCV中进行旋转和翻译顺序问题

时间:2014-11-22 23:28:51

标签: c++ opencv opengl rotation rotational-matrices

我有一个openGL 3D渲染和一个看着棋盘的相机。 3D渲染在3D渲染棋盘上显示一个小十字准线,由3D渲染相机指向。当现实生活中的相机移动时,十字线和3D相机应该相同地移动。我的翻译工作得很好。但我的旋转始终围绕棋盘的原点旋转,而不是围绕其当前位置旋转。我知道这意味着存在错误的矩阵乘法顺序。但我不知道它在哪里。我所遵循的程序应该尽我所知。

这是我遵循的基本流程:

  • SolvePnP - >得到rvec和tvec
  • rodrigues - > rvec到RMatrix
  • 转置RMatrix(-RMatrix * tvec) - >获得TVector
  • CVpose =

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

  • 转置(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))  ;`

我错过了什么吗?如果需要,我会提供更多信息。

1 个答案:

答案 0 :(得分:0)

我通过以下程序解决了这个问题:

  1. SolvePnP() - &gt;得到rvec和tvec
  2. rodrigues() - &gt; rvec到RMatrix
  3. 转置RMatrix - &gt; RTMatrix
  4. ( - RTMatrix * tvec) - &gt;获得TVector
  5. 创建一个4x4 openGL标识matirx - &gt; (我称之为GLtransform)
  6. GLtransform = glm :: translate() - &gt;由TVector翻译
  7. GLtransform = glm :: rotate() - &gt;由rvec
  8. 旋转
  9. GLtransform = glm :: rotate() - &gt;旋转以修复CV和GL轴之间的兼容性
  10. 绘制GLtransform