def get_rot_tran_matrix(img_pnts, mod_pnts, cam_matrix):
    (u1, v1) = img_pnts[0]
    (u2, v2) = img_pnts[1]
    (u3, v3) = img_pnts[2]
    (u4, v4) = img_pnts[3]
    (u5, v5) = img_pnts[4]
    (X1, Y1, Z1) = model_points[0]
    (X2, Y2, Z2) = model_points[1]
    (X3, Y3, Z3) = model_points[2]
    (X4, Y4, Z4) = model_points[3]
    (X5, Y5, Z5) = model_points[4]
    fx = cam_matrix[0][0]
    fy = cam_matrix[1][1]
    cx = cam_matrix[0][2]
    cy = cam_matrix[1][2]
    (r31, r32, r33, t3) = (0, 0, 0, 1)
    D = np.array([[X1, Y1, Z1, 1], [X2, Y2, Z2, 1], [X3, Y3, Z3, 1], [X4, Y4, Z4, 1]])
    D1 = np.array([[((v1 - cy) / fy), Y1, Z1, 1], [((v2 - cy) / fy), Y2, Z2, 1], [((v3 - cy) / fy), Y3, Z3, 1], [((v4 - cy) / fy), Y4, Z4, 1]])
    D2 = np.array([[X1, ((v1 - cy) / fy), Z1, 1], [X2, ((v2 - cy) / fy), Z2, 1], [X3, ((v3 - cy) / fy), Z3, 1], [X4, ((v4 - cy) / fy), Z4, 1]])
    D3 = np.array([[X1, Y1, ((v1 - cy) / fy), 1], [X2, Y2, ((v2 - cy) / fy), 1], [X3, Y3, ((v3 - cy) / fy), 1], [X4, Y4, ((v4 - cy) / fy), 1]])
    D4 = np.array([[X1, Y1, Z1, ((v1 - cy) / fy)], [X2, Y2, Z2, ((v2 - cy) / fy)], [X3, Y3, Z3, ((v3 - cy) / fy)], [X4, Y4, Z4, ((v4 - cy) / fy)]])
    r21 = (np.linalg.det(D1) / np.linalg.det(D))
    r22 = (np.linalg.det(D2) / np.linalg.det(D))
    r23 = (np.linalg.det(D3) / np.linalg.det(D))
    t2 = (np.linalg.det(D4) / np.linalg.det(D))
    D1 = np.array([[((u1 - cx) / fx), Y1, Z1, 1], [((u2 - cx) / fx), Y2, Z2, 1], [((u3 - cx) / fx), Y3, Z3, 1], [((u4 - cx) / fx), Y4, Z4, 1]])
    D2 = np.array([[X1, ((u1 - cx) / fx), Z1, 1], [X2, ((u2 - cx) / fx), Z2, 1], [X3, ((u3 - cx) / fx), Z3, 1], [X4, ((u4 - cx) / fx), Z4, 1]])
    D3 = np.array([[X1, Y1, ((u1 - cx) / fx), 1], [X2, Y2, ((u2 - cx) / fx), 1], [X3, Y3, ((u3 - cx) / fx), 1], [X4, Y4, ((u4 - cx) / fx), 1]])
    D4 = np.array([[X1, Y1, Z1, ((u1 - cx) / fx)], [X2, Y2, Z2, ((v2 - cy) / fy)], [X3, Y3, Z3, ((u3 - cx) / fx)], [X4, Y4, Z4, ((u4 - cx) / fx)]])
    r11 = (np.linalg.det(D1) / np.linalg.det(D))
    r12 = (np.linalg.det(D2) / np.linalg.det(D))
    r13 = (np.linalg.det(D3) / np.linalg.det(D))
    t1 = (np.linalg.det(D4) / np.linalg.det(D))
    R = np.array([[r11, r12, r13, t1], [r21, r22, r23, t2], [r31, r32, r33, t3]])
    return R
def get_rot_tran_matrix(img_pnts, mod_pnts, cam_matrix):
    (u1, v1) = img_pnts[0]
    (u2, v2) = img_pnts[1]
    (u3, v3) = img_pnts[2]
    (u4, v4) = img_pnts[3]
    (u5, v5) = img_pnts[4]
    (X1, Y1, Z1) = model_points[0]
    (X2, Y2, Z2) = model_points[1]
    (X3, Y3, Z3) = model_points[2]
    (X4, Y4, Z4) = model_points[3]
    (X5, Y5, Z5) = model_points[4]
    fx = cam_matrix[0][0]
    fy = cam_matrix[1][1]
    cx = cam_matrix[0][2]
    cy = cam_matrix[1][2]
    (r31, r32, r33, t3) = (0, 0, 0, 1)
    D = np.array([[X1, Y1, Z1, 1], [X2, Y2, Z2, 1], [X3, Y3, Z3, 1], [X4, Y4, Z4, 1]])
    D1 = np.array([[((v1 - cy) / fy), Y1, Z1, 1], [((v2 - cy) / fy), Y2, Z2, 1], [((v3 - cy) / fy), Y3, Z3, 1], [((v4 - cy) / fy), Y4, Z4, 1]])
    D2 = np.array([[X1, ((v1 - cy) / fy), Z1, 1], [X2, ((v2 - cy) / fy), Z2, 1], [X3, ((v3 - cy) / fy), Z3, 1], [X4, ((v4 - cy) / fy), Z4, 1]])
    D3 = np.array([[X1, Y1, ((v1 - cy) / fy), 1], [X2, Y2, ((v2 - cy) / fy), 1], [X3, Y3, ((v3 - cy) / fy), 1], [X4, Y4, ((v4 - cy) / fy), 1]])
    D4 = np.array([[X1, Y1, Z1, ((v1 - cy) / fy)], [X2, Y2, Z2, ((v2 - cy) / fy)], [X3, Y3, Z3, ((v3 - cy) / fy)], [X4, Y4, Z4, ((v4 - cy) / fy)]])
    r21 = (np.linalg.det(D1) / np.linalg.det(D))
    r22 = (np.linalg.det(D2) / np.linalg.det(D))
    r23 = (np.linalg.det(D3) / np.linalg.det(D))
    t2 = (np.linalg.det(D4) / np.linalg.det(D))
    D1 = np.array([[((u1 - cx) / fx), Y1, Z1, 1], [((u2 - cx) / fx), Y2, Z2, 1], [((u3 - cx) / fx), Y3, Z3, 1], [((u4 - cx) / fx), Y4, Z4, 1]])
    D2 = np.array([[X1, ((u1 - cx) / fx), Z1, 1], [X2, ((u2 - cx) / fx), Z2, 1], [X3, ((u3 - cx) / fx), Z3, 1], [X4, ((u4 - cx) / fx), Z4, 1]])
    D3 = np.array([[X1, Y1, ((u1 - cx) / fx), 1], [X2, Y2, ((u2 - cx) / fx), 1], [X3, Y3, ((u3 - cx) / fx), 1], [X4, Y4, ((u4 - cx) / fx), 1]])
    D4 = np.array([[X1, Y1, Z1, ((u1 - cx) / fx)], [X2, Y2, Z2, ((v2 - cy) / fy)], [X3, Y3, Z3, ((u3 - cx) / fx)], [X4, Y4, Z4, ((u4 - cx) / fx)]])
    r11 = (np.linalg.det(D1) / np.linalg.det(D))
    r12 = (np.linalg.det(D2) / np.linalg.det(D))
    r13 = (np.linalg.det(D3) / np.linalg.det(D))
    t1 = (np.linalg.det(D4) / np.linalg.det(D))
    R = np.array([[r11, r12, r13, t1], [r21, r22, r23, t2], [r31, r32, r33, t3]])
    return R