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
评论列表
文章目录