[cam_pose_charuco_board]

- some experiements with board angle calculation
- added rotMat to Euler

cam_pose_charuco_board.py

@@ -7,6 +7,21 @@ import math
 
 CAM_CALIB_FILE = "results/cam_calib_charuco.npz"
 
+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 factory(dict_type: int, board_size: cv.typing.Size, square_length: float=5.0, marker_length: float=3.0):
 	aruco_dict = cv.aruco.getPredefinedDictionary(dict_type)
 	board = cv.aruco.CharucoBoard(board_size, square_length, marker_length, aruco_dict)
@@ -70,6 +85,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 
 	img_size = None
 	last_rotation = [0,0,0]
+	use_euler_angle = True
 	while True:
 		# Read a new frame
 		ok, img = cap.read()
@@ -94,10 +110,16 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 		else:
 			r_vec, t_vec = process(detector, board, img, mtx, dist, None, None)
 			if r_vec is not None:
-				rotation = [int(180.0 / math.pi * v) for v in r_vec[:, 0]]
-				if last_rotation != rotation:
-					print(f"Rotation X|Y|Z: {rotation[0]:.1f} | {rotation[1]:.1f} | {rotation[2]:.1f}")
-				last_rotation = rotation
+				if use_euler_angle:
+					r_mat = np.ndarray((3,3))
+					cv.Rodrigues(r_vec, r_mat)
+					res = rotation_matrix_to_euler_angles(r_mat)
+				else:
+					res = r_vec
+				rot = [int(180.0 / math.pi * v) for v in res]
+				if rot != last_rotation or True:
+					print(f"Rotation X|Y|Z: {rot[0]:.1f} | {rot[1]:.1f} | {rot[2]:.1f}")
+				last_rotation = rot
 
 		cv.imshow('img', img)