diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py index 9ae5a32..98c26af 100644 --- a/ar_tag_pose/detector_board.py +++ b/ar_tag_pose/detector_board.py @@ -1,16 +1,13 @@ -import os import abc -import math import cv2 as cv import numpy as np -from pathlib import Path -from ar_tag_pose.utils import to_board_pose_euler -import mimetypes +from reedsolo import RSCodec, ReedSolomonError RESULTS_FOLDER = "results" class BoardDetector(abc.ABC): name = None + ids = None mtx = None dist = None r_vecs = None @@ -21,8 +18,15 @@ class BoardDetector(abc.ABC): def __repr__(self): return self.name - def __init__(self, name): + def __init__(self, name: str, board_id: str, n_syms: int): self.name = name + msg_size = len(board_id) + n_rs = n_syms - msg_size + rs = RSCodec(n_rs, c_exp=7) + msg = bytes(f"{board_id}", 'utf-8') + byte_string = rs.encode(msg) + self.ids = np.array([x for x in byte_string]) + print(f"{name}: markers: {self.ids}") @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 20c971a..a17f5b7 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -3,10 +3,10 @@ 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, marker_length: float=100.0, marker_sep=10): +def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100.0, marker_sep=10, ids: np.array=None): aruco_dict = cv.aruco.getPredefinedDictionary(dict_type) params = cv.aruco.DetectorParameters() - board = cv.aruco.GridBoard(size=board_size, markerLength=marker_length, markerSeparation=marker_sep, dictionary=aruco_dict) + board = cv.aruco.GridBoard(size=board_size, markerLength=marker_length, markerSeparation=marker_sep, dictionary=aruco_dict, ids=ids) detector = cv.aruco.ArucoDetector(aruco_dict, params) return detector, board @@ -14,11 +14,11 @@ 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) + def __init__(self, name: str, dict_type: int, board_id: str, board_size: tuple[int, int]): self.dict_type = dict_type self.board_size = board_size - self.detector, self.board = factory(dict_type, board_size) + self.detector, self.board = factory(dict_type, board_size, ids=self.ids) + BoardDetector.__init__(self, name, board_id, len(self.board.getIds())) def get_board_name(self): grid_size = self.board.getGridSize() diff --git a/ar_tag_pose/detector_board_charuco.py b/ar_tag_pose/detector_board_charuco.py index 340d37f..4cbbb90 100644 --- a/ar_tag_pose/detector_board_charuco.py +++ b/ar_tag_pose/detector_board_charuco.py @@ -3,10 +3,10 @@ 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): +def factory(dict_type: int, board_size: cv.typing.Size, square_length: float = 5.0, marker_length: float = 3.0, ids: np.array=None): 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) + board = cv.aruco.CharucoBoard(size=board_size, squareLength=square_length, markerLength=marker_length, dictionary=aruco_dict, ids=ids) detector = cv.aruco.CharucoDetector(board, params) return detector, board @@ -14,11 +14,11 @@ 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) + def __init__(self, name: str, dict_type: int, board_id: str, board_size: tuple[int, int]): + self.detector, self.board = factory(dict_type, board_size, ids=self.ids) self.dict_type = dict_type self.board_size = board_size - self.detector, self.board = factory(dict_type, board_size) + BoardDetector.__init__(self, name, board_id, len(self.board.getIds())) def get_board_name(self): grid_size = self.board.getChessboardSize() diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index df6ef0d..c1c8953 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -154,6 +154,13 @@ 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 ap = argparse.ArgumentParser() @@ -175,16 +182,22 @@ def main(): ap.add_argument("-m", "--margin_length", type=int, default=10, help="Margin length of board [Pixels]") + ap.add_argument("-n", "--num_boards", type=int, + default=1, + help="Number of boards") args = vars(ap.parse_args()) + msg_size = 3 board_detector = [] if "Aruco" in args["type"]: - for n in range(2): - det = BoardDetectorAruco(f"Aruco_det-{n}", args["dict_type"], (args["board_width"], args["board_height"])) + for n in range(args["num_boards"]): + board_id = f"{n:0{msg_size}d}" + det = BoardDetectorAruco(f"Aruco_det-{n}", args["dict_type"], board_id, (args["board_width"], args["board_height"])) board_detector.append(det) elif "Charuco" in args["type"]: - for n in range(2): - det = BoardDetectorCharuco(f"Charuco_det-{n}", args["dict_type"], (args["board_width"], args["board_height"])) + for n in range(args["num_boards"]): + board_id = f"{n:0{msg_size}d}" + det = BoardDetectorCharuco(f"Charuco_det-{n}", args["dict_type"], board_id, (args["board_width"], args["board_height"])) board_detector.append(det) detector = Detector(args["source"], board_detector) @@ -194,4 +207,4 @@ def main(): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/pyproject.toml b/pyproject.toml index 4b6a14c..491e81a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,7 +14,8 @@ authors = [ dependencies = [ "opencv-python", "opencv-contrib-python", - "scipy" + "scipy", + "reedsolo" ] readme = "README.md"