OpenCV triangulatePoints() 得到负深度，我做错了什么？

Question

所以设置如下所示的坐标系，其中z轴指向屏幕外（朝向您），相机焦距为270像素，图像分辨率为640x480，然后我们在 3D 空间某处有一个物体，两架无人机d1 和d2 在两个不同的视点进行两次观察，其中d1 在(6, 3, 2) 处，该物体对应的图像坐标为(320, 280)，并且d2 的标题是(9.5, 4.5, 3) 和(160, 408)，d1 的标题是-20 度从y 轴和d2 的标题是+30 度从y 轴，目标是确定(x, y, z)对象所在的位置，无人机正在xy平面上空盘旋

Click Here for Image Illustration

给定信息，通过让d1作为参考系，我们可以得到相机内在函数K = [[270, 0, 320], [0, 270, 240], [0, 0, 1]]，变换是旋转+50度，z-axis作为旋转轴，平移@987654345 @，因此是我的代码

import numpy as np
import cv2

def pixel2cam(pt, K):
    u = (pt[0] - K[0][2]) / K[0][0]
    v = (pt[1] - K[1][2]) / K[1][1]
    return np.array([u, v], dtype=np.float32)

def triangulate(points_1, points_2, K, R, t):
    cam_pts_1 = pixel2cam(points_1, K).reshape(2, 1)
    cam_pts_2 = pixel2cam(points_2, K).reshape(2, 1)
    T1 = np.array([[1, 0, 0, 0], 
                   [0, 1, 0, 0], 
                   [0, 0, 1, 0]], dtype=np.float32)
    T2 = np.hstack((R, t))
    X = cv2.triangulatePoints(T1, T2, cam_pts_1, cam_pts_2)
    X /= X[3]
    return X

K = np.array([[270, 0, 320], [0, 270, 240], [0, 0, 1]], dtype=np.float32)
# rotate +50 degrees along z axis
R = np.array([[0.643, -0.766, 0], [0.766, 0.643, 0], [0, 0, 1]], dtype=np.float32)
t = np.array([[3.5], [1.5], [1]], dtype=np.float)

pt_1 = (320, 280)
pt_2 = (160, 408)

X = triangulate(pt_1, pt_2, K, R, t)

这会给你一个齐次坐标X = [[-2.4155867], [ -5.1455526], [-12.032189], [1.]])，其中z 是负数，所以我的问题是

• 我在这里正确表述了R 和t 吗？
• 如果是，那么错误可能是由于此处使用的相机坐标系与OpenCV 中使用的相机坐标系不同？

感谢任何帮助！

Answer 1

我在您的代码中发现了多个问题。我将尝试在下面的不同部分中逐一检查它们。

OpenCV 点三角剖分

首先，OpenCV cv2.triangulatePoints() 采用投影矩阵从世界到像素坐标，以及图像上世界点的像素坐标。请参阅cv2.triangulatePoints()documentation。您还可以在this page 的“详细描述”部分阅读 OpenCV 投影背后的数学。

这是您的 triangulate() 函数的更正版本：

def triangulate(points_1, points_2, K, R, t):
    T1 = np.array([[1, 0, 0, 0], 
                   [0, 1, 0, 0], 
                   [0, 0, 1, 0]], dtype=np.float32)
    T2 = np.hstack((R, t))
    proj1 = np.matmul(K, T1)
    proj2 = np.matmul(K, T2)
    X = cv2.triangulatePoints(proj1, proj2, points_1, points_2)
    X /= X[3]
    return X

旋转矩阵

在 OpenCV 坐标系中，相机的 z 轴是光轴（参见this answer 中的图表）。绕 z 轴旋转与您在问题中链接的图像不匹配，绕 y 轴旋转 -50 度似乎更正确。

您可以使用cv2.Rodrigues() 重新计算您的旋转矩阵。请注意，此函数以弧度表示角度，请参阅documentation。

rotation_vector = np.array([0, -50 / 180 * np.pi, 0])
R, _ = cv2.Rodrigues(rotation_vector)

单位

您说您的焦点是像素单位。查看数据，我非常怀疑焦距和平移值是否一致，至少如果它们以像素表示。我看到了两种可能性：

• 数据正确，但您在编写问题时误解了焦距单位。既然如此，就忘记本节的其余部分了；
• 您的问题中所述的单位是正确的，并且您正在尝试使用不一致的数据对点进行三角测量。

要解决这个问题，请确保焦距和平移都以像素或相同的距离单位表示。您可以通过按相机的像素间距进行从像素到距离单位的转换，并通过乘以像素间距进行从距离单位到像素的转换。像素间距应写在您使用的相机型号的数据表中。

结果

应用上述三个校正给我一个X 具有正 z 坐标的向量。我得到的值并没有多大意义，因为我不确定数据的正确值和单位。