triangulate.py 文件源码

def triangulate(cam1, cam2, p1, p2):
    p1 = np.asarray([p1[0],p1[1],1])
    p2 = np.asarray([p2[0],p2[1],1])
    c1, v1 = cam_center_vector(cam1, p1)
    c2, v2 = cam_center_vector(cam2, p2)
    t = c2 - c1
    v3 = np.cross(v1, v2)
    X = np.stack((v1, v3, -v2), axis=1)

    alpha = np.matmul(la.inv(X), t)
    output = c1 + v1 * alpha[0] + alpha[1]*0.5*v3
    return output