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

Question

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

我知道,cv::solvePnP能帮助我,看完这个和这个我明白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++),但这对我来说没有多大意义:

• rvec,来自solvePnP的tvec输出是3x1矩阵,3个元素向量
• cv2.Rodrigues(rvec)是一个3x3矩阵
• cv2.Rodrigues(rvec)[0]是3x1矩阵,3个元素向量
• cameraPosition是一个3x1*1x3矩阵乘法,是一个.. 3x3矩阵.我如何在opengl中使用这个简单glTranslatef和glRotate调用？

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

更新:稍后,要使用TOpenGL,您必须记住,相机框架的轴在OpenCV和OpenGL之间有所不同.

OpenCV使用通常用于计算机视觉的参考:X指向右侧,Y指向下方,Z指向前方(如此图像中所示).OpenGL中相机的框架是:X指向右侧,Y指向上方,Z指向后方(如此图像的左侧).因此,您需要围绕180度的X轴应用旋转.这个旋转矩阵的公式在维基百科中.

// 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以矩阵顺序存储.

Answer 2

如果你想把它变成一个标准的 4x4 姿势矩阵，指定你的相机的位置。使用 rotM 作为左上角的 3x3 正方形，tvec 作为右侧的 3 个元素，0,0,0,1 作为底行

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

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