# Untitled

unknown
python
a year ago
2.2 kB
1
Indexable
Never
```import numpy as np
import extrinsic
import intrinsic

# https://web.cse.ohio-state.edu/~shen.94/781/Site/Slides_files/quaternion.pdf p.11
# 參考上面連結得到 rotation matrix
# 旁邊一欄加上 translation matrix
# 下面一列加上 [0, 0, 0, 1]
def extrinsic_matrix(extrinsic):
x, y, z, qx, qy, qz, qw = extrinsic
rot = np.array([[1.0 - 2 * (qy * qy + qz * qz), 2 * (qx * qy + qw * qz), 2 * (qx * qz - qw * qy)],
[2 * (qx * qy - qw * qz), 1.0 - 2 * (qx * qx + qz * qz), 2 * (qy * qz + qw * qx)],
[2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx), 1.0 - 2 * (qx * qx + qy * qy)]])
trans = np.array([x, y, z])
rot = rot.T
return np.row_stack((np.column_stack((rot,trans)),np.array([0,0,0,1])))

# 2D -> 3D
def uv_to_XYZ(points, P, E):
pts = []
camera_w = np.dot(E, np.array([0,0,0,1])) # 相機中心在世界座標的位置
for u,v in points:
XYZ = np.dot(np.linalg.pinv(P), np.array([u,v,1]))
XYZ_c = XYZ / np.linalg.norm(XYZ)
XYZ_w = np.dot(E, XYZ_c)
t = -1.63 / XYZ_w[2]
pt = t * XYZ_w + camera_w
pts.append(pt[:3])

return pts

P_f = intrinsic.f_projection_matrix
P_fr = intrinsic.fr_projection_matrix
P_fl = intrinsic.fl_projection_matrix
P_b = intrinsic.b_projection_matrix

E_f = extrinsic_matrix(extrinsic.f2bs_extrinsic)
E_fr = extrinsic_matrix(extrinsic.fr2f_extrinsic)
E_fl = extrinsic_matrix(extrinsic.fl2f_extrinsic)
E_b = extrinsic_matrix(extrinsic.b2fl_extrinsic)

if camera_loc == 'f':
pts = uv_to_XYZ(points, P_f, np.linalg.inv(E_f))

elif camera_loc == 'fr':
#pts = uv_to_XYZ(points, P_fr, E_fr @ E_f)
pts = uv_to_XYZ(points, P_fr, np.linalg.inv(E_f @ E_fr))

elif camera_loc == 'fl':
#pts = uv_to_XYZ(points, P_fl, E_fl @ E_f)
pts = uv_to_XYZ(points, P_fl, np.linalg.inv(E_f @ E_fl))

elif camera_loc == 'b':
#pts = uv_to_XYZ(points, P_b, (E_b @ E_fl) @ E_f)
pts = uv_to_XYZ(points, P_b, np.linalg.inv(E_f @ E_fl @ E_b))

return pts
```