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

我有一个校准过的相机（固有矩阵和畸变系数），我想知道相机的位置，知道一些 3d 点及其在图像中的对应点（2d 点）。

我知道cv::solvePnP可以帮助我，阅读后this and this我知道我的输出solvePnPrvec and 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 个元素向量
• cv2.Rodrigues(rvec) 是一个 3x3 矩阵
• cv2.Rodrigues(rvec)[0] 是一个 3x1 矩阵，3 个元素向量
• cameraPosition 是一个 3x1 * 1x3 矩阵乘法，即 3x3 矩阵。我怎样才能在opengl中使用这个简单的glTranslatef and glRotate calls?

如果“世界坐标”指的是“对象坐标”，则必须对 pnp 算法给出的结果进行逆变换。

有一个反转变换矩阵的技巧，可以让您节省反转操作，这通常是昂贵的，并且解释了 Python 中的代码。给定一个变换[R|t]，我们有那个inv([R|t]) = [R'|-R'*t], where 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

Update:以后要用到T对于 OpenGL，您必须记住 OpenCV 和 OpenGL 之间相机框架的轴不同。

OpenCV 使用计算机视觉中常用的参考：X 指向右侧，Y 指向下方，Z 指向前方（如这个图片）。 OpenGL 中相机的坐标系是：X 指向右侧，Y 指向上方，Z 指向后方（如左手边所示）这个图片）。因此，您需要绕 X 轴旋转 180 度。该旋转矩阵的公式为维基百科.

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