[cam_pose_charuco_board]

- improved capture resolution and exposure
- 'x'-key clears camera calibration and pattern

cam_pose_charuco_board.py

@@ -54,15 +54,25 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 		pass
 
 	# Open camera
-	video = cv.VideoCapture(0)
+	cap = cv.VideoCapture(0)
-	if not video.isOpened():
+
+	if not cap.isOpened():
 		print("Cannot open camera")
 		exit()
 
+	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, 20)
+	cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
+
 	img_size = None
+	last_rotation = [0,0,0]
 	while True:
 		# Read a new frame
-		ok, img = video.read()
+		ok, img = cap.read()
 		if not ok:
 			return
 		k = cv.waitKey(1) & 0xFF
@@ -77,10 +87,19 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 			mtx, dist, rvecs, tvecs = calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, None, None)
 			np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
 		elif k == ord('x'):
+			mtx = None
+			dist = None
 			obj_points_list = []
 			img_points_list = []
 		else:
-			process(detector, board, img, mtx, dist, None, None)
+			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
+
+		cv.imshow('img', img)
 
 def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
 	img_size = None
@@ -103,17 +122,16 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
 
 	return img_size, obj_points, img_points
 
-def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvecs=None, tvecs=None):
+def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvecs=None, tvecs=None, flags=0):
 	if len(obj_points_list) > 0 and len(img_points_list) > 0:
-		flags = 0
-		if mtx is not None:
-			flags = cv.CALIB_USE_INTRINSIC_GUESS
 		ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(obj_points_list, img_points_list, img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs, flags=flags)
 		print(f"Camera calibration finished. Result = {ret}")
 	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
+	r_vec = None
+	t_vec = None
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
 	charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray)
 	if charuco_corners is not None:
@@ -127,15 +145,12 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx,
 		if len(charuco_ids) >= len(board.getIds()):
 			obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
 			if mtx is not None and dist is not None:
-				pose, rvec, tvec = 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, rvec=rvecs, tvec=tvecs)
-				rot = [float(180/math.pi*v) for v in rvec[:,0]]
-				print(f"Rotation X|Y|Z: {rot[0]:.1f} | {rot[1]:.1f} | {rot[2]:.1f}")
 				if pose:
 					# Draw Axis
-					cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10)
+					cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10)
-
-	cv.imshow('img', img)
 
+	return r_vec, t_vec
 
 if __name__ == '__main__':
 	main()