diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py index c29426e..9e25309 100644 --- a/ar_tag_pose/detector_board.py +++ b/ar_tag_pose/detector_board.py @@ -1,7 +1,6 @@ import abc import cv2 as cv import numpy as np -from reedsolo import RSCodec, ReedSolomonError RESULTS_FOLDER = "results" @@ -14,7 +13,6 @@ class BoardDetector(abc.ABC): t_vecs = None obj_points_list = [] img_points_list = [] - rs = None board_id = None def __repr__(self): return self.name @@ -22,22 +20,22 @@ class BoardDetector(abc.ABC): def __init__(self, name: str, board_id: str, n_syms: int): self.name = name self.board_id = board_id - msg_size = len(board_id) - n_rs = n_syms - msg_size - self.rs = RSCodec(n_rs, c_exp=7) + offset = int(board_id)*n_syms + self.ids = np.array([x + offset for x in range(n_syms)]) + print(f"{self.name}: markers: {self.ids}") - msg = bytes(f"{board_id}", 'utf-8') - byte_string = self.rs.encode(msg) - self.ids = np.array([x for x in byte_string]) - print(f"{name}: markers: {self.ids}") + def match_ids(self, ids: np.array): + offset = int(self.board_id) * len(self.ids) + if ids is None: + return False - def decode(self, ids: np.array): - print(ids) - try: - msg = self.rs.decode(ids) - return msg[0:len(self.board_id)] - except ReedSolomonError: - return None + for n in range(len(ids)): + if (ids[n] - offset) < 0: + return False + if (ids[n] - offset) > len(self.ids): + return False + + return True @abc.abstractmethod def get_board_name(self) -> str: diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py index 5242579..1c0d7f5 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -36,6 +36,9 @@ class BoardDetectorAruco(BoardDetector): obj_points = None img_points = None corners, ids, _ = self.detector.detectMarkers(image) + if not self.match_ids(ids): + return False, None, None + if corners is not None: if len(ids) >= len(self.board.getIds()): _obj_points, _img_points = self.board.matchImagePoints(corners, ids) @@ -65,6 +68,9 @@ class BoardDetectorAruco(BoardDetector): if not self.is_calibrated(): return pose, None, None + if not self.match_ids(ids): + return pose, None, None + # Draw marker axis if draw_marker_axis: _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=self.mtx, distCoeffs=self.dist) diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index c1c8953..185870b 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -154,12 +154,6 @@ class Detector(abc.ABC): cv.destroyAllWindows() -def gen_marker_list(): - msg = bytes(f"{n:0{msg_size}d}", 'utf-8') - byte_string = rs.encode(msg) - marker_list = [x for x in byte_string] - print(marker_list) - def main(): # construct the argument parser and parse the arguments