OpenCV triangulatePoints（）获得负深度，我做错了什么？

Question

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

Click Here for Image Illustration

给定这些信息，通过让d1作为参考帧，我们可以得到相机的内部属性K = [[270, 0, 320], [0, 270, 240], [0, 0, 1]]，转换是以z轴作为旋转轴旋转+50度，以及平移t = [3.5, 1.5, 1]，因此我的代码

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点三角剖分

首先，OpenCVcv2.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)

单位

你说你的焦距是以像素为单位的。看看数据，我高度怀疑焦距和平移值是否一致，至少如果它们以像素表示的话。我认为有两种可能性：

• 数据是正确的，但您在写问题时误解了焦距单位。在这种情况下，忘记了本节的其余部分
• 您问题中所述的单位是正确的，您正在尝试使用不一致的数据对点进行三角测量

要解决此问题，请确保焦距和平移均以像素或相同的距离单位表示。您可以通过俯冲相机的像素间距来完成从像素到距离单位的转换，并通过乘以像素间距来完成从距离单位到像素的转换。像素间距应写入所使用相机型号的数据表中

结果

应用上面描述的三个修正，我得到了一个z坐标为正的X向量。不过，我获得的值并不重要，因为我不确定数据的正确值和单位