""" ************************************************************************* // Example program using OpenCV library // python >3.7 - OpenCV 4.5 // @file e9c.py // @author Luis M. Jimenez // @date 2022 // // @brief Course: Computer Vision (1782) // Dept. of Systems Engineering and Automation // Automation, Robotics and Computer Vision Lab (ARVC) // http://arvc.umh.es // University Miguel Hernandez // // @note Description: // - Loads camera calibration data from file (JSON type) // - Capture images from a camera // - Detects markers using ARUCO library // - Reads ARUCO Board Configuration file // - Calculates POSE from camera calibration and board file // - Draws axis 3D over captured image // ************************************************************************* """ # Import libraries import cv2 as cv import numpy as np import argparse import json import collections # ----------------------------------------- # Global variables # ----------------------------------------- WINDOW_CAMERA1 = '(W1) Camera 1' # window id CAMERA_ID = 0 # default camera CALIB_FILE = 'camera.json' # default camera calibration file BOARD_FILE = 'ARUCO_board.json' # default ARUCO board data file # check command line parameters (camera id) parser = argparse.ArgumentParser(description='OpenCV example: Markers detection - POSE') parser.add_argument('-c', dest='cameraID', type=int, default=CAMERA_ID, metavar='id', help='camera id') parser.add_argument('-cal', dest='calibFile', type=str, default=CALIB_FILE, metavar='calibFile', help='Camera Calibration File (JSON)') parser.add_argument('-board', dest='boardFile', type=str, default=BOARD_FILE, metavar='boardFile', help='ARUCO Board Data File (JSON)') CAMERA_ID = parser.parse_args().cameraID CALIB_FILE = parser.parse_args().calibFile BOARD_FILE = parser.parse_args().boardFile # ----------------------------------------- # Put here the code to Initialize objets # ----------------------------------------- # Init ARUCO dictionary dictionary = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_ARUCO_ORIGINAL) # define ARUCO detector object detector = cv.aruco.ArucoDetector(dictionary) # Load camera calibration data try: with open(CALIB_FILE) as file: data = json.load(file) except: print('Camera Calibration File not valid') exit() cameraMatrix = np.array(data['camera_matrix']) distCoeffs = np.array(data['distortion_coefficients']) print(f"{cameraMatrix=}") print(f"{distCoeffs=}") # Load ARUCO Board data try: with open(BOARD_FILE) as file: data = json.load(file) except: print('Board Data File not valid') exit() # data['aruco_bc_markers'] contains the marker's data (ids, 3D coordenates) as a dictionary # split ids and objPoints (corners) in two separate lists boardIds = [ item["id"] for item in data['aruco_bc_markers'] ] boardPoints = [ item["corners"] for item in data['aruco_bc_markers'] ] # convert lists to np.array keeping first dimension even with only one marker boardIds = np.array(boardIds).astype(int) boardPoints = np.array(boardPoints).astype(np.float32) #print(f"{boardIds=}") #print(f"{boardPoints=}") # Create cv.aruco.Board Object (id's and 3D corner's coordinates) board = cv.aruco.Board(boardPoints, dictionary, boardIds) # Single marker data (used for individual marker POSE) marker_size = 57.0 # mm marker3Dpoints = np.array([ [-marker_size/2, -marker_size/2, 0], # (0,0,0) center of the marker [-marker_size/2, marker_size/2, 0], [ marker_size/2, marker_size/2, 0], [ marker_size/2, -marker_size/2, 0] ], dtype=np.float32) # Open camera object camera = cv.VideoCapture(CAMERA_ID) if not camera.isOpened(): print("you need to connect a camera, sorry.") exit() # Getting camera resolution cameraWidth = int(camera.get(cv.CAP_PROP_FRAME_WIDTH)) cameraHeight = int(camera.get(cv.CAP_PROP_FRAME_HEIGHT)) # Creating visualization windows cv.namedWindow(WINDOW_CAMERA1, cv.WINDOW_AUTOSIZE) print(f"Capturing images from camera {CAMERA_ID} ({cameraWidth},{cameraHeight})") print("...Hit q/Q/Esc to exit.") # ----------------------------------------- # Main Loop # while there are images ... # ----------------------------------------- while True: # Capture frame-by-frame ret, capture = camera.read() # if frame is read correctly ret is True if not ret: print("Can't receive frame (stream end?). Exiting ...") break # ----------------------------------------- # Put your image processing code here # ----------------------------------------- gray_image = cv.cvtColor(capture, cv.COLOR_BGR2GRAY) # transforms to gray level corners, ids, rejectedImgPoints = detector.detectMarkers(gray_image) dispimage = cv.aruco.drawDetectedMarkers(capture, corners, ids, borderColor=(0,0,255)) # display corner order for item in corners: for i in range(4): cv.putText(dispimage, str(i), item[0,i].astype(int), cv.FONT_HERSHEY_DUPLEX, 0.4, (0,255,255), 1, cv.LINE_AA) # Calculate POSE if markers are detected if len(corners)>0: # Find 3D coordinates for each corner of the markers detected using Board data # returns a flat coordinates array with all matching corners objPoints, imgPoints = board.matchImagePoints(corners, ids) # Calculate POSE with all board corners (if detected) if objPoints is not None: retval, rvec, tvec = cv.solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, flags=cv.SOLVEPNP_ITERATIVE ) # Draw board axis dispimage = cv.drawFrameAxes(dispimage, cameraMatrix, distCoeffs, rvec, tvec, length=50.0) #print(f"{rvec=} \n {tvec=}") ############################################## # Calculate individual POSE for each marker for item in corners: retval, rvec, tvec = cv.solvePnP(marker3Dpoints, item, cameraMatrix, distCoeffs, flags=cv.SOLVEPNP_ITERATIVE ) # Draw axis for each marker dispimage = cv.drawFrameAxes(dispimage, cameraMatrix, distCoeffs, rvec, tvec, length=25.0, thickness=2) # enf if len(corners)>0: # ----------------------------------------- # Put your visualization code here # ----------------------------------------- cv.imshow(WINDOW_CAMERA1, dispimage) # Display the resulting frame # check keystroke to exit (image window must be on focus) key = cv.pollKey() if key == ord('q') or key == ord('Q') or key == 27: break # End while (main loop) # ----------------------------------------- # free windows and camera resources # ----------------------------------------- cv.destroyAllWindows() if camera.isOpened(): camera.release()