ZED Camera, Multiple ArUco Marker Pose Detection - Carleton-SRCL/SPOT GitHub Wiki

  """
  BFD_RPY_Stereo.py is the live pose detection and conversion into roll, pitch, yaw frame from captured video 
  from a stereo camera. The code from the live stereo feed is able to detect aruco markers, determine their pose 
  convert this data into yaw pitch roll and finally export it into a test file.
  
  This code was written by Austin Dahlseide and further developed by Hayden Arms to work for a stereo feed
  
  November 2023
  
  """
  
  import cv2 as cv
  from cv2 import aruco
  import numpy as np
  import math
  import time
  
  calib_data_path = r"/On table Cal"
  L_cam_mat = np.load(calib_data_path + '/Left_calibration_matrix.npy')
  R_cam_mat = np.load(calib_data_path + '/Right_calibration_matrix.npy')
  L_dist_coef = np.load(calib_data_path + '/Left_distortion_coefficients.npy')
  R_dist_coef = np.load(calib_data_path + '/Right_distortion_coefficients.npy')
  r_vectors = np.load(calib_data_path + '/RotationVector.npy')
  t_vectors = np.load(calib_data_path + '/TranslationVector.npy')
  
  # calib_data_path = r"/Camera_model.npy"
  # camera_model = np.load(calib_data_path)
  # L_cam_mat = camera_model["ML"]
  # R_cam_mat = camera_model["MR"]
  # L_dist_coef = camera_model["distL"]
  # R_dist_coef = camera_model["distR"]
  # r_vectors = camera_model["R"]
  # t_vectors = camera_model["T"]
  
  
  MARKER_SIZE = 3.33  # centimeters
  marker_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_1000)
  param_markers = aruco.EstimateParameters()
  
  
  def isRotationMatrix(R):
      Rt = np.transpose(R)
      shouldBeIdentity = np.dot(Rt, R)
      I = np.identity(3, dtype=R.dtype)
      n = np.linalg.norm(I - shouldBeIdentity)
      return n < 1e-6
  
  
  # Calculates rotation matrix to euler angles
  # The result is the same as MATLAB except the order
  # of the euler angles ( x and z are swapped ).
  def rotationMatrixToEulerAngles(R):
      assert (isRotationMatrix(R))
  
      sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
  
      singular = sy < 1e-6
  
      if not singular:
          x = math.atan2(R[2, 1], R[2, 2])
          y = math.atan2(-R[2, 0], sy)
          z = math.atan2(R[1, 0], R[0, 0])
      else:
          x = math.atan2(-R[1, 2], R[1, 1])
          y = math.atan2(-R[2, 0], sy)
          z = 0
  
      return np.array([x, y, z])
  
  
  def DnD(MARKER_SIZE, marker_corners, marker_IDs, frame, cam_mat, dist_coef, rVec, tVec, roll_dict, pitch_dict, yaw_dict, timestamp, data_combined):
      # Detect and Draw
      rVec, tVec, _ = aruco.estimatePoseSingleMarkers(marker_corners, MARKER_SIZE, cam_mat, dist_coef)
      total_markers = range(0, marker_IDs.size)
  
      # Append the rotation vector of each marker to the list
      rvecs_list.extend(rVec)
      # Calculate the timestamp as the time elapsed since the start
      timestamp = time.time() - start_time
      for ids, corners, i in zip(marker_IDs, marker_corners, total_markers):
          cv.polylines(frame, [corners.astype(np.int32)], True, (0, 255, 255), 4, cv.LINE_AA)
  
          corners = corners.reshape(4, 2)
          corners = corners.astype(int)
          top_right = corners[0].ravel()
          top_left = corners[1].ravel()
          bottom_right = corners[2].ravel()
          bottom_left = corners[3].ravel()
  
          # -- Obtain the rotation matrix tag->camera
          R_ct = np.matrix(cv.Rodrigues(rVec[i])[0])
          R_tc = R_ct.T
  
          # -- Get the attitude in terms of euler 321 (Needs to be flipped first)
          roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(R_flip * R_tc)
  
          # Store Euler angles in dictionaries based on marker ID
          roll_dict[ids[0]] = math.degrees(roll_marker)
          pitch_dict[ids[0]] = math.degrees(pitch_marker)
          yaw_dict[ids[0]] = math.degrees(yaw_marker)
  
          data_combined.append(
              [ids[0], roll_dict[ids[0]], pitch_dict[ids[0]], yaw_dict[ids[0]], tVec[i][0][0], tVec[i][0][1], tVec[i][0][2], timestamp])
  
          # Calculating the distance
          distance = np.linalg.norm(tVec[i])
  
          # Draw the pose of the marker
          point = cv.drawFrameAxes(frame, cam_mat, dist_coef, rVec[i], tVec[i], 4, 4)
          cv.putText(frame, f"id: {ids[0]} Dist ('z'): {round(distance, 2)}", top_right, cv.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 255), 2, cv.LINE_AA,)
          cv.putText(frame, f"x:{round(tVec[i][0][0], 1)} y: {round(tVec[i][0][1], 1)} ", bottom_right, cv.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 255), 2, cv.LINE_AA,)
          cv.putText(frame, "RPY: R=%4.0f  P=%4.0f  Y=%4.0f" % (math.degrees(roll_marker), math.degrees(pitch_marker), math.degrees(yaw_marker)), top_left, cv.FONT_HERSHEY_PLAIN, 1.0, (0, 125, 0), 2, cv.LINE_AA,)
          # print(ids, "  ", corners)
      return frame, roll_dict, pitch_dict, yaw_dict, data_combined
  
  
  # --- 180 deg rotation matrix around the x axis
  R_flip = np.zeros((3, 3), dtype=np.float32)
  R_flip[0, 0] = 1.0
  R_flip[1, 1] = -1.0
  R_flip[2, 2] = -1.0
  
  # Record the start time
  start_time = time.time()
  # Initialize
  data_combined_L = []  # List to store combined data
  data_combined_R = []  # List to store combined data
  # Initialize empty dictionaries for Roll, Pitch, and Yaw Angles
  roll_dict_R = {}
  pitch_dict_R = {}
  yaw_dict_R = {}
  roll_dict_L = {}
  pitch_dict_L = {}
  yaw_dict_L = {}
  
  # Initialize an empty list to store the rotation vectors 
  rvecs_list = []
  
  cap = cv.VideoCapture(0)
  cap.set(cv.CAP_PROP_FRAME_WIDTH, 1920)
  cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1080)
  
  while True:
      ret, Fullframe = cap.read()
      if not ret:
          break  # check if imported correctly
  
      h, w, _ = Fullframe.shape  # Split inputs into left and right
      w_c = w // 2
      frame_left = Fullframe[0:h, 0:w_c]
      frame_right = Fullframe[0:h, w_c:w]
      R_gray_frame = cv.cvtColor(frame_right, cv.COLOR_BGR2GRAY)
      L_gray_frame = cv.cvtColor(frame_left, cv.COLOR_BGR2GRAY)
      L_marker_corners, L_marker_IDs, reject = aruco.detectMarkers(L_gray_frame, marker_dict, parameters=aruco.DetectorParameters())
      R_marker_corners, R_marker_IDs, reject = aruco.detectMarkers(R_gray_frame, marker_dict, parameters=aruco.DetectorParameters())
  
      time_stamp = time.time() - start_time
  
      for marker_id in roll_dict_L.keys():
          roll_dict_L[marker_id] = None
          pitch_dict_L[marker_id] = None
          yaw_dict_L[marker_id] = None
      for marker_id in roll_dict_R.keys():
          roll_dict_R[marker_id] = None
          pitch_dict_R[marker_id] = None
          yaw_dict_R[marker_id] = None
  
      # Split into left and right frames and process. Detecting markers in each frame then combining them into a single
      # Reset Euler angles for all marker IDs to None at the beginning of each iteration
  
      # frame for display
      if L_marker_corners:
          L_img, roll_dict_L, pitch_dict_L, yaw_dict_L, combined_data_L = DnD(MARKER_SIZE, L_marker_corners, L_marker_IDs, frame_left, L_cam_mat, L_dist_coef, r_vectors, t_vectors, roll_dict_L, pitch_dict_L, yaw_dict_L, time_stamp, data_combined_L)
      else:
          L_img = frame_left
  
      if R_marker_corners:
          R_img, roll_dict_R, pitch_dict_R, yaw_dict_R, combined_data_R = DnD(MARKER_SIZE, R_marker_corners, R_marker_IDs, frame_right, R_cam_mat, R_dist_coef, r_vectors, t_vectors, roll_dict_R, pitch_dict_R, yaw_dict_R, time_stamp, data_combined_R)
      else:
          R_img = frame_right
  
      # stack side by side
      Hori = np.concatenate((L_img, R_img), axis=1)
      cv.imshow('HORIZONTAL', Hori)
      key = cv.waitKey(1)
      if key == ord("q"):
          break
  cap.release()
  
  np.savetxt('combined_data_L.csv', data_combined_L, delimiter=',', fmt='%d,%f,%f,%f,%f,%f,%f,%f',
             header="ID,Roll,Pitch,Yaw,tVec_x,tVec_y,tVec_z,time")
  np.savetxt('combined_data_R.csv', data_combined_R, delimiter=',', fmt='%d,%f,%f,%f,%f,%f,%f,%f',
             header="ID,Roll,Pitch,Yaw,tVec_x,tVec_y,tVec_z,time")
  # data_combined.append([ids[0], roll_dict[ids[0]], pitch_dict[ids[0]], yaw_dict[ids[0]], tVec[i][0][0], tVec[i][0][1], tVec[i][0][2], timestamp])
  cv.destroyAllWindows()