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

Posted 2023-02-16

【中文标题】来自 cv::solvePnP 的世界坐标中的相机位置

【英文标题】：Camera position in world coordinate from cv::solvePnP

【发布时间】：2013-09-09 08:33:47

【问题描述】：

我有一个校准过的相机（固有矩阵和失真系数），我想知道相机位置，知道图像中的一些 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 个元素向量 cv2.Rodrigues(rvec) 是一个 3x3 矩阵 cv2.Rodrigues(rvec)[0] 是一个 3x1 矩阵，3 个元素向量 cameraPosition 是一个 3x1 * 1x3 矩阵乘法，它是一个.. 3x3 矩阵。如何通过简单的glTranslatef 和glRotate 调用在opengl 中使用它？

【参考方案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 矩阵则以列优先顺序存储。

【讨论】：

似乎工作，我得到glRotatef的角度，公式取自：euclideanspace.com/maths/geometry/rotations/conversions/…，然后从径向到度数的正常转换。但是，如果我在 opengl 中插入这些值，我仍然会得到错误的相机旋转（旋转 X 大约是 45° 错误）和稍微错误的平移..

这可能是由于OpenCV和OpenGL中的相机帧不同。检查我的扩展答案。

是的，我知道opencv和opengl在内存中矩阵顺序的区别。而且我还必须翻转y和z轴（=>使用opencv y作为opengl z，并使用opencv z作为opengl y）

快到了！结果与几次试验一致。在我看来，X 轴上存在 45° 的误差（我不明白的 opencv 和 opengl 之间的框架可能存在一些差异）和 y 角度上的 10° 误差（我不知道如何解释）

你确定轴是你说的吗？我认为他们就像我的例子一样，但我可能错了。还要检查相机的投影参数在opencv和opengl中是否相同。

【参考方案2】：

如果您想将其转换为指定相机位置的标准 4x4 姿势矩阵。使用 rotM 作为左上角的 3x3 正方形，tvec 作为右边的 3 个元素，0,0,0,1 作为底行

pose = [rotation   tvec(0)
        matrix     tvec(1)
        here       tvec(2)
        0  , 0, 0,  1]

然后反转它（得到相机的姿势而不是世界的姿势）