Untitled

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

def to_base_link(points, camera_loc: str):
    
    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, E_f)
        
    elif camera_loc == 'fr': 
        #pts = uv_to_XYZ(points, P_fr, E_fr @ E_f)
        pts = uv_to_XYZ(points, P_fr, 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, 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, E_f @ E_fl @ E_b)

    return pts