c++ - 来自 cv::solvePnP 的世界坐标中的相机位置

Question

我有一个校准过的相机(固有矩阵和失真系数)，我想知道相机位置，知道图像中的一些 3d 点及其对应点(2d 点)。

我知道 cv::solvePnP 可以帮助我，并且在阅读 this 之后和 this我了解solvePnP rvec 和tvec 的输出是对象在相机坐标系中的旋转和平移。

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

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

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 个元素 vector
• cv2.Rodrigues(rvec) 是一个 3x3 矩阵
• cv2.Rodrigues(rvec)[0] 是一个 3x1 矩阵，3 个元素 vector
• cameraPosition 是一个 3x1 * 1x3 矩阵乘法，它是一个.. 3x3 矩阵。如何通过简单的 glTranslatef 和 glRotate 调用在 opengl 中使用它？

Answer 1

如果“世界坐标”是指“物体坐标”，则必须得到 pnp 算法给出的结果的逆变换。

有一个求逆转换矩阵的技巧，可以让您节省通常很昂贵的求逆操作，并且可以解释 Python 中的代码。给定一个变换[R|t]，我们有inv([R|t]) = [R'|-R'*t]，其中 R' 是 R 的转置。所以，你可以编码(未测试):

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

更新:稍后，要将 T 与 OpenGL 一起使用，您必须记住，OpenCV 和 OpenGL 之间的相机帧的轴不同。

OpenCV 使用计算机视觉中常用的引用:X 指向右侧，Y 向下，Z 指向前面(如 this image 中)。 OpenGL 中相机的帧是:X 指向右侧，Y 向上，Z 指向后面(如 this image 的左侧)。因此，您需要围绕 X 轴旋转 180 度。这个旋转矩阵的公式在wikipedia .

// 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 的矩阵以列优先顺序存储。