Python 如何从两幅图像点对应关系中估计3D点？

我试图在Python中使用OpenCV从运动管道实现一个基本结构，从两幅图像中给定两幅图像中的对应点生成一个点云。我设法从中获得了3D点，但位置没有真正意义

import sys, cv2, numpy as np
from numpy import *

def valid_cameras(inliers1, inliers2, rot, trans):
    # check if the point correspondences are in front of both images
    rot_inv = rot
    for first, second in zip(inliers1, inliers2):
        first_z = np.dot(rot[0, :] - second[0]*rot[2, :], trans) / np.dot(rot[0, :] - second[0]*rot[2, :], second)
        first_3d_point = np.array([first[0] * first_z, second[0] * first_z, first_z])
        second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T, trans)

        if first_3d_point[2] < 0 or second_3d_point[2] < 0:
            return False

    return True

# points1 point2 are corresponding pixel in the two images that match computer with SIFT and RANSAC
def point_cloud(points1, points2):
    F, inliers = cv2.findFundamentalMat(points2, points1, cv2.RANSAC)
    mask = np.where( inliers.flatten() )


    for x1, x2 in zip(np.int32(points1[mask]), np.int32(points2[mask])):
        x1 = x1.tolist()
        x2 = x2.tolist()

        x1.append(1)
        x2.append(1)

        x1 = np.array(x1)
        x2 = np.array(x2)

        #print x1.T.dot(F.dot(x2)) should be approxiatemly 0.0

    # FROM HERE
    # Is this correct for iPhone 6?
    focal  = 4.89 # mm EFL
    x, y   = 2448, 3264
    sx, sy = 24, 36
    fx, fy = focal * x / sx, focal * y / sy

    K  = np.array([
      [fx, 0.0, 0.0],
      [0.0, fy, 0.0],
      [0.0, 0.0, 1.0,],
    ])

    K_inv = np.linalg.inv(K)

    # Decompose into the essential matrix
    E = K.T.dot(F).dot(K)

    # Decompose essential matrix into R, t (See Hartley and Zisserman 9.13)
    U, S, Vt = np.linalg.svd(E)
    W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
    # print .flatten()
    mask = mask[0].tolist()

    inliers1, inliers2 = [], []
    for i in range( len(mask) ):
        if mask[i]:
            # normalize and homogenize the image coordinates
            inliers1.append(K_inv.dot([points1[i][0], points1[i][1], 1.0]))
            inliers2.append(K_inv.dot([points2[i][0], points2[i][1], 1.0]))

    # Determine the correct choice of second camera matrix
    # only in one of the four configurations will all the points be in front of both cameras
    # First choice: R = U * Wt * Vt, T = +u_3 (See Hartley Zisserman 9.19)
    R = U.dot(W).dot(Vt)
    T = U[:, 2]

    if not valid_cameras(inliers1, inliers2, R, T):
        # Second choice: R = U * W * Vt, T = -u_3
        T = - U[:, 2]
        if not valid_cameras(inliers1, inliers2, R, T):
            # Third choice: R = U * Wt * Vt, T = u_3
            R = U.dot(W.T).dot(Vt)
            T = U[:, 2]
            if not valid_cameras(inliers1, inliers2, R, T):
                # Fourth choice: R = U * Wt * Vt, T = -u_3
                T = - U[:, 2]

    T = T.reshape(1, 3)

    P1 = np.mat('1 0 0 0 ; 0 1 0 0 ; 0 0 1 0')
    P2 = np.bmat([[R, T.T]])

    points1 = np.hstack((points1, np.ones((points1.shape[0], 1))))
    points2 = np.hstack((points2, np.ones((points2.shape[0], 1))))

    X = cv2.triangulatePoints(P1[:3], P2[:3], points1.T[:2], points2.T[:2])

    # Remember to divide out the 4th row. Make it homogeneous
    X /= X[3]

    # Recover the origin arrays from PX
    x1 = dot(P1[:3],X)
    x2 = dot(P2[:3],X)

    # Again, put in homogeneous form before using them
    x1 /= x1[2]
    x2 /= x2[2]

    print X # 3d points

将系统、cv2、numpy作为np导入
从numpy进口*
def有效_摄像头（入口1、入口2、rot、trans）：
#检查点对应是否位于两幅图像的前面
rot\u inv=rot
对于zip中的第一个，第二个（inliers1，inliers2）：
第一个点（rot[0，：]-second[0]*rot[2，：]，trans）/np.dot（rot[0，：]-second[0]*rot[2，：]，second）
first_3d_point=np.数组（[first[0]*first_z，second[0]*first_z，first_z]）
第二个点=np点（旋转T，第一个点）-np点（旋转T，平移）
如果第一个\u 3d \u点[2]<0或第二个\u 3d \u点[2]<0：
返回错误
返回真值
#点1点2是两幅图像中的对应像素，这两幅图像将计算机与SIFT和RANSAC匹配
def点云（点1、点2）：
F、 inliers=cv2.findFundamentalMat（点S2、点S1、cv2.RANSAC）
mask=np.where（inliers.flatte（））
对于zip中的x1、x2（np.int32（points1[mask]）、np.int32（points2[mask]）：
x1=x1.tolist（）
x2=x2.tolist（）
x1.附加（1）
x2.追加（1）
x1=np.数组（x1）
x2=np.数组（x2）
#打印x1.T点（F点（x2））应大约为0.0
#从这里
#这对iphone6正确吗？
焦距=4.89毫米EFL
x、 y=24483264
sx，sy=24,36
fx，fy=focal*x/sx，focal*y/sy
K=np.array([
[fx，0.0，0.0]，
[0.0，fy，0.0]，
[0.0, 0.0, 1.0,],
])
K_inv=np.linalg.inv（K）
#分解为本质矩阵
E=K.T.dot（F）。dot（K）
#将基本矩阵分解为R，t（见Hartley和Zisserman 9.13）
U、 S，Vt=np.linalg.svd（E）
W=np.数组（[0.0，-1.0,0.0,1.0,0.0,0.0,0.0,1.0]）。重塑（3,3）
#print.flatte（）
掩码=掩码[0]。tolist（）
inliers1，inliers2=[]，[]
对于范围内的i（len（mask））：
如果面具[i]：
#标准化和均匀化图像坐标
inliers1.append（K_inv.dot（[points1[i][0]，points1[i][1]，1.0]））
inliers2.append（K_inv.dot（[points2[i][0]，points2[i][1]，1.0]））
#确定第二个摄影机矩阵的正确选择
#只有在四种配置中的一种配置中，所有点都将位于两个摄像头的前面
#第一选择：R=U*Wt*Vt，T=+U_3（见Hartley-Zisserman 9.19）
R=U.dot（W）.dot（Vt）
T=U[：，2]
如果无效\u摄像机（输入框1、输入框2、R、T）：
#第二选择：R=U*W*Vt，T=-U_3
T=-U[：，2]
如果无效\u摄像机（输入框1、输入框2、R、T）：
#第三种选择：R=U*Wt*Vt，T=U_3
R=U.dot（W.T.），dot（Vt）
T=U[：，2]
如果无效\u摄像机（输入框1、输入框2、R、T）：
#第四个选项：R=U*Wt*Vt，T=-U_3
T=-U[：，2]
T=T.重塑（1，3）
P1=np.mat（'1 0 0；0 1 0 0；0 0 1 0'）
P2=np.bmat（[[R，T.T]]）
点s1=np.hstack（（点s1，np.one（（点s1.shape[0]，1）））
points2=np.hstack（（points2，np.one（（points2.shape[0]，1）））
X=cv2.三角点（P1[：3]，P2[：3]，点1.T[：2]，点2.T[：2]）
#记住把第四排分开。使其均匀
X/=X[3]
#从PX恢复源阵列
x1=点（P1[：3]，X）
x2=点（P2[：3]，X）
#同样，在使用它们之前，请将其放在同质形式中
x1/=x1[2]
x2/=x2[2]
打印X#3d点

---

我担心评论中的逻辑是否正确？特别是iPhone 6的摄像头矩阵是否正确，以及从2d对应关系计算3d点的逻辑是否正确？

您假设找到的入口是完美的，然后如果任何点

z0

点，则拒绝摄像头配置。