- aruco board: use solvePnP

- refactored

cam_pose_aruco_board.py

@@ -58,7 +58,7 @@ def process_video(detector: cv.aruco.ArucoDetector, board):
 		if k == ord('q'):
 			break
 
-def process(detector: cv.aruco.ArucoDetector, board, img, mtx, dist):	# Load previously saved data
+def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.Board, img, mtx, dist):	# Load previously saved data
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
 	(corners, ids, rejected) = detector.detectMarkers(gray)
 
@@ -69,11 +69,11 @@ def process(detector: cv.aruco.ArucoDetector, board, img, mtx, dist):	# Load pre
 
 	if len(corners) > 0:
 		# Estimate pose of each marker and return the values rvec and tvec---(different from those of camera coefficients)
-		pose, rvec, tvec = cv.aruco.estimatePoseBoard(corners, ids, board, mtx, dist, None, None)
-
+		obj_points, img_points = board.matchImagePoints(corners, ids)
+		pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None)
 		if pose:
 			# Draw Axis
-			cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 0.02)
+			cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 20)
 
 
 	cv.imshow('img', img)

cam_pose_charuco_board.py

@@ -57,7 +57,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 		if k == ord('q'):
 			break
 
-def process(detector: cv.aruco.CharucoDetector, board, img, mtx, dist):	# Load previously saved data
+def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist):	# Load previously saved data
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
 	charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray)
 	if charuco_corners is not None:
@@ -68,7 +68,7 @@ def process(detector: cv.aruco.CharucoDetector, board, img, mtx, dist):	# Load p
 		# Draw a square around the markers
 		cv.aruco.drawDetectedMarkers(img, marker_corners)
 
-		if len(charuco_ids) > 5:
+		if len(charuco_ids) >= len(board.getIds()):
 			obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
 			pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None)
 			if pose:

cam_pose_charuco_diamond.py

@@ -58,22 +58,25 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
 		if k == ord('q'):
 			break
 
-def process(detector: cv.aruco.CharucoDetector, board, img, mtx, dist):	# Load previously saved data
+def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist):	# Load previously saved data
 	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
 	diamond_corners, diamond_ids, marker_corners, marker_ids = detector.detectDiamonds(gray)
-
-	# Draw a square around the markers
-	cv.aruco.drawDetectedMarkers(img, marker_corners)
-
 	if diamond_corners is not None:
 		# Draw a square around the markers
-		cv.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids)
-		if diamond_ids is not None and len(diamond_ids) > 0:
-			obj_points, img_points = board.matchImagePoints(diamond_corners, diamond_ids)
-			pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None)
-			if pose:
-				# Draw Axis
-				cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10)
+		cv.aruco.drawDetectedCornersCharuco(img, diamond_corners, diamond_ids)
+
+		# Draw a square around the markers
+		cv.aruco.drawDetectedMarkers(img, marker_corners)
+
+		if len(diamond_ids) >= len(board.getIds()):
+			# Draw a square around the markers
+			cv.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids)
+			if False and diamond_ids is not None and len(diamond_ids) > 0:
+				obj_points, img_points = board.matchImagePoints(diamond_corners, diamond_ids)
+				pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None)
+				if pose:
+					# Draw Axis
+					cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10)
 
 	cv.imshow('img', img)