fixed aruco tag detection for newest opencv

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: