Python 使用多个摄影机进行位置跟踪，将二维位置转换为单个三维点

首先，我对OpenCV知之甚少

我头顶上有一个红外信号灯。使用一些网络摄像头，我能够获得每个摄像头的（x，y）信标位置。如果我把这些摄像头放在我的房间周围，我希望能够在我房间的真实3D空间中跟踪我的（x，y，z）位置

我正在尝试复制此视频：。这段视频是开源的，所以我看了他的代码并试图修改它以适合我。但是，我发现这些函数返回的值没有多大意义（比如所有函数都返回0，或者是负值，有时只是错误，我不明白为什么）

据我所知，我需要先校准软件，然后才能获得我的位置。校准包括将信标精确移动1米，并告诉软件将这些点保存1米，然后对更多的点和位置执行相同的操作。我已经有了相机的失真/系数矩阵

我正在使用python和opencv，到目前为止，我已经尝试使用solvepnp、triangulatePoints、perspectiveTransform，但我不了解矩阵数学，不知道这些函数在做什么

def calculate3DPosition():

    world3DPoints1 = np.array([[[0],[0],[70]],[[100],[0],[70]],[[200],[0],[70]]], dtype=np.float32)
    world3DPoints2 = np.array([[[0],[0],[70]],[[100],[0],[70]],[[200],[0],[70]]], dtype=np.float32)

    Point2D1 = np.array([[[168], [335]], [[315], [240]], [[430], [166]]], dtype=np.float32)
    Point2D2 = np.array([[[400], [288]], [[300], [187]], [[225], [125]]], dtype=np.float32)

    projection_matrix1, position1 = calculateProjectionMatrix(Point2D1, world3DPoints1)
    projection_matrix2, position2 = calculateProjectionMatrix(Point2D2, world3DPoints2)

    Point2D1 = np.array([[[168],[315],[430]],[[400],[300],[225]]], dtype=np.float32)
    Point2D2 = np.array([[[400],[300],[225]],[[288],[187],[125]]], dtype=np.float32)

    triangulationOutput = cv2.triangulatePoints(np.eye(4)[:3], projection_matrix2, Point2D1, Point2D2)

    mypoint1 = np.array([triangulationOutput[0], triangulationOutput[1], triangulationOutput[2]])
    mypoint1 = mypoint1.reshape(-1, 3)
    mypoint1 = np.array([mypoint1])
    P_24x4 = np.resize(projection_matrix1, (4,4))
    P_24x4[3,0] = 0
    P_24x4[3,1] = 0
    P_24x4[3,2] = 0
    P_24x4[3,3] = 1

    projected = cv2.perspectiveTransform(mypoint1, P_24x4)
    output = triangulationOutput[:-1]/triangulationOutput[-1]
    return output

def calculateProjectionMatrix(listPoint2D, listPoint3D):
    camera_matrix = np.float64([[247.9139798, 0., 155.30251177], [0., 248.19822494, 100.74688813], [0.,  0., 1.]])
    distortion_coefficient = np.float64([-0.45977769,  0.29782977, -0.00162724,  0.00046035, -0.16351777])
    projection_matrix = np.array([], dtype=np.float32).reshape(0,3,4)

    ret, rvec, tvec = cv2.solvePnP(listPoint3D, listPoint2D, camera_matrix, distortion_coefficient)
    rotM_cam = cv2.Rodrigues(rvec)[0]

    cameraPosition = -np.matrix(rotM_cam).T * np.matrix(tvec)

    camMatrix = np.append(cv2.Rodrigues(rvec)[0], tvec, 1)
    projectionMatrix = np.dot(camera_matrix, camMatrix)

    return projectionMatrix, cameraPosition

print(calculate3DPosition())

---

因为我在使用他的代码，我不知道它在做什么，但我知道它对他有用。如果您能为我指出正确的方向，我们将不胜感激。

相机的固有值和失真参数适用于我的相机。这些点列表还表示我的实际点和实际像素值。您可能需要读取和。