From dc966a6b27d48370f541c993564a5da1c9e328b0 Mon Sep 17 00:00:00 2001 From: Markus Schiffer Date: Thu, 4 May 2023 15:53:39 -0700 Subject: [PATCH] fixed aruco tag detection for newest opencv --- stretch_core/nodes/detect_aruco_markers | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/stretch_core/nodes/detect_aruco_markers b/stretch_core/nodes/detect_aruco_markers index bef696ba..76299fee 100755 --- a/stretch_core/nodes/detect_aruco_markers +++ b/stretch_core/nodes/detect_aruco_markers @@ -191,12 +191,19 @@ class ArucoMarker: self.depth_image = depth_image self.camera_matrix = np.reshape(self.camera_info.K, (3,3)) self.distortion_coefficients = np.array(self.camera_info.D) - rvecs, tvecs, unknown_variable = aruco.estimatePoseSingleMarkers([self.corners], - self.length_of_marker_mm, - self.camera_matrix, - self.distortion_coefficients) + rvecs = np.zeros((len(self.corners), 1, 3), dtype=np.float64) + tvecs = np.zeros((len(self.corners), 1, 3), dtype=np.float64) + points_3D = np.array([ + (-self.length_of_marker_mm / 2, self.length_of_marker_mm / 2, 0), + (self.length_of_marker_mm / 2, self.length_of_marker_mm / 2, 0), + (self.length_of_marker_mm / 2, -self.length_of_marker_mm / 2, 0), + (-self.length_of_marker_mm / 2, -self.length_of_marker_mm / 2, 0), + ]) + for marker_num in range(len(self.corners)): + unknown_variable, rvecs_ret, tvecs_ret = cv2.solvePnP(objectPoints=points_3D, imagePoints=self.corners[marker_num], cameraMatrix=self.camera_matrix, distCoeffs=self.distortion_coefficients) + rvecs[marker_num][:] = np.transpose(rvecs_ret) + tvecs[marker_num][:] = np.transpose(tvecs_ret) self.aruco_rotation = rvecs[0][0] - # Convert ArUco position estimate to be in meters. self.aruco_position = tvecs[0][0]/1000.0 aruco_depth_estimate = self.aruco_position[2] @@ -520,6 +527,7 @@ class ArucoMarkerCollection: self.aruco_detection_parameters.cornerRefinementMethod = aruco.CORNER_REFINE_SUBPIX self.aruco_detection_parameters.cornerRefinementWinSize = 2 self.collection = {} + self.detector = aruco.ArucoDetector(self.aruco_dict, self.aruco_detection_parameters) self.frame_number = 0 def __iter__(self): @@ -531,7 +539,7 @@ class ArucoMarkerCollection: yield marker def draw_markers(self, image): - return aruco.drawDetectedMarkers(image, self.aruco_corners, self.aruco_ids) + return self.detector.drawDetectedMarkers(image, self.aruco_corners, self.aruco_ids) def broadcast_tf(self, tf_broadcaster): # Create TF frames for each of the markers. Only broadcast each @@ -548,9 +556,7 @@ class ArucoMarkerCollection: self.depth_image = depth_image self.gray_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_BGR2GRAY) image_height, image_width = self.gray_image.shape - self.aruco_corners, self.aruco_ids, aruco_rejected_image_points = aruco.detectMarkers(self.gray_image, - self.aruco_dict, - parameters = self.aruco_detection_parameters) + self.aruco_corners, self.aruco_ids, aruco_rejected_image_points = self.detector.detectMarkers(self.gray_image) if self.aruco_ids is None: num_detected = 0 else: