diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py index 9e25309..84d7bc0 100644 --- a/ar_tag_pose/detector_board.py +++ b/ar_tag_pose/detector_board.py @@ -5,37 +5,35 @@ import numpy as np RESULTS_FOLDER = "results" class BoardDetector(abc.ABC): - name = None - ids = None - mtx = None - dist = None - r_vecs = None - t_vecs = None - obj_points_list = [] - img_points_list = [] - board_id = None def __repr__(self): return self.name def __init__(self, name: str, board_id: str, n_syms: int): + self.mtx = None + self.dist = None + self.r_vecs = None + self.t_vecs = None self.name = name + self.obj_points_list = [] + self.img_points_list = [] self.board_id = board_id + + # Create IDs 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}") - def match_ids(self, ids: np.array): + def match_ids(self, corners_in, ids_in: np.array): + corners = () + ids = [] offset = int(self.board_id) * len(self.ids) - if ids is None: - return False + if ids_in is not None: + for n in range(len(ids_in)): + if ((ids_in[n] - offset) >= 0) and ((ids_in[n] - offset) < len(self.ids)): + temp = corners_in[n] + corners += (temp,) + ids.append(ids_in[n]) - for n in range(len(ids)): - if (ids[n] - offset) < 0: - return False - if (ids[n] - offset) > len(self.ids): - return False - - return True + return corners, np.array(ids) @abc.abstractmethod def get_board_name(self) -> str: @@ -87,15 +85,11 @@ class BoardDetector(abc.ABC): ret, mtx, dist, r_vecs, t_vecs = cv.calibrateCamera(self.obj_points_list, self.img_points_list, image.shape, self.mtx, self.dist, rvecs=r_vecs, tvecs=t_vecs, flags=flags) - print(f"Camera calibration finished: ", end='') if ret < 3: - print(f"Success ({ret})!") success = True self.mtx = mtx self.dist = dist self.r_vecs = r_vecs self.t_vecs = t_vecs - else: - print(f"Failed ({ret})!") return success diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py index 1c0d7f5..39a2ff7 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -35,17 +35,16 @@ class BoardDetectorAruco(BoardDetector): success = False 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) - if len(_obj_points) > 0 and len(_img_points) > 0: - success =True - obj_points = _obj_points - img_points = _img_points + corners, ids, _ = self.detector.detectMarkers(image) + corners, ids = self.match_ids(corners, ids) + + 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 @@ -55,30 +54,28 @@ class BoardDetectorAruco(BoardDetector): t_vec = None gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY) - (corners, ids, rejected) = self.detector.detectMarkers(gray) + corners, ids, rejected = self.detector.detectMarkers(gray) + corners, ids = self.match_ids(corners, ids) - # Draw a square around the markers - if draw_marker_box: - cv.aruco.drawDetectedMarkers(image, corners) + if len(ids) > 0: + # Draw a square around the markers + if draw_marker_box: + cv.aruco.drawDetectedMarkers(image, corners) - # Draw marker id - if draw_marker_id: - cv.aruco.drawDetectedMarkers(image, corners, ids) + # Draw marker id + if draw_marker_id: + cv.aruco.drawDetectedMarkers(image, corners, ids) - if not self.is_calibrated(): - return pose, None, None + 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) + if _r_vecs is not None: + for i in range(len(_r_vecs)): + cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05) - # Draw marker axis - if draw_marker_axis: - _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=self.mtx, distCoeffs=self.dist) - if _r_vecs is not None: - for i in range(len(_r_vecs)): - cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05) - - if len(corners) > 0: # Estimate pose of each marker and return the values r_vec and t_vec---(different from those of camera coefficients) obj_points, img_points = self.board.matchImagePoints(corners, ids) pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, rvec=r_vecs, tvec=t_vecs) diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index 185870b..60f604c 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -61,11 +61,11 @@ class Detector(abc.ABC): name = det.get_board_name() cv.imwrite(f"{RESULTS_FOLDER}/{name}.png", image) - def calibrate_save(self): - for det in self.detector: - filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz" - mtx, dist, r_vecs, t_vecs = det.calibration_get() - np.savez(f"{filename}", mtx=mtx, dist=dist, rvecs=r_vecs, tvecs=t_vecs) + @staticmethod + def calibrate_save(det: BoardDetector): + filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz" + mtx, dist, r_vecs, t_vecs = det.calibration_get() + np.savez(f"{filename}", mtx=mtx, dist=dist, rvecs=r_vecs, tvecs=t_vecs) def calibrate_load(self): for det in self.detector: @@ -127,13 +127,12 @@ class Detector(abc.ABC): success = det.collect_points(gray) if success: print(f"{det}: Points matching success") - else: - print(f"{det}: Points matching failed, try again") elif k == ord('c'): gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) for det in self.detector: - det.calibrate_camera(gray) - self.calibrate_save() + if det.calibrate_camera(gray): + print(f"{det}: Camera calibration success") + self.calibrate_save(det) elif k == ord('x'): for det in self.detector: det.calibration_clear() diff --git a/images/cube_0001.png b/images/cube_0001.png index 6fc5210..e51c30a 100644 Binary files a/images/cube_0001.png and b/images/cube_0001.png differ diff --git a/images/cube_0002.png b/images/cube_0002.png index 1370c91..3eb4090 100644 Binary files a/images/cube_0002.png and b/images/cube_0002.png differ diff --git a/images/cube_0003.png b/images/cube_0003.png index 2abca8d..67fc1a7 100644 Binary files a/images/cube_0003.png and b/images/cube_0003.png differ diff --git a/images/cube_0004.png b/images/cube_0004.png index 6fa887c..985fa09 100644 Binary files a/images/cube_0004.png and b/images/cube_0004.png differ diff --git a/images/cube_0005.png b/images/cube_0005.png index 1ac5b34..bedfd93 100644 Binary files a/images/cube_0005.png and b/images/cube_0005.png differ diff --git a/images/cube_0006.png b/images/cube_0006.png index b380d49..465c7a5 100644 Binary files a/images/cube_0006.png and b/images/cube_0006.png differ diff --git a/images/cube_0007.png b/images/cube_0007.png index b8c43cf..f883bd2 100644 Binary files a/images/cube_0007.png and b/images/cube_0007.png differ diff --git a/images/cube_0008.png b/images/cube_0008.png index e454fca..6082416 100644 Binary files a/images/cube_0008.png and b/images/cube_0008.png differ diff --git a/images/cube_0009.png b/images/cube_0009.png index 26ab59a..d9db867 100644 Binary files a/images/cube_0009.png and b/images/cube_0009.png differ diff --git a/images/cube_0010.png b/images/cube_0010.png index fe869a3..1bb30a2 100644 Binary files a/images/cube_0010.png and b/images/cube_0010.png differ diff --git a/images/cube_0011.png b/images/cube_0011.png index 895a1c4..f1253c0 100644 Binary files a/images/cube_0011.png and b/images/cube_0011.png differ diff --git a/images/cube_0012.png b/images/cube_0012.png index 136bb17..9736033 100644 Binary files a/images/cube_0012.png and b/images/cube_0012.png differ diff --git a/images/cube_0013.png b/images/cube_0013.png index 645b4d4..27a3a01 100644 Binary files a/images/cube_0013.png and b/images/cube_0013.png differ diff --git a/images/cube_0014.png b/images/cube_0014.png index d8bf665..19ae683 100644 Binary files a/images/cube_0014.png and b/images/cube_0014.png differ diff --git a/images/cube_0015.png b/images/cube_0015.png index 7691b32..7a5bf4d 100644 Binary files a/images/cube_0015.png and b/images/cube_0015.png differ diff --git a/images/cube_0016.png b/images/cube_0016.png index 1c5ffec..896581b 100644 Binary files a/images/cube_0016.png and b/images/cube_0016.png differ diff --git a/images/cube_0017.png b/images/cube_0017.png index ad7a1eb..5548cc1 100644 Binary files a/images/cube_0017.png and b/images/cube_0017.png differ diff --git a/images/cube_0018.png b/images/cube_0018.png index ead4d68..8d7bde7 100644 Binary files a/images/cube_0018.png and b/images/cube_0018.png differ diff --git a/images/cube_0019.png b/images/cube_0019.png index 3d09967..6546c31 100644 Binary files a/images/cube_0019.png and b/images/cube_0019.png differ diff --git a/images/cube_0020.png b/images/cube_0020.png index 7eee160..7e646ef 100644 Binary files a/images/cube_0020.png and b/images/cube_0020.png differ diff --git a/images/cube_0021.png b/images/cube_0021.png index 45b8227..bb9c50c 100644 Binary files a/images/cube_0021.png and b/images/cube_0021.png differ diff --git a/images/cube_0022.png b/images/cube_0022.png index c06ca95..82c6248 100644 Binary files a/images/cube_0022.png and b/images/cube_0022.png differ diff --git a/images/cube_0023.png b/images/cube_0023.png index f70e6be..4cc7db2 100644 Binary files a/images/cube_0023.png and b/images/cube_0023.png differ diff --git a/images/cube_0024.png b/images/cube_0024.png index ea8978c..997f2a1 100644 Binary files a/images/cube_0024.png and b/images/cube_0024.png differ diff --git a/images/cube_0025.png b/images/cube_0025.png index a41e5c2..d3dcdb7 100644 Binary files a/images/cube_0025.png and b/images/cube_0025.png differ diff --git a/images/cube_0026.png b/images/cube_0026.png index 78e7fba..6296b1a 100644 Binary files a/images/cube_0026.png and b/images/cube_0026.png differ diff --git a/images/cube_0027.png b/images/cube_0027.png index e9aade8..bd57c3b 100644 Binary files a/images/cube_0027.png and b/images/cube_0027.png differ diff --git a/images/cube_0028.png b/images/cube_0028.png index 077b778..f15951e 100644 Binary files a/images/cube_0028.png and b/images/cube_0028.png differ diff --git a/images/cube_0029.png b/images/cube_0029.png index a49ddb1..dc7ed91 100644 Binary files a/images/cube_0029.png and b/images/cube_0029.png differ diff --git a/images/cube_0030.png b/images/cube_0030.png index 61f0f57..7c6b111 100644 Binary files a/images/cube_0030.png and b/images/cube_0030.png differ diff --git a/images/cube_0031.png b/images/cube_0031.png index 97cde5b..9044b09 100644 Binary files a/images/cube_0031.png and b/images/cube_0031.png differ diff --git a/images/cube_0032.png b/images/cube_0032.png index 52b56f0..da0e051 100644 Binary files a/images/cube_0032.png and b/images/cube_0032.png differ diff --git a/images/cube_0033.png b/images/cube_0033.png index 45ea509..fd67861 100644 Binary files a/images/cube_0033.png and b/images/cube_0033.png differ diff --git a/images/cube_0034.png b/images/cube_0034.png index 8dbb1be..4934b7a 100644 Binary files a/images/cube_0034.png and b/images/cube_0034.png differ diff --git a/images/cube_0035.png b/images/cube_0035.png index d34c234..1483e97 100644 Binary files a/images/cube_0035.png and b/images/cube_0035.png differ diff --git a/images/cube_0036.png b/images/cube_0036.png index c8d3bd1..4523d98 100644 Binary files a/images/cube_0036.png and b/images/cube_0036.png differ diff --git a/images/cube_0037.png b/images/cube_0037.png index e4f168b..b3aaf03 100644 Binary files a/images/cube_0037.png and b/images/cube_0037.png differ diff --git a/images/cube_0038.png b/images/cube_0038.png index 6d53bcb..f187ab3 100644 Binary files a/images/cube_0038.png and b/images/cube_0038.png differ diff --git a/images/cube_0039.png b/images/cube_0039.png index 49cb7d7..426251a 100644 Binary files a/images/cube_0039.png and b/images/cube_0039.png differ diff --git a/images/cube_0040.png b/images/cube_0040.png index 2869a63..87a5411 100644 Binary files a/images/cube_0040.png and b/images/cube_0040.png differ diff --git a/images/cube_0041.png b/images/cube_0041.png index 7793ee2..879ed65 100644 Binary files a/images/cube_0041.png and b/images/cube_0041.png differ diff --git a/images/cube_0042.png b/images/cube_0042.png index d6e7bd1..82ab3b0 100644 Binary files a/images/cube_0042.png and b/images/cube_0042.png differ diff --git a/images/cube_0043.png b/images/cube_0043.png index 263eeee..dd1e023 100644 Binary files a/images/cube_0043.png and b/images/cube_0043.png differ diff --git a/images/cube_0044.png b/images/cube_0044.png index 6014ed6..fabf4ee 100644 Binary files a/images/cube_0044.png and b/images/cube_0044.png differ diff --git a/images/cube_0045.png b/images/cube_0045.png index 4d34ad1..e2e68ce 100644 Binary files a/images/cube_0045.png and b/images/cube_0045.png differ diff --git a/images/cube_0046.png b/images/cube_0046.png index 5ea30dd..75ecfeb 100644 Binary files a/images/cube_0046.png and b/images/cube_0046.png differ diff --git a/images/cube_0047.png b/images/cube_0047.png index 2aff593..6bc7266 100644 Binary files a/images/cube_0047.png and b/images/cube_0047.png differ diff --git a/images/cube_0048.png b/images/cube_0048.png index 33c5414..cd8b368 100644 Binary files a/images/cube_0048.png and b/images/cube_0048.png differ diff --git a/images/cube_0049.png b/images/cube_0049.png index 1e1fdd8..9c440d4 100644 Binary files a/images/cube_0049.png and b/images/cube_0049.png differ diff --git a/images/cube_0050.png b/images/cube_0050.png index cded883..fdd2c31 100644 Binary files a/images/cube_0050.png and b/images/cube_0050.png differ diff --git a/images/cube_0051.png b/images/cube_0051.png index f376f4a..b79f7aa 100644 Binary files a/images/cube_0051.png and b/images/cube_0051.png differ diff --git a/images/cube_0052.png b/images/cube_0052.png index 9ea41bf..540af7d 100644 Binary files a/images/cube_0052.png and b/images/cube_0052.png differ diff --git a/images/cube_0053.png b/images/cube_0053.png index ad6c37d..cb2b069 100644 Binary files a/images/cube_0053.png and b/images/cube_0053.png differ diff --git a/images/cube_0054.png b/images/cube_0054.png index ef98852..52d83a3 100644 Binary files a/images/cube_0054.png and b/images/cube_0054.png differ diff --git a/images/cube_0055.png b/images/cube_0055.png index 288a4fa..9dcb89f 100644 Binary files a/images/cube_0055.png and b/images/cube_0055.png differ diff --git a/images/cube_0056.png b/images/cube_0056.png index f910661..3a60e44 100644 Binary files a/images/cube_0056.png and b/images/cube_0056.png differ diff --git a/images/cube_0057.png b/images/cube_0057.png index 84f7c7b..3883c9d 100644 Binary files a/images/cube_0057.png and b/images/cube_0057.png differ diff --git a/images/cube_0058.png b/images/cube_0058.png index 745e217..1beeb13 100644 Binary files a/images/cube_0058.png and b/images/cube_0058.png differ diff --git a/images/cube_0059.png b/images/cube_0059.png index 7988145..36c4cf9 100644 Binary files a/images/cube_0059.png and b/images/cube_0059.png differ diff --git a/images/cube_0060.png b/images/cube_0060.png index 060c7b7..4a1afab 100644 Binary files a/images/cube_0060.png and b/images/cube_0060.png differ diff --git a/images/cube_0061.png b/images/cube_0061.png index 22dc800..9f4a40b 100644 Binary files a/images/cube_0061.png and b/images/cube_0061.png differ diff --git a/images/cube_0062.png b/images/cube_0062.png index cbc6427..97b9c0a 100644 Binary files a/images/cube_0062.png and b/images/cube_0062.png differ diff --git a/images/cube_0063.png b/images/cube_0063.png index b38c31d..f120741 100644 Binary files a/images/cube_0063.png and b/images/cube_0063.png differ diff --git a/images/cube_0064.png b/images/cube_0064.png index 0a8b4d1..4f4994e 100644 Binary files a/images/cube_0064.png and b/images/cube_0064.png differ diff --git a/images/cube_0065.png b/images/cube_0065.png index af1be07..9d7c5b5 100644 Binary files a/images/cube_0065.png and b/images/cube_0065.png differ diff --git a/images/cube_0066.png b/images/cube_0066.png index e186924..04a00e5 100644 Binary files a/images/cube_0066.png and b/images/cube_0066.png differ diff --git a/images/cube_0067.png b/images/cube_0067.png index 5715163..f2127bb 100644 Binary files a/images/cube_0067.png and b/images/cube_0067.png differ diff --git a/images/cube_0068.png b/images/cube_0068.png index ee1310b..f02e63f 100644 Binary files a/images/cube_0068.png and b/images/cube_0068.png differ diff --git a/images/cube_0069.png b/images/cube_0069.png index 897a344..d355aa6 100644 Binary files a/images/cube_0069.png and b/images/cube_0069.png differ diff --git a/images/cube_0070.png b/images/cube_0070.png index 64fd95b..9a07b44 100644 Binary files a/images/cube_0070.png and b/images/cube_0070.png differ diff --git a/images/cube_0071.png b/images/cube_0071.png index bc9e0d6..9d0b8d6 100644 Binary files a/images/cube_0071.png and b/images/cube_0071.png differ