37 lines
1009 B
Python
37 lines
1009 B
Python
import numpy as np
|
|
import cv2 as cv
|
|
import math
|
|
|
|
def rotation_matrix_to_euler_angles(R):
|
|
sy = math.sqrt(R[0,0]**2 + R[1,0]**2)
|
|
singular = sy < 1e-6
|
|
|
|
if not singular:
|
|
x = math.atan2(R[2,1] , R[2,2])
|
|
y = math.atan2(-R[2,0], sy)
|
|
z = math.atan2(R[1,0], R[0,0])
|
|
else:
|
|
# Handle singularity
|
|
x = math.atan2(-R[1,2], R[1,1])
|
|
y = math.atan2(-R[2,0], sy)
|
|
z = 0
|
|
return np.array([x, y, z])
|
|
|
|
def to_board_pose_euler(r_vec, t_vec):
|
|
r_mat = np.ndarray((3, 3))
|
|
cv.Rodrigues(r_vec, r_mat)
|
|
r_mat = np.hstack((r_mat, t_vec))
|
|
r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
|
|
r_mat = np.linalg.inv(r_mat)
|
|
res = rotation_matrix_to_euler_angles(r_mat)
|
|
return res, r_mat[:,3]
|
|
|
|
def cap_init(cap: cv.VideoCapture):
|
|
cap.set(cv.CAP_PROP_SETTINGS, 1)
|
|
cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
|
|
cap.set(cv.CAP_PROP_FPS, 60.0)
|
|
cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280)
|
|
cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024)
|
|
cap.set(cv.CAP_PROP_EXPOSURE, 30)
|
|
cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
|