jens/ocv_cam_pose
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- add BoardDetectorCharuco

Browse Source 
- refactored
This commit is contained in:
jens
2025-11-30 15:17:04 +01:00
parent d021084635
commit a6ea630f60
4 changed files with 104 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/detector_board.py
-2
View File
@@ -10,8 +10,6 @@ import mimetypes
RESULTS_FOLDER = "results"
class BoardDetector(abc.ABC):
detector: cv.aruco.ArucoDetector | cv.aruco.CharucoDetector = None
board: cv.aruco.GridBoard | cv.aruco.CharucoBoard = None
name = None
mtx = None
dist = None
ar_tag_pose/detector_board_aruco.py
+2
View File
@@ -11,6 +11,8 @@ def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100
return detector, board
class BoardDetectorAruco(BoardDetector):
detector: cv.aruco.ArucoDetector = None
board: cv.aruco.GridBoard = None
dict_type = None
def __init__(self, name: str, dict_type: int, board_size: tuple[int, int], marker_ids: list[int]=None):
BoardDetector.__init__(self, name)
ar_tag_pose/detector_board_charuco.py
+88
View File
@@ -0,0 +1,88 @@
import cv2 as cv
import numpy as np
from ar_tag_pose.detector_board import BoardDetector
from ar_tag_pose.aruco_types import ARUCO_DICT
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)
params = cv.aruco.CharucoParameters()
board = cv.aruco.CharucoBoard(size=board_size, squareLength=square_length, markerLength=marker_length, dictionary=aruco_dict)
detector = cv.aruco.CharucoDetector(board, params)
return detector, board
class BoardDetectorCharuco(BoardDetector):
detector: cv.aruco.CharucoDetector = None
board: cv.aruco.CharucoBoard = None
dict_type = None
def __init__(self, name: str, dict_type: int, board_size: tuple[int, int], marker_ids: list[int]=None):
BoardDetector.__init__(self, name)
self.dict_type = dict_type
self.board_size = board_size
self.detector, self.board = factory(dict_type, board_size)
def get_board_name(self):
grid_size = self.board.getChessboardSize()
keyname = [key for key, val in ARUCO_DICT.items() if val == self.dict_type][0]
return f"{self.name}_w{grid_size[0]}_h{grid_size[1]}_{keyname}"
def get_board_image(self, margin_length: int=10) -> np.ndarray:
image_size = self._image_size(margin_length)
image = self.board.generateImage(image_size, None, margin_length, 1)
return image
def match_points(self, image: cv.typing.MatLike):
success = False
obj_points = None
img_points = None
corners, ids, _, _ = self.detector.detectBoard(image)
if corners is not None:
if len(ids) >= len(self.board.getIds()):
_obj_points, _img_points = self.board.matchImagePoints(corners, ids)
if len(_obj_points) > 0 and len(_img_points) > 0:
success = True
obj_points = _obj_points
img_points = _img_points
return success, obj_points, img_points
def process(self, image: cv.typing.MatLike, r_vecs: np.ndarray=None, t_vecs: np.ndarray=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False):
pose = False
r_vec = None
t_vec = None
gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
charuco_corners, charuco_ids, marker_corners, marker_ids = self.detector.detectBoard(gray)
if charuco_corners is not None:
# Draw marker box
if draw_marker_box:
cv.aruco.drawDetectedMarkers(image, marker_corners)
# Draw marker id
if draw_marker_id:
cv.aruco.drawDetectedCornersCharuco(image, charuco_corners, charuco_ids)
# Draw marker axis
if draw_marker_axis and self.is_calibrated():
_r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1,
cameraMatrix=self.mtx, distCoeffs=self.dist)
for i in range(len(_r_vecs)):
cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05)
if len(charuco_ids) >= len(self.board.getIds()):
obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids)
offset = int(self.board.getMarkerLength() * self.board.getSquareLength())
obj_points -= [offset, offset, 0]
if self.is_calibrated():
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, useExtrinsicGuess=False,
rvec=r_vecs, tvec=t_vecs, flags=cv.SOLVEPNP_ITERATIVE)
if pose:
# Draw
cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10)
return pose, r_vec, t_vec
def _image_size(self, margin_length: int):
return 800, 600
ar_tag_pose/detector_board_main.py
+14 -13
View File
@@ -3,17 +3,16 @@ import abc
import math
import cv2 as cv
import numpy as np
from pathlib import Path
from ar_tag_pose.detector_board_aruco import BoardDetectorAruco
from ar_tag_pose.utils import to_board_pose_euler
import mimetypes
from detector_board import BoardDetector
import argparse
import mimetypes
from pathlib import Path
from ar_tag_pose.utils import to_board_pose_euler
from detector_board import BoardDetector
from detector_board_aruco import BoardDetectorAruco
from detector_board_charuco import BoardDetectorCharuco
RESULTS_FOLDER = "results"
def cap_init(cap: cv.VideoCapture):
cap.set(cv.CAP_PROP_SETTINGS, 1)
cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
@@ -178,15 +177,17 @@ def main():
help="Margin length of board [Pixels]")
args = vars(ap.parse_args())
board_detector = None
board_detector = []
if "Aruco" in args["type"]:
board_detector = (BoardDetectorAruco
("Det-0", args["dict_type"], (args["board_width"], args["board_height"])))
for n in range(2):
det = BoardDetectorAruco(f"Aruco_det-{n}", args["dict_type"], (args["board_width"], args["board_height"]))
board_detector.append(det)
elif "Charuco" in args["type"]:
board_detector = (BoardDetectorAruco
("Det-0", args["dict_type"], (args["board_width"], args["board_height"])))
for n in range(2):
det = BoardDetectorCharuco(f"Charuco_det-{n}", args["dict_type"], (args["board_width"], args["board_height"]))
board_detector.append(det)
detector = Detector(args["source"], [board_detector])
detector = Detector(args["source"], board_detector)
detector.board_image_generate(margin_length=args["margin_length"])
detector.calibrate_load()
detector.process()