[cam_pose_charuco_board]

- convert cam pose to board rotation

README.md

@@ -12,3 +12,7 @@ https://github.com/opencv/opencv/tree/4.x/samples/cpp/tutorial_code/objectDetect
 
 ## Python
 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

cam_pose_charuco_board.py

@@ -22,6 +22,15 @@ def rotation_matrix_to_euler_angles(R):
 		z = 0
 	return np.array([x, y, z])
 
+def board_rotation(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
+
 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)
@@ -55,7 +64,7 @@ def main():
 	print("[INFO] start processing...")
 	process_video(detector, board)
 
-def process_video(detector: cv.aruco.CharucoDetector, board):
+def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard):
 	mtx = None
 	dist = None
 	obj_points_list = []
@@ -80,12 +89,11 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 	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, 20)
+	cap.set(cv.CAP_PROP_EXPOSURE, 30)
 	cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
 
 	img_size = None
 	last_rotation = [0,0,0]
-	use_euler_angle = True
 	while True:
 		# Read a new frame
 		ok, img = cap.read()
@@ -109,16 +117,11 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 			img_points_list = []
 		else:
 			r_vec, t_vec = process(detector, board, img, mtx, dist, None, None)
-			if r_vec is not None:
+			if r_vec is not None and t_vec is not None:
-				if use_euler_angle:
+				res = board_rotation(r_vec, t_vec)
-					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:
+				if rot != last_rotation:
-					print(f"Rotation X|Y|Z: {rot[0]:.1f} | {rot[1]:.1f} | {rot[2]:.1f}")
+					print(f"Rotation Tilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}")
 				last_rotation = rot
 
 		cv.imshow('img', img)
@@ -151,7 +154,7 @@ def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvec
 	return mtx, dist, rvecs, tvecs
 
 
-def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist, rvecs=None, tvecs=None):	# Load previously saved data
+def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, img, mtx, dist, rvecs=None, tvecs=None):	# Load previously saved data
 	r_vec = None
 	t_vec = None
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
@@ -164,10 +167,17 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx,
 		# Draw a square around the markers
 		cv.aruco.drawDetectedMarkers(img, marker_corners)
 
+		# Draw an axis around the markers
+		rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist)
+		for i in range(len(rvecs)):
+			cv.drawFrameAxes(img, mtx, dist, rvecs[i], tvecs[i],0.05)
+
 		if len(charuco_ids) >= len(board.getIds()):
 			obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
+			offset = int(board.getMarkerLength() * board.getSquareLength())
+			obj_points -= [offset, offset, 0]
 			if mtx is not None and dist is not None:
-				pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, rvec=rvecs, tvec=tvecs)
+				pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, useExtrinsicGuess=False, rvec=rvecs, tvec=tvecs, flags=cv.SOLVEPNP_ITERATIVE)
 				if pose:
 					# Draw Axis
 					cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10)