- aruco_board working

- refactored

cam_pose_aruco_board.py

@@ -4,15 +4,16 @@ import cv2 as cv
 import argparse
 from aruco_types import ARUCO_DICT
 
-def factory(aruco_type: int):
+def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100.0, marker_sep=10, margins=10):
-	aruco_dict = cv.aruco.getPredefinedDictionary(aruco_type)
+	aruco_dict = cv.aruco.getPredefinedDictionary(dict_type)
 	params = cv.aruco.DetectorParameters()
 	detector = cv.aruco.ArucoDetector(aruco_dict, params)
-
+	board = cv.aruco.GridBoard(board_size, markerLength=marker_length, markerSeparation=marker_sep, dictionary=aruco_dict)
-	board = cv.aruco.GridBoard(size=(5, 7), markerLength=4, markerSeparation=2, dictionary=aruco_dict)
+	im_w = int(board_size[0] * (marker_length + marker_sep) - marker_sep + 2 * margins)
-	image = np.zeros((800,600), np.uint8)
+	im_h = int(board_size[1] * (marker_length + marker_sep) - marker_sep + 2 * margins)
-#	image = board.generateImage((800, 600), None, 0, 1)
+	image = board.generateImage((im_w, im_h), None, margins, 1)
 	cv.imwrite("results/aruco_board.png", image)
+
 	return detector, board
 
 def main():
@@ -32,14 +33,14 @@ def main():
 	# load the ArUCo dictionary, grab the ArUCo parameters, and detect
 	# the markers
 	print("[INFO] detecting '{}' tags...".format(args["type"]))
-	detector, board = factory(ARUCO_DICT[args["type"]])
+	detector, board = factory(ARUCO_DICT[args["type"]], (5, 5))
 
 	# load the input image from disk and resize it
 	print("[INFO] start processing...")
 	process_video(detector, board)
 
 def process_video(detector: cv.aruco.ArucoDetector, board):
-	with np.load('results/cam.npz') as X:
+	with np.load('results/cam_calib_charuco.npz') as X:
 		mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
 
 	# Open camera
@@ -58,22 +59,31 @@ def process_video(detector: cv.aruco.ArucoDetector, board):
 		if k == ord('q'):
 			break
 
-def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.Board, img, mtx, dist):	# Load previously saved data
+def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.GridBoard, img, mtx, dist, rvecs=None, tvecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=True):	# Load previously saved data
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
 	(corners, ids, rejected) = detector.detectMarkers(gray)
 
-	detector.refineDetectedMarkers(img, board, corners, ids, rejected, mtx, dist)
-
 	# Draw a square around the markers
-	cv.aruco.drawDetectedMarkers(img, corners)
+	if draw_marker_box:
+		cv.aruco.drawDetectedMarkers(img, corners)
+
+	# Draw marker id
+	if draw_marker_id:
+		cv.aruco.drawDetectedMarkers(img, corners, ids)
+
+	# Draw marker axis
+	if draw_marker_axis:
+		_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist)
+		for i in range(len(_rvecs)):
+			cv.drawFrameAxes(img, mtx, dist, _rvecs[i], _tvecs[i], 0.05)
 
 	if len(corners) > 0:
 		# Estimate pose of each marker and return the values rvec and tvec---(different from those of camera coefficients)
 		obj_points, img_points = board.matchImagePoints(corners, ids)
-		pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None)
+		pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvecs, tvecs)
 		if pose:
 			# Draw Axis
-			cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 20)
+			cv.drawFrameAxes(img, mtx, dist, rvec, tvec, board.getMarkerLength()*1.5)
 
 
 	cv.imshow('img', img)

cam_pose_charuco_board.py

@@ -144,8 +144,8 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, im
 
 		# Draw marker axis
 		if draw_marker_axis:
-			_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist)
+			_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist)
-			for i in range(len(rvecs)):
+			for i in range(len(_rvecs)):
 				cv.drawFrameAxes(img, mtx, dist, _rvecs[i], _tvecs[i], 0.05)
 
 		if len(charuco_ids) >= len(board.getIds()):

cam_pose_charuco_diamond.py

@@ -96,7 +96,7 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, im
 
 		# Draw marker axis
 		if draw_marker_axis:
-			_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist)
+			_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist)
 			for i in range(len(_rvecs)):
 				cv.drawFrameAxes(img, mtx, dist, _rvecs[i], _tvecs[i], 0.05)