refactored

README.md

@@ -15,4 +15,8 @@ https://github.com/opencv/opencv_contrib/tree/4.x/modules/aruco
 
 
 # Rotation
-https://stackoverflow.com/questions/13823296/converting-opencv-rotation-and-translation-vectors-to-xyz-rotation-and-xyz-posit
+https://stackoverflow.com/questions/13823296/converting-opencv-rotation-and-translation-vectors-to-xyz-rotation-and-xyz-posit
+
+
+# Camera pose to object pose
+https://answers.opencv.org/question/64315/solvepnp-object-to-camera-pose/

cam_pose_charuco_board.py

@@ -22,14 +22,14 @@ def rotation_matrix_to_euler_angles(R):
 		z = 0
 	return np.array([x, y, z])
 
-def board_rotation(r_vec, t_vec):
+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[0:3, 0:3])
-	return res
+	res = rotation_matrix_to_euler_angles(r_mat)
+	return res, r_mat[:,3]
 
 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)
@@ -118,8 +118,8 @@ def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoa
 		else:
 			r_vec, t_vec = process(detector, board, img, mtx, dist, None, None)
 			if r_vec is not None and t_vec is not None:
-				res = board_rotation(r_vec, t_vec)
-				rot = [int(180.0 / math.pi * v) for v in res]
+				r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec)
+				rot = [int(180.0 / math.pi * v) for v in r_vec_obj]
 				if rot != last_rotation:
 					print(f"Rotation Tilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}")
 				last_rotation = rot