关于c ++：来自cv :: solvePnP的世界坐标中的摄像机位置

Camera position in world coordinate from cv::solvePnP

我有一个校准的相机(固有矩阵和畸变系数)，我想知道相机的位置，知道一些三维点及其在图像中的对应点(二维点)。

我知道cv::solvePnP可以帮助我，在读了这篇和这篇文章之后，我理解solvepnp rvec和tvec的输出是相机坐标系中物体的旋转和平移。

所以我需要找出摄像机在世界坐标系中的旋转/平移。

从上面的链接来看，代码似乎很简单，在python中：

1
2
3

found,rvec,tvec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_coefs)
rotM = cv2.Rodrigues(rvec)[0]
cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)

我不知道Python／NuMPy插件(我使用C++)，但这对我来说没有什么意义。

来自solvepnp的rvec、tvec输出是3x1矩阵，3个元素向量
cv2.rodrigues(rvec)是3x3矩阵
rodrigues(rvec)[0]是3x1矩阵，3个元素向量
cameraposition是一个3x1*1x3矩阵乘法，即a。3x3矩阵。在OpenGL中，如何在简单的glTranslatef和glRotate调用中使用它？

如果"世界坐标"的意思是"对象坐标"，则必须对PNP算法给出的结果进行逆变换。

有一个技巧可以反转转换矩阵，它允许您保存反转操作，这通常很昂贵，这也解释了Python中的代码。给定一个转换[R|t]，我们得到inv([R|t]) = [R'|-R'*t]，其中R'是R的转置。因此，您可以编写代码(未测试)：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

cv::Mat rvec, tvec;
solvePnP(..., rvec, tvec, ...);
// rvec is 3x1, tvec is 3x1

cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3

R = R.t(); // rotation of inverse
tvec = -R * tvec; // translation of inverse

cv::Mat T = cv::Mat::eye(4, 4, R.type()); // T is 4x4
T( cv::Range(0,3), cv::Range(0,3) ) = R * 1; // copies R into T
T( cv::Range(0,3), cv::Range(3,4) ) = tvec * 1; // copies tvec into T

// T is a 4x4 matrix with the pose of the camera in the object frame

更新：稍后，要在OpenGL中使用T，您必须记住，相机帧的轴在OpenCV和OpenGL之间是不同的。

opencv使用计算机视觉中常用的参考：x指向右边，y向下，z指向前面(如图中所示)。OpenGL中相机的帧是：x指向右边，y向上，z指向后面(如此图像的左侧)。所以，你需要绕x轴旋转180度。这个旋转矩阵的公式在维基百科中。

1
2
3

// T is your 4x4 matrix in the OpenCV frame
cv::Mat RotX = ...; // 4x4 matrix with a 180 deg rotation around X
cv::Mat Tgl = T * RotX; // OpenGL camera in the object frame

这些转换总是令人困惑，我可能在某个步骤出错，所以请用一点盐来解决这个问题。

最后，考虑到opencv中的矩阵以行主顺序存储在内存中，而opengl中的矩阵以列主顺序存储。