- Aruco: fixed multidetection

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

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)

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()