refactored

legacy/utils.py

@@ -0,0 +1,36 @@
+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)