def setobj(flag): cap = cv2.VideoCapture(1) w = cap.get(cv2.CAP_PROP_FRAME_WIDTH) h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT) # print w # print h time.sleep(0) while True: ret, frame = cap.read() framebw = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250) parameters = aruco.DetectorParameters_create() corners, ids, _ = aruco.detectMarkers(framebw, aruco_dict, parameters=parameters) # print ids t = len(ids) #gives total aruco markers # print t l = 0 l1 = 0 for i in range(0, t): z = int(ids[i][0]) # print'>>>>>>>' cv2.circle(frame, (corners[i][0][0][0], corners[i][0][0][1]), 5, (125, 125, 125), -1) cv2.circle(frame, (corners[i][0][1][0], corners[i][0][1][1]), 5, (0, 255, 0), -1) cv2.circle(frame, (corners[i][0][2][0], corners[i][0][2][1]), 5, (180, 105, 255), -1) cv2.circle(frame, (corners[i][0][3][0], corners[i][0][3][1]), 5, (255, 255, 255), -1) x = ((corners[i][0][0][0] + corners[i][0][2][0]) / 2) y = ((corners[i][0][0][1] + corners[i][0][2][1]) / 2) cv2.circle(frame, (int(x), int(y)), 5, (255, 0, 0), -1) #center x1 = (corners[i][0][0][0] + corners[i][0][1][0]) / 2 y1 = (corners[i][0][0][1] + corners[i][0][1][1]) / 2 cv2.circle(frame, (int(x1), int(y1)), 5, (0, 0, 255), -1) #midpoint of starting edge cv2.line(frame, (int(x), int(y)), (int(x1), int(y1)), (255, 0, 0), 3) a = y1 - y b = x1 - x theta = 3.14 - math.atan2( a, b ) #taking negative to the resultant angle .Angle lies between [-pi,pi] # print theta # print '........' font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(frame, str(theta), (int(x + 100), int(y - 20)), font, 0.8, (0, 255, 0), 2, cv2.LINE_AA) q = ( x - 320 ) * 0.003351064 #0.003351064 #(x-320)*0.00303077335822 #0.00353077335822ipx along x axis=0.003515625 m r = ( y - 240 ) * 0.003351064 #0.003389831 #(y-240)*0.00302814921311 #0.00352814921311ipx along y axis=0.0034375 m emptyBuff = bytearray() orientation = [0, 0, theta] if z == 0: position = [-q, r, +4.1000e-02] returnCode1, outInts1, path_pos, outStrings1, outBuffer1 = vrep.simxCallScriptFunction( clientID, 'script', vrep.sim_scripttype_childscript, 'shift', [], position, [], emptyBuff, vrep.simx_opmode_blocking) returnCode1, outInts1, path_pos, outStrings1, outBuffer1 = vrep.simxCallScriptFunction( clientID, 'script', vrep.sim_scripttype_childscript, 'rotate', [], orientation, [], emptyBuff, vrep.simx_opmode_blocking) # print '0303030303' else: if flag == 0: if z in freshfruits_id: position = [-q, r, +4.1000e-02] if position[0] > 0: if position[1] > 0: returnCode = vrep.simxSetObjectPosition( clientID, freshfruits[0], -1, position, vrep.simx_opmode_oneshot_wait) if position[1] < 0: returnCode = vrep.simxSetObjectPosition( clientID, freshfruits[1], -1, position, vrep.simx_opmode_oneshot_wait) if position[0] < 0: returnCode = vrep.simxSetObjectPosition( clientID, freshfruits[2], -1, position, vrep.simx_opmode_oneshot_wait) # print returnCode # print r # print -q # print '03030330-------------' if z in damagedfruits_id: position = [-q, r, +4.1000e-02] returnCode = vrep.simxSetObjectPosition( clientID, damagedfruits[l1], -1, position, vrep.simx_opmode_oneshot_wait) # print returnCode # print r # print -q # print '03030330-------------' l1 = l1 + 1 flag = flag + 1 cv2.imshow('frame', frame) if i == t - 1: cap.release() cv2.destroyAllWindows() return
def custom_detect_Aruco( img, n, C): # returns the detected aruco list dictionary with id: corners aruco_list = {} gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if n == 'ARUCO' and C == 'ORIGINAL': aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) elif n == '4': if C == '50': aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) elif C == '100': aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_100) elif C == '250': aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250) elif C == '1000': aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000) elif n == '5': if C == '50': aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_50) elif C == '100': aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_100) elif C == '250': aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250) elif C == '1000': aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000) elif n == '6': if C == '50': aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50) elif C == '100': aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_100) elif C == '250': aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) elif C == '1000': aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000) elif n == '7': if C == '50': aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_50) elif C == '100': aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_100) elif C == '250': aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_250) elif C == '1000': aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_1000) print(aruco_dict) parameters = aruco.DetectorParameters_create( ) # refer opencv page for clarification # lists of ids and the corners beloning to each id print(parameters) corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) # corners is the list of corners(numpy array) of the detected markers. For each marker, its four corners are returned in their original order (which is clockwise starting with top left). So, the first corner is the top left corner, followed by the top right, bottom right and bottom left. # print len(corners), corners, ids print(corners) print(len(corners)) gray = aruco.drawDetectedMarkers(gray, corners, ids) # cv2.imshow('frame',gray) # print (type(corners[0])) if len(corners): # returns no of arucos # print (len(corners)) # print (len(ids)) for k in range(len(corners)): temp_1 = corners[k] temp_1 = temp_1[0] temp_2 = ids[k] temp_2 = temp_2[0] aruco_list[temp_2] = temp_1 return aruco_list
def main(): # First we set up the cameras to start streaming # Define some constants resolution_width = 1280 # pixels resolution_height = 720 # pixels frame_rate = 30 # fps dispose_frames_for_stablisation = 30 # frames # Enable the streams from all the intel realsense devices rs_config = rs.config() rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate) rs_config.enable_stream(rs.stream.infrared, 1, resolution_width, resolution_height, rs.format.y8, frame_rate) rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate) # Use the device manager class to enable the devices and get the frames device_manager = DeviceManager(rs.context(), rs_config) device_manager.enable_all_devices() device_manager._available_devices.sort() device_list = device_manager._available_devices # Allow some frames for the auto-exposure controller to stablise for frame in range(dispose_frames_for_stablisation): frames = device_manager.poll_frames_keep() assert (len(device_manager._available_devices) > 0) #Then we calibrate the images # Get the intrinsics of the realsense device intrinsics_devices = device_manager.get_device_intrinsics(frames) # Set the charuco board parameters for calibration aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) charuco_width = 8 charuco_height = 5 square_length = 0.03425 marker_length = .026 coordinate_dimentions = 3 charuco_board = aruco.CharucoBoard_create(charuco_width, charuco_height, square_length, marker_length, aruco_dict) # Set the charuco board parameters for robot aruco_dict_r = aruco.Dictionary_get(aruco.DICT_4X4_250) charuco_width_r = 3 charuco_height_r = 3 square_length_r = 0.0311 marker_length_r = .0247 charuco_board_robot = aruco.CharucoBoard_create(charuco_width_r, charuco_height_r, square_length_r, marker_length_r, aruco_dict_r) # Set the charuco board parameters for robot aruco_dict_ro = aruco.Dictionary_get(aruco.DICT_5X5_100) charuco_width_ro = 3 charuco_height_ro = 3 square_length_ro = 0.0311 marker_length_ro = .0247 charuco_board_robot_2 = aruco.CharucoBoard_create(charuco_width_ro, charuco_height_ro, square_length_ro, marker_length_ro, aruco_dict_ro) # Decide if you want to use previous calibration or make a new one calibration_decision = input( 'To use previous calibration, press "y". For new calibration press any key \n' ) if calibration_decision != 'y': # Choose amount of frames to average correct_input = False while not correct_input: the_input = input( "Write amount of frames you want to use to calibrate \n") try: amount_frames = int(the_input) if amount_frames > 0: correct_input = True else: print("Frame count has to be positive") except ValueError: print('Input has to be int') # Make a dict to store all images for calibration frame_dict = {} transform_dict = {} rms_dict = {} for from_device in device_list: transform_dict[from_device] = {} rms_dict[from_device] = {} for to_device in device_list: transform_dict[from_device][to_device] = {} rms_dict[from_device][to_device] = np.inf print("Starting to take images in 5 seconds") time.sleep(5) devices_stitched = False while not devices_stitched: print("taking new set of images") for frame_count in range(amount_frames): print("taking image") print(amount_frames - frame_count, "images left") frames = device_manager.poll_frames_keep() time.sleep(0.5) frame_dict[frame_count] = {} for device in device_list: ir_frame = np.asanyarray( frames[device][(rs.stream.infrared, 1)].get_data()) depth_frame = np.asanyarray( post_process_depth_frame( frames[device][rs.stream.depth]).get_data()) frame_dict[frame_count][device] = { 'ir_frame': ir_frame, 'depth_frame': depth_frame } del frames # Make transfer matrices between all possible cameras for idx, from_device in enumerate(device_list[:-1]): for to_device in device_list[idx + 1:]: if to_device != from_device: temp_transform, temp_rms = get_transformation_matrix_wout_rsobject( frame_dict, [from_device, to_device], intrinsics_devices, charuco_board) if temp_rms < rms_dict[from_device][to_device]: rms_dict[from_device][to_device] = temp_rms rms_dict[to_device][from_device] = temp_rms transform_dict[from_device][ to_device] = temp_transform transform_dict[to_device][ from_device] = temp_transform.inverse() #Use Djikstra's to fin shortest path and check if all cameras are connected transformation_matrices = least_error_transfroms( transform_dict, rms_dict) if transformation_matrices != 0: devices_stitched = True # Prints rms error between camera transforms test = matrix_viewer(rms_dict) print(test) # Turns transformation matrices into Transfomation objects transformation_devices = {} for matrix in transformation_matrices: the_matirx = transformation_matrices[matrix] transformation_devices[matrix] = Transformation( the_matirx[:3, :3], the_matirx[:3, 3]) # Saves calibration transforms if the user wants to save_calibration = input( 'Press "y" if you want to save calibration \n') if save_calibration == "y": calibration_name = input saved = False while not saved: name = input( "Type in name of file to save. remember to end name with '.npy' \n" ) try: np.save(name, transformation_matrices) saved = True except: print( "could not save, try another name and remember '.npy' in the end" ) frame_dict.clear() else: correct_filename = False while not correct_filename: file_name = input('Type in calibration file name \n') try: transfer_matirices_save = np.load(file_name, allow_pickle=True) transfer_matrices = transfer_matirices_save[()] correct_filename = True except: print('No such file in directory: "', file_name, '"') transformation_devices = {} for matrix in transfer_matrices: the_matrix = transfer_matrices[matrix] transformation_devices[matrix] = Transformation( the_matrix[:3, :3], the_matrix[:3, 3]) print("Calibration completed...") # Enable the emitter of the devices and extract serial numbers to identify cameras device_manager.enable_emitter(True) key_list = device_manager.poll_frames().keys() while True: visualisation = input( 'Presss "1" for RMS error, "2" for chessboard visualisation and "3" for 3d pointcloud and "4" for robot to camea calibration\n' ) if visualisation == '1': calculate_avg_rms_error(device_list, device_manager, transformation_devices, charuco_board, intrinsics_devices) elif visualisation == '2': visualise_chessboard(device_manager, device_list, intrinsics_devices, charuco_board, transformation_devices) elif visualisation == '3': visualise_point_cloud(key_list, resolution_height, resolution_width, device_manager, coordinate_dimentions, transformation_devices) elif visualisation == '4': robot_calibration = CameraRobotCalibration(transformation_devices, device_manager, charuco_board_robot_2, charuco_board_robot) input("Set target in position for calibration\n") test = robot_calibration.test_functions() print(test) else: print("Input not recognised")
import copy import tf import csv cwd = os.getcwd() fk_ur5 = UR5Kin() ################### Initialisation ################################ camera = 2 # number of cameras rootDir = "/home/zheyu/ur5_calib_toolkit/ZED/" robotYAML = "{}robotCalibration.yaml".format(rootDir) cameraYAML = "cameraCalibration.yaml" extrinsicDataDir = "{}calibDataset/".format(rootDir) tableDataDir = "{}tableCalibDataset/".format(rootDir) fileName = "jointAngles.csv" aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000) # 4X4 = 4x4 bit markers charuco_board = aruco.CharucoBoard_create( 4, 6, 0.063, 0.048, aruco_dict) # width, height (full board), square length, marker length ################################################################## class chArucoCalib: def __init__(self): images = np.array([ extrinsicDataDir + f for f in os.listdir(extrinsicDataDir) if f.endswith(".png") ]) order = np.argsort( [int(p.split(".")[-2].split("_")[-1]) for p in images])
def detectARUCO(self): i = 0 aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() lists = [] idd = "" cv_file = cv2.FileStorage("../Calibration/calib_images/Data.yaml", cv2.FILE_STORAGE_READ) #note we also have to specify the type to retrieve other wise we only get a # FileNode object back instead of a matrix camera_matrix = cv_file.getNode("camera_matrix").mat() dist_matrix = cv_file.getNode("dist_coeff").mat() cap = cv2.VideoCapture(0) # termination criteria criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) objp = np.zeros((6 * 7, 3), np.float32) objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2) # Arrays to store object points and image points from all the images. objpoints = [] # 3d point in real world space imgpoints = [] # 2d points in image plane. images = glob.glob('../Calibration/calib_images/*.jpg') for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find the chess board corners ret, corners = cv2.findChessboardCorners(gray, (7, 6), None) # If found, add object points, image points (after refining them) if ret == True: objpoints.append(objp) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) imgpoints.append(corners2) # Draw and display the corners img = cv2.drawChessboardCorners(img, (7, 6), corners2, ret) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( objpoints, imgpoints, gray.shape[::-1], None, None) while (i < 25): # print(i) ide = "" ret, frame = cap.read() # operations on the frame come here gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #lists of ids and the corners beloning to each id corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) font = cv2.FONT_HERSHEY_SIMPLEX #font for displaying text (below) if np.all(ids != None): rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners[0], 0.05, mtx, dist ) #Estimate pose of each marker and return the values rvet and tvec---different from camera coefficients (rvec - tvec).any() # get rid of that nasty numpy value array error aruco.drawAxis(frame, mtx, dist, rvec[0], tvec[0], 0.1) #Draw Axis aruco.drawDetectedMarkers( frame, corners) #Draw A square around the markers ###### DRAW ID ##### cv2.putText(frame, "Id: " + str(ids), (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA) # Display the resulting frame idd = str(ids) # print ("___________> ids = ",ids) for i in range(len(idd)): if idd[i] != "[" and idd[i] != "]" and idd[ i] != "\n" and idd[i] != " ": if idd[i + 1] != "[" and idd[i + 1] != "]" and idd[ i + 1] != "\n" and idd[i + 1] != " ": ide = str(idd[i]) + str(idd[i + 1]) if int(ide) not in lists: lists.append(int(ide)) if int(idd[i]) not in lists and ide == "": lists.append(int(idd[i])) # print("") cv2.imshow('frame', frame) i = i + 1 lists.sort() cap.release() cv2.destroyAllWindows() return lists
# System information: # - Linux Mint 18.1 Cinnamon 64-bit # - Python 2.7 with OpenCV 3.2.0 import numpy import cv2 from cv2 import aruco import pickle import glob # ChAruco board variables CHARUCOBOARD_ROWCOUNT = 7 CHARUCOBOARD_COLCOUNT = 5 ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_1000) # Create constants to be passed into OpenCV and Aruco methods CHARUCO_BOARD = aruco.CharucoBoard_create(squaresX=CHARUCOBOARD_COLCOUNT, squaresY=CHARUCOBOARD_ROWCOUNT, squareLength=0.051, markerLength=0.0255, dictionary=ARUCO_DICT) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime objectPoints = [] imagePointsLeft = []
# Initialize communication with intel realsense '''pipeline = rs.pipeline() realsense_cfg = rs.config() realsense_cfg.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 0) pipeline.start(realsense_cfg)''' # Check communication ''''print("Test dara source...") try: np.asanyarray(pipeline.wait_for_frames().get_color_frame().get_data()) except: raise Exception("Can't get rgb frame from data source")''' # Define what the calibration board looks like (same as the pdf) board = cv2.aruco.CharucoBoard_create(4, 4, .025, .0125, aruco.Dictionary_get(dictionary)) record_count = 0 # Create two arrays to store the recorded corners and ids all_corners = [] all_ids = [] print("Start recording [1/4]") print( "1. Move the grid from calibration directory a little bit in front of the camera and press [r] to make a record (if auto record is not set to True)" ) print("2. Finish this task and start calculation press [c]") print("3. Interrupt application [ESC]") cap = cv2.VideoCapture(0) scaling_factor = 0.5 while True: # Get frame from realsense and convert to grayscale image
def detect_markers(device_id, hold_info_count=15): cap = cv2.VideoCapture(device_id) # termination criteria criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) objp = np.zeros((6 * 7, 3), np.float32) objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2) # Arrays to store object points and image points from all the images. objpoints = [] # 3d point in real world space imgpoints = [] # 2d points in image plane. print('1. start read calib imgs') images = glob.glob(os.path.join(calib_imgs, '*.png')) if not len(images): print("calib images not exist!!!!!!!!!") return for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find the chess board corners ret, corners = cv2.findChessboardCorners(gray, (7, 6), None) # If found, add object points, image points (after refining them) if ret: objpoints.append(objp) corners2 = cv2.cornerSubPix( gray, corners, (11, 11), (-1, -1), criteria) imgpoints.append(corners2) # Draw and display the corners if enable_gui: img = cv2.drawChessboardCorners(img, (7, 6), corners2, ret) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( objpoints, imgpoints, gray.shape[::-1], None, None) print('2. start detect markers') aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() update_counter = 0 while True: ret, frame = cap.read() # operations on the frame come here gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # lists of ids and the corners beloning to each id corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) font = cv2.FONT_HERSHEY_SIMPLEX # font for displaying text (below) if np.all(ids != None): # print(type(ids), ids) result = [] for id_, corner in zip(ids, corners): rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corner, 0.05, mtx, dist) result.append((id_[0], rvec, tvec)) update_detected(result) update_counter = hold_info_count if enable_gui: # Draw Axis aruco.drawAxis(frame, mtx, dist, rvec[0], tvec[0], 0.1) # Draw A square around the markers aruco.drawDetectedMarkers(frame, corners) cv2.putText(frame, "Id: " + str(ids), (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA) # Display the resulting frame if enable_gui: cv2.imshow('frame', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break # clear update if update_counter <= 0: update_detected([]) update_counter = 0 update_counter -= 1 # When everything done, release the capture cap.release() cv2.destroyAllWindows()
def arucoid(ide, dire, flag): global an print ide, dire while flag: ret, frame = cap.read() #print(frame.shape) #480x640 # Our operations on the frame come here gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_50) #print aruco_dict parameters = aruco.DetectorParameters_create() ''' detectMarkers(...) detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI mgPoints]]]]) -> corners, ids, rejectedImgPoints ''' #lists of ids and the corners beloning to each id corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) #print "im here" frame = aruco.drawDetectedMarkers(frame, corners) if len(corners): #print(corners) corners = corners[0][0] tl = corners[0] bl = corners[3] cv2.circle(frame, tuple(corners[0]), 4, color=(255, 0, 0)) cv2.circle(frame, tuple(corners[3]), 4, color=(0, 255, 0)) #print tl,bl dist_px = np.linalg.norm(tl - bl) #print dist_px distance = (aru_size * focal_len) / dist_px #print distance '''pixel/focal=actual/dist''' ids = ids[0][0] if distance < 1000: if str(ids) == str(ide): #print "im in" #arduino.flush() #arduino.write('4') logger('@rasp found marker id {}'.format(ids)) ''' flagfile=open('flag.txt','w+') flagfile.write('1') flagfile.close()''' #print ard_dire[dire] print ide, dire ard.write(str(ard_dire[dire])) logger('@arduino sent to arduino') # with the port open, the response will be buffered # so wait a bit longer for response here # Serial read section time.sleep(2) msg = ard.readline(ard.inWaiting()) logger('@arduino received {}'.format(msg)) print 'arduino', msg print("found id no : {} \n in distance of : {}".format( ids, distance)) #time.sleep(1) #arduino.write('1') flag = False cv2.putText(frame, str(ids), tuple(corners[2]), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
def cv_exercise7(): frame = cv2.imread('markers.png') gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) height, width = gray.shape deltaX = 0 delyaY = 0 absX = int(width / 2) absY = int(height / 2) print("Corner: ", corners) if len(corners): #returns no of arucos #print (len(corners) aruco_list = {} #print (len(ids)) markerOne = corners[0][0] cornerOne = markerOne[0] cornerTwo = markerOne[1] cornerThree = markerOne[2] cornerFour = markerOne[3] centerX1 = int((cornerTwo[0] + cornerFour[0]) / 2) centerY1 = int((cornerTwo[1] + cornerFour[1]) / 2) centerX2 = int((cornerOne[0] + cornerThree[0]) / 2) centerY2 = int((cornerOne[1] + cornerThree[1]) / 2) centerX = (centerX1 + centerX2) / 2 centerY = (centerY1 + centerY2) / 2 deltaX = abs(absX - centerX) deltaY = abs(absY - centerY) xFOV = (deltaX / width) * 54 yFOV = (deltaY / height) * 41 angle = np.sqrt(xFOV * xFOV + yFOV * yFOV) print("Angle: ", angle) print("CenterX: ", centerX, "CenterY: ", centerY) cv2.imshow("angles", frame) #print("test") # start video capture for distance cap = cv2.VideoCapture(0) while (True): # Capture frame-by-frame ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) #if ids != None: #print("Found ids:") #print(ids) #else: #print("Aruco Marker not found") gray = aruco.drawDetectedMarkers(gray, corners, ids) height, width = gray.shape deltaX = 0 delyaY = 0 absX = int(width / 2) absY = int(height / 2) #print("Corner: ", corners) if len(corners): #returns no of arucos #print (len(corners) aruco_list = {} #print (len(ids)) markerOne = corners[0][0] cornerOne = markerOne[0] cornerTwo = markerOne[1] cornerThree = markerOne[2] cornerFour = markerOne[3] deltaX1 = abs(cornerTwo[0] - cornerOne[0]) deltaX2 = abs(cornerThree[0] - cornerFour[0]) deltaY1 = abs(cornerFour[1] - cornerOne[1]) deltaY2 = abs(cornerThree[1] - cornerTwo[1]) arucoWidth = (deltaX1 + deltaX2) / 2 arucoHeight = (deltaY1 + deltaY2) / 2 #figure out width of screen screenWidth = 0.15 / (arucoWidth / width) screenHeight = 0.15 / (arucoHeight / height) # figure out how many pixels correlates t f = 0.0036 x = 0.0038 y = 0.0029 Z1 = f * (screenWidth / x) Z2 = f * (screenHeight / y) finalDistance = (Z1 + Z2) / 2 print("Distance: ", finalDistance) cv2.imshow('frame', gray) if cv2.waitKey(1) & 0xFF == ord('q'): break
def visionSystem(server, in_q): pass """ Vision functionality for the system (includes AI-yolo5 and aruco marker detection). If the arm is ready to pickup a package, determines what the parcel is (AI) and the postion of said parcel. Using the bounding box (if parcel was detected), gets center of parcel and returns a depth value. Used for TCP values. Args: server.systemStatus (string): Used to determine if the vision system needs to shutdown or go to standby. in_q (Queue): Queue shared between videoStream and the visionSystem (allows for thread-safe communication). Inserts a frame into the queue to send to the user client. Returns: None """ # #Check this function call. # #Add error handling sensor = rs.DepthCamera() ret, depth_frame, color_frame = sensor.get_frames() model, objects, obj_colors = yolov5.create_model('weight_v1.pt') while (server.systemStatus != "Offline"): if (server.readyForTCPValues == True): row_position_average = np.zeros(0) column_position_average = np.zeros(0) id_matrix = np.zeros(20) worldPosition = 0 ret, depth_frame, color_frame = sensor.get_frames() status, depth, bounds, frame = yolov5.detect( model, color_frame, depth_frame, 192, objects, obj_colors) if (status == False): continue #Length is same as moving average length for counter in range(0, 50): gray = cv2.cvtColor(color_frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000) arucoParameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=arucoParameters) frame = aruco.drawDetectedMarkers(color_frame, corners) if (in_q.empty() == True): in_q.put(frame) try: #Set id matrix position to 1 (marker visible) for i in range(0, len(ids)): if (ids[i] > 20): print("") else: id_matrix[ids[i]] = 1 row_position_average, column_position_average, worldPosition = server.findLocation( id_matrix, row_position_average, column_position_average) id_matrix = np.zeros(20) ret, depth_frame, color_frame = sensor.get_frames() except Exception as error: print(error) server.readyForTCPValues == False #Can use simple trig to get an aproximate of the depth. # marker_distance would hold the values for the displacement between the markers and the camera (x-axis) x_distance = server.marker_distance[round( row_position_average)] depth = math.sqrt(pow(depth, 2) - pow(x_distance, 2)) jsonResult = { "first": "Target TCP", "second": (worldPosition[0] / 1000), "third": (worldPosition[1] / 1000), "fourth": (depth / 1000) } server.send(jsonResult, 1) else: gray = cv2.cvtColor(color_frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000) arucoParameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=arucoParameters) frame = aruco.drawDetectedMarkers(color_frame, corners) if (in_q.empty() == True): in_q.put(frame) ret, depth_frame, color_frame = sensor.get_frames()
def main(): face_landmark_path = './shape_predictor_68_face_landmarks.dat' #flags/initializations to be set kalman_flag = 1 skip_frame = 1 skip_frame_to_send = 4 socket_connect = 1 # set to 0 if we are testing the code locally on the computer with only the realsense tracking. simplified_calib_flag = 0 # this is set to 1 if we want to do one-time calibration arucoposeflag = 1 img_idx = 0 # img_idx keeps track of image index (frame #). RSTrSum = np.zeros( (4, 4) ) #initialization of empty buffer for sending the mean of the transformation matrix across every skip_frames_to_send frames skip_frame_reinit = 120 #after every 150 frames, reinitialize detection N_samples_calib = 10 # number of samples for computing the calibration matrix using homography # kalman filter initialization stateMatrix = np.zeros((12, 1), np.float32) # [x, y, delta_x, delta_y] estimateCovariance = np.eye(stateMatrix.shape[0]) transitionMatrix = np.array([[1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]], np.float32) processNoiseCov = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]], np.float32) * 0.001 measurementStateMatrix = np.zeros((6, 1), np.float32) observationMatrix = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]], np.float32) measurementNoiseCov = np.array( [[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]], np.float32) * 1 kalman = KalmanFilter(X=stateMatrix, P=estimateCovariance, F=transitionMatrix, Q=processNoiseCov, Z=measurementStateMatrix, H=observationMatrix, R=measurementNoiseCov) if socket_connect == 1: # create a socket object s = socket.socket() print("Socket successfully created") # reserve a port on your computer in our case it is 2020 but it can be anything port = 2020 s.bind(('', port)) print("socket binded to %s" % (port)) # put the socket into listening mode s.listen(5) print("socket is listening") c, addr = s.accept() print('got connection from ', addr) # dlib face detector dlibdetector = dlib.get_frontal_face_detector() # dlib landmark detector predictor = dlib.shape_predictor('./shape_predictor_68_face_landmarks.dat') if socket_connect == 1 and simplified_calib_flag == 0: arucoinstance = arucocalibclass() ReturnFlag = 1 aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250) marker_len = 0.0645 MLRSTr = arucoinstance.startcamerastreaming(c, ReturnFlag, marker_len, aruco_dict, N_samples_calib) print(MLRSTr) elif socket_connect == 1 and simplified_calib_flag == 1: T_M2_RS = np.array([ -1.0001641, 0.00756584, 0.00479072, 0.03984956, -0.00774137, -0.99988126, -0.03246199, -0.01359556, 0.00453644, -0.03251681, 0.99971441, -0.00428408, 0., 0., 0., 1. ]) T_M2_RS = T_M2_RS.reshape(4, 4) MLRSTr = simplified_calib(T_M2_RS, c) else: MLRSTr = np.array((1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1)) MLRSTr = MLRSTr.reshape(4, 4) print(MLRSTr) # Configure depth and color streams pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # Start streaming profile = pipeline.start(config) align_to = rs.stream.color align = rs.align(align_to) print('Start detecting pose ...') while True: # get video frame frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) color_frame = aligned_frames.get_color_frame() aligned_depth_frame = aligned_frames.get_depth_frame() if not aligned_depth_frame or not color_frame: continue # Intrinsics & Extrinsics of Realsense depth_intrin = profile.get_stream( rs.stream.depth).as_video_stream_profile().get_intrinsics() intr = profile.get_stream( rs.stream.color).as_video_stream_profile().get_intrinsics() depth_to_color_extrin = aligned_depth_frame.profile.get_extrinsics_to( color_frame.profile) mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]]) dist = np.array(intr.coeffs) input_img = np.asanyarray(color_frame.get_data()) img_idx = img_idx + 1 img_h, img_w, _ = np.shape(input_img) # Process the first frame and every frame after "skip_frame" frames if img_idx == 1 or img_idx % skip_frame == 0: gray_img = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) # detect faces using dlib rects = dlibdetector(gray_img, 1) input_imgcopy = input_img for rect in rects: # detect facial landmarks shape = predictor(gray_img, rect) #head pose estimation reprojectdst, rotation_vec, rotation_mat, translation_vec, euler_angle, image_pts = get_head_pose( shape, mtx, dist) # Project a 3D point (0, 0, 1000.0) onto the image plane. #We use this to draw a line sticking out of the nose (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vec, translation_vec, mtx, dist) for p in image_pts: cv2.circle(input_img, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1) # draw line sticking out of the nose tip and showing the head pose p1 = (int(image_pts[0][0]), int(image_pts[0][1])) p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1])) cv2.line(input_img, p1, p2, (255, 0, 0), 2) # convert landmarks detected to numpy type shape = face_utils.shape_to_np(shape) landmarks = np.float32(shape) for (x, y) in landmarks: cv2.circle(input_img, (x, y), 1, (0, 0, 255), -1) #get 3D co-ord of nose depth = aligned_depth_frame.get_distance( image_pts[0][0], image_pts[0][1]) cv2.circle(input_img, (image_pts[0][0], image_pts[0][1]), 3, (0, 255, 0), -1) depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, [image_pts[0][0], image_pts[0][1]], depth) depth_point = np.array(depth_point) depth_point = np.reshape(depth_point, [1, 3]) print("depth_point", depth_point) if (depth_point[0][2] != 0 and depth_point[0][2] < 0.8): if kalman_flag == 0: # or img_idx - img_sent > reinit_thresh or img_idx % skip_frame_reinit == 0: ##print("reset img_sent", img_idx, img_sent) print("reinit", img_idx, skip_frame_reinit) Posarr = np.array([ depth_point[0][0], depth_point[0][1], depth_point[0][2] ]) Posarr = Posarr.reshape(1, 3) print("Posarr", Posarr) r = R.from_euler('zyx', euler_angle, degrees=True) RotMat = r.as_matrix() img_sent = img_idx else: # execute the following if kalman filter to be applied ##print("kalman loop", img_idx, img_sent) print("euler angle", euler_angle, euler_angle[0], euler_angle[1], euler_angle[2]) current_measurement = np.append( depth_point, euler_angle) current_prediction = kalman.predict() current_prediction = np.array(current_prediction, np.float32) current_prediction = current_prediction.transpose()[0] # predicted euler angles euler_angle_P = current_prediction[3:6] # predicted posarr Posarr_P = np.array(current_prediction[:3]).reshape( [1, 3]) # convert to rotation matrix using r function r = R.from_euler('zyx', euler_angle_P, degrees=True) rotation_mat = r.as_matrix() # update the estimate of the kalman filter print("current measurement", current_measurement) #kalman.correct(np.transpose(current_measurement)) kalman.correct( np.transpose( np.reshape(current_measurement, [1, 6]))) Posarr_noKalman = np.array([ depth_point[0][0], depth_point[0][1], depth_point[0][2] ]) depth_point = Posarr_P print("Posarr_P", depth_point, Posarr_noKalman, euler_angle, euler_angle_P) #RSTr is the head pose - combine depth point (which gives the translation, and the rotation to get RSTr) RSTr = np.hstack([rotation_mat, depth_point.transpose()]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]]) print("head pose", RSTr) # compute mean of the head pose every few frames RSTrSum += RSTr if img_idx == skip_frame_to_send: RSTrTosend = RSTrSum / skip_frame_to_send RSTr = RSTrTosend RSTrSum = np.zeros((4, 4)) if arucoposeflag == 1: print("aruco") gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) # set dictionary size depending on the aruco marker selected aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) # detector parameters can be set here (List of detection parameters[3]) parameters = aruco.DetectorParameters_create() parameters.adaptiveThreshConstant = 10 # lists of ids and the corners belonging to each id corners, ids, rejectedImgPoindetectorts = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) # check if the ids list is not empty if np.all(ids != None): # estimate pose of each marker intr = profile.get_stream( rs.stream.color ).as_video_stream_profile().get_intrinsics( ) #profile.as_video_stream_profile().get_intrinsics() mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]]) dist = np.array(intr.coeffs) rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, 0.045, mtx, dist) # for i in range(0, ids.size): # draw axis for the aruco markers aruco.drawAxis(input_img, mtx, dist, rvec[i], tvec[i], 0.1) # draw a square around the markers aruco.drawDetectedMarkers(input_img, corners) #Combine rotation matrix and translation vector to get Aruco pose R_rvec = R.from_rotvec(rvec[0]) R_rotmat = R_rvec.as_matrix() AruRSTr = np.hstack( [R_rotmat[0], tvec[0].transpose()]) AruRSTr = np.vstack([AruRSTr, [0, 0, 0, 1]]) RSTr = AruRSTr print("Aruco pose", AruRSTr) if img_idx % skip_frame_to_send == 0: # Since pose detected in OpenCV will be right handed coordinate system, it needs to be converted to left-handed coordinate system of Unity RSTr_LH = np.array([ RSTr[0][0], RSTr[0][2], RSTr[0][1], RSTr[0][3], RSTr[2][0], RSTr[2][2], RSTr[2][1], RSTr[2][3], RSTr[1][0], RSTr[1][2], RSTr[1][1], RSTr[1][3], RSTr[3][0], RSTr[3][1], RSTr[3][2], RSTr[3][3] ]) # converting to left handed coordinate system RSTr_LH = RSTr_LH.reshape(4, 4) #Compute the transformed pose to be streamed to MagicLeap HeadPoseTr = np.matmul(MLRSTr, RSTr_LH) ArrToSend = np.array([ HeadPoseTr[0][0], HeadPoseTr[0][1], HeadPoseTr[0][2], HeadPoseTr[0][3], HeadPoseTr[1][0], HeadPoseTr[1][1], HeadPoseTr[1][2], HeadPoseTr[1][3], HeadPoseTr[2][0], HeadPoseTr[2][1], HeadPoseTr[2][2], HeadPoseTr[2][3], HeadPoseTr[3][0], HeadPoseTr[3][1], HeadPoseTr[3][2], HeadPoseTr[3][3] ]) ArrToSendPrev = ArrToSend if socket_connect == 1: dataTosend = struct.pack('f' * len(ArrToSend), *ArrToSend) c.send(dataTosend) else: print("No Face Detected") cv2.imshow('Landmark_Window', input_img) key = cv2.waitKey(1)
# System information: # - Linux Mint 18.1 Cinnamon 64-bit # - Python 2.7 with OpenCV 3.2.0 import numpy import cv2 from cv2 import aruco import pickle import glob # ChAruco board variables CHARUCOBOARD_ROWCOUNT = 7 CHARUCOBOARD_COLCOUNT = 5 ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_4X4_100) # Create constants to be passed into OpenCV and Aruco methods CHARUCO_BOARD = aruco.CharucoBoard_create(squaresX=CHARUCOBOARD_COLCOUNT, squaresY=CHARUCOBOARD_ROWCOUNT, squareLength=0.04, markerLength=0.02, dictionary=ARUCO_DICT) # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime # This requires a set of images or a video taken with the camera you want to calibrate # I'm using a set of images taken with the camera with the naming convention: # 'camera-pic-of-charucoboard-<NUMBER>.jpg' # All images used should be the same size, which if taken with the same camera shouldn't be a problem
def track_truck(rp): cap = cv2.VideoCapture(1) w = cap.get(cv2.CAP_PROP_FRAME_WIDTH) h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT) # print w # print h d_pos = 1 #distance difference between cb and truck while True: ret, frame = cap.read() if ret == False: continue framebw = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250) parameters = aruco.DetectorParameters_create() corners, ids, _ = aruco.detectMarkers(framebw, aruco_dict, parameters=parameters) # print ids i = ids.tolist().index([0]) #gives total aruco markers # print i l = 0 l1 = 0 z = int(ids[i][0]) # print z cv2.circle(frame, (corners[i][0][0][0], corners[i][0][0][1]), 5, (125, 125, 125), -1) cv2.circle(frame, (corners[i][0][1][0], corners[i][0][1][1]), 5, (0, 255, 0), -1) cv2.circle(frame, (corners[i][0][2][0], corners[i][0][2][1]), 5, (180, 105, 255), -1) cv2.circle(frame, (corners[i][0][3][0], corners[i][0][3][1]), 5, (255, 255, 255), -1) x = ((corners[i][0][0][0] + corners[i][0][2][0]) / 2) y = ((corners[i][0][0][1] + corners[i][0][2][1]) / 2) cv2.circle(frame, (int(x), int(y)), 5, (255, 0, 0), -1) #center x1 = (corners[i][0][0][0] + corners[i][0][1][0]) / 2 y1 = (corners[i][0][0][1] + corners[i][0][1][1]) / 2 cv2.circle(frame, (int(x1), int(y1)), 5, (0, 0, 255), -1) #midpoint of starting edge cv2.line(frame, (int(x), int(y)), (int(x1), int(y1)), (255, 0, 0), 3) a = y1 - y b = x1 - x theta = 3.14 - math.atan2( a, b ) #taking negative to the resultant angle .Angle lies between [-pi,pi] # print theta # print '........' font = cv2.FONT_HERSHEY_SIMPLEX q = ( x - 320 ) * 0.003351064 #0.003351064 #(x-320)*0.00303077335822 #0.00353077335822ipx along x axis=0.003515625 m r = ( y - 240 ) * 0.003351064 #0.003389831 #(y-240)*0.00302814921311 #0.00352814921311ipx along y axis=0.0034375 m emptyBuff = bytearray() position = [-q, r, +4.1000e-02] orientation = [0, 0, theta] # print position # print orientation x1 = position[0] - rp[0] y1 = position[1] - rp[1] d_pos = math.sqrt((x1)**2 + (y1)**2) if d_pos < 0.015: break cap.release() cv2.destroyAllWindows() return
# marker_center_list.clear() while True: pk_obj.device_get_capture() color_image_handle = pk_obj.capture_get_color_image() depth_image_handle = pk_obj.capture_get_depth_image() if color_image_handle and depth_image_handle: color_image = pk_obj.image_convert_to_numpy(color_image_handle) # cv2.imshow("test", color_image) # cv2.waitKey(0) point_cloud = pk_obj.transform_depth_image_to_point_cloud( depth_image_handle) parameters = aruco.DetectorParameters_create() color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) corners, ids, rejectedImgPoints = aruco.detectMarkers( color_image, dictionary=aruco.Dictionary_get(aruco.DICT_4X4_250), parameters=parameters) if len(corners) == 0: pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() continue image_xy_list = [] for corner_list in corners: image_xy_list.append( np.mean(np.mean(corner_list, axis=0), axis=0).astype(np.int16)) flag = False pcd_pnt_list = [] for image_xy in image_xy_list: pcd_pnt = pk_obj.transform_color_xy_to_pcd_xyz( input_color_image_handle=color_image_handle,
# draw pillars in blue color for i, j in zip(range(4), range(4, 8)): img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3) # draw top layer in red color img = cv2.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3) return img cameraMatrix = np.load('mtx.npy') distCoeffs = np.load('dist.npy') ARUCO_PARAMETERS = aruco.DetectorParameters_create() ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_6X6_250) board = aruco.GridBoard_create(markersX=1, markersY=1, markerLength=0.09, markerSeparation=0.01, dictionary=ARUCO_DICT) # Create vectors we'll be using for rotations and translations for postures rotation_vectors, translation_vectors = None, None axis = np.float32([[-.5, -.5, 0], [-.5, .5, 0], [.5, .5, 0], [.5, -.5, 0], [-.5, -.5, 1], [-.5, .5, 1], [.5, .5, 1], [.5, -.5, 1]]) # Make output image fullscreen cv2.namedWindow('ProjectImage', cv2.WINDOW_NORMAL) pipeline = rs.pipeline() config = rs.config()
def CameraService(): """ Example entry point; please see Enumeration example for more in-depth comments on preparing and cleaning up the system. :return: True if successful, False otherwise. :rtype: bool """ # Retrieve singleton reference to system object system = PySpin.System.GetInstance() # Retrieve list of cameras from the system cam_list = system.GetCameras() num_cameras = cam_list.GetSize() print "Number of cameras detected: %d" % num_cameras # Finish if there are no cameras if num_cameras == 0: # Clear camera list before releasing system cam_list.Clear() # Release system system.ReleaseInstance() print "Not enough cameras!" raw_input("Done! Press Enter to exit...") return False # Run example on each camera for i in range(num_cameras): cam = cam_list.GetByIndex(i) Cam1 = CameraParams(5813.48508684566, 2560.092268747669, 5857.787526106612, 1928.738413521021, 1.0, -0.0862, 0.3082, 0.0, 0.0, 0.0) print "Running example for camera %d..." % i result = run_single_camera(cam) print "Camera %d example complete..." % i # Operations on the frame if result != False: frame = np.array(result.GetData(), dtype="uint8").reshape( (result.GetHeight(), result.GetWidth(), 1)) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( frame, aruco_dict, parameters=parameters) print corners idx_All = np.zeros([4, 1]) Corners_All = np.zeros([4, 4, 2]) if len(ids) == 4: for k in range(4): m = corners[k] idx = ids[k] idx_All[idx - 1, :] = idx idx_All[idx - 1, :] = idx Corners_All[idx - 1, :, :] = m[0, :, :] Pca, Rca = get_object_pose_M( np.reshape(Corners_All[0:4, :, :], [16, 2]), 97.92, 103.49, 103.49, Cam1.camMatrix, Cam1.distCoeff) Rcatmp = np.vstack((np.hstack((Rca, [[0], [0], [0]])), [0, 0, 0, 1])) qca = quaternion_from_matrix(Rcatmp) Rca = quaternion_matrix( [1.0 * qca[1], 1.0 * qca[0], -1.0 * qca[3], -1.0 * qca[2]]) Rca = Rca[0:3, 0:3] Poa, Roa = Map_Camera2Robot(Pca, Rca) Pset = np.matmul(Roa, np.array([-0.3, 0.528, 0.13415 ])) #(-z,x,y)[-0.3,0.538,0.18415] #Rset = [[0.975004629674004,-0.012471665796434,-0.221834239166364],[0.006172005904842,0.999558362226998,-0.029068657437025],[ 0.222098803367709,0.026972913345476,0.974651005995628]] #ZYX Rset = [[ 0.993265901528871, -0.009385243115853, -0.115476257610362 ], [0.009746998214369, 0.999949198262679, 0.002568446930442], [ 0.115446285716984, -0.003676697632625, 0.993306919843344 ]] #ZYX #Rset = [[0.993313684906749, -0.000007686718905 , -0.115446625407115],[0.000280162683008 , 0.999997213667025 , 0.002343963962486],[ 0.115446285716984, -0.002360635317184, 0.993310919357606]] #ZYX Roa = np.matmul(Roa, Rset) Rtmp = np.vstack((np.hstack((Roa, [[0], [0], [0]])), [0, 0, 0, 1])) qoa = quaternion_from_matrix(Rtmp) #tmp = euler_from_matrix(Rtmp) # print tmp # qoa2 = quaternion_from_euler(1.0*tmp[2],1.0*tmp[0],-1.0*tmp[0]) #Roa2 = quaternion_matrix([qoa[3],qoa[0],qoa[1],qoa[2]]) #Roa2 = Roa2[0:3,0:3] print 'Poa:', Poa print 'Roa:', Roa #print 'Roa2:',Roa2 print 'qoa_***:', [qoa[3], qoa[0], qoa[1], qoa[2]] print 'set ponit', Pset Poa = Poa + Pset #print 'qoa2:',[qoa2[3],qoa2[0],qoa2[1],qoa2[2]] # Release reference to camera # NOTE: Unlike the C++ examples, we cannot rely on pointer objects being automatically # cleaned up when going out of scope. # The usage of del is preferred to assigning the variable to None. del cam # Clear camera list before releasing system cam_list.Clear() # Release instance system.ReleaseInstance() #raw_input("Done! Press Enter to exit...") return Poa, [qoa[3], qoa[0], qoa[1], qoa[2]]
def loadArucoDict(requestedDict): #TODO global ARUCO_DICT ARUCO_DICT = aruco.Dictionary_get(aruLibrary[requestedDict])
path = [int("99")] * (2) for raj in range(2*(len(jail))): print("raj changed") if raj<(2*(len(jail))): if raj%2==0: if raj==0: while True: #cap = cv.VideoCapture(1) ret, im = cap.read() cropped = im[y_1:y_2,x_1:x_2] img = cv.resize(cropped, (dim, dim), interpolation=cv.INTER_AREA) aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250) parameters = aruco.DetectorParameters_create() gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) #print(size(corners)) #cap.release() try: print(corners) c1x = (corners[0][0][0][0] + corners[0][0][1][0]) / 2 c2x = (corners[0][0][2][0] + corners[0][0][3][0]) / 2 c1y = (corners[0][0][0][1] + corners[0][0][1][1]) / 2 c2y = (corners[0][0][2][1] + corners[0][0][3][1]) / 2 cenx = (c1x + c2x) / 2 ceny = (c1y + c2y) / 2 print(cenx,ceny) distblue=[-1,-1,-1,-1]
def __init__(self): camera_id = str(rospy.get_param('~camera_id')) color_width = rospy.get_param('~color_width') color_high = rospy.get_param('~color_high') charuco_row = rospy.get_param('~charuco_row') charuco_col = rospy.get_param('~charuco_col') square_length = rospy.get_param('~square_length') marker_length = rospy.get_param('~marker_length') self.cnd = 0 self.frameId = 0 self.pipeline = rs.pipeline() rs_config = rs.config() rs_config.enable_stream(rs.stream.color, color_width, color_high, rs.format.bgr8, 30) self.pipeline.start(rs_config) # Constant parameters used in Aruco methods self.aruco_param = aruco.DetectorParameters_create() self.aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) # Create grid board object we're using in our stream self.charuco_board = aruco.CharucoBoard_create( squaresX=charuco_col, squaresY=charuco_row, squareLength=square_length, markerLength=marker_length, dictionary=self.aruco_dict) # Check for camera calibration data config = ConfigParser.ConfigParser() config.optionxform = str rospack = rospkg.RosPack() self.curr_path = rospack.get_path('hand_eye') config.read(self.curr_path + '/config/camera_' + str(camera_id) + '_internal.ini') internal_name = 'Internal_' + str(color_width) + '_' + str(color_high) b00 = float(config.get(internal_name, "Key_1_1")) b01 = float(config.get(internal_name, "Key_1_2")) b02 = float(config.get(internal_name, "Key_1_3")) b10 = float(config.get(internal_name, "Key_2_1")) b11 = float(config.get(internal_name, "Key_2_2")) b12 = float(config.get(internal_name, "Key_2_3")) b20 = float(config.get(internal_name, "Key_3_1")) b21 = float(config.get(internal_name, "Key_3_2")) b22 = float(config.get(internal_name, "Key_3_3")) self.cameraMatrix = np.mat([[b00, b01, b02], [b10, b11, b12], [b20, b21, b22]]) # c_x = 643.47548083 # c_y = 363.67742746 # f_x = 906.60886808 # f_y = 909.34831447 # k_1 = 0.16962942 # k_2 = -0.5560001 # p_1 = 0.00116353 # p_2 = -0.00122694 # k_3 = 0.52491878 # c_x = 649.007507324219 # c_y = 356.122222900391 # f_x = 922.76806640625 # f_y = 923.262023925781 # self.cameraMatrix = np.array([[f_x, 0, c_x], # [0, f_y, c_y], # [0, 0, 1]]) # self.distCoeffs = np.array([k_1, k_2, p_1, p_2, k_3]) self.distCoeffs = np.array([0.0, 0, 0, 0, 0]) # Create vectors we'll be using for rotations and translations for postures self.rvecs = None self.tvecs = None self.rvecs_arr = np.zeros((3, NUMBER)) self.tvecs_arr = np.zeros((3, NUMBER)) self.cam = None self.QueryImg = None self.init_server() frames = self.pipeline.wait_for_frames() color_frame = frames.get_color_frame() self.QueryImg = np.asanyarray(color_frame.get_data())
import numpy as np import cv2 import cv2.aruco as aruco # Select type of aruco marker (size) aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000) # Create an image from the marker # second param is ID number # last param is total image size for x in list(range(101,131)): img = aruco.drawMarker(aruco_dict, x, 800) cv2.imwrite(str(x)+".jpg", img) # Display the image to us cv2.imshow('frame', img) # Exit on any key cv2.waitKey(0) cv2.destroyAllWindows()
def get(self, img): gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters)
def generateImage( nNumMark, strName, strDstPath = "./generated/", strSrcPath = "./imgs/" ): try: os.makedirs( strDstPath ) except: pass w=2100 h=2970 # ~ A4 proportions white = 255,255,255 grey = 127,127,127 lgrey = 191,191,191 llgrey = 223,223,223 black = 0,0,0 strImgIn = strSrcPath + strName + "." if os.path.exists(strImgIn+"png"): strImgIn += "png" else: strImgIn += "jpg" pic = cv2.imread(strImgIn,cv2.IMREAD_UNCHANGED) # also read potential alpha layer hp, wp, nNbrPlane = pic.shape if nNbrPlane == 4: # set white to pixel containing alpha at 0 pic = rgba2rgb( pic, white ) #~ BGRA[y,x,3] global global_dictionnary_type aruco_dict = aruco.Dictionary_get(global_dictionnary_type) #~ computeStatOnArucoDict(aruco_dict) aru = aruco.drawMarker(aruco_dict,nNumMark, 256) strImgOut = strDstPath + "card_" + strName + ".png" if 0: # reduce image to debug w//=4 h//=4 h_pic_dst = int(h/2.1) # define the final size of the pictures on the page w_pic_dst = (h_pic_dst * wp)//hp if w_pic_dst > w: w_pic_dst = int(w * 0.8) h_pic_dst = (w_pic_dst*hp)//wp xpic = (w-w_pic_dst)//2 ypic = (h-h_pic_dst)//2 w_aru = int(w/3.4) # 3.4 was fine, but seen from too much distance. /3.1 define the final size of the mark on the page h_aru = w_aru xaru = (w-w_aru)//2 rAruMarginCoef = 0.036 # 0.015 yaru = int(0+h*rAruMarginCoef) yaru2 = int(0+h*(1-rAruMarginCoef)-h_aru) im = np.zeros( (h,w,3), dtype=np.uint8 ) im[:] = white pic = cv2.resize( pic, (w_pic_dst, h_pic_dst) ) if 0: # turn mark to grey aru[:] //= 2 aru[:] += 128 aru = cv2.resize( aru, (w_aru, h_aru) ) aru = cv2.cvtColor(aru,cv2.COLOR_GRAY2RGB) im[ypic:h_pic_dst+ypic,xpic:w_pic_dst+xpic]=pic[:] im[yaru:h_aru+yaru,xaru:w_aru+xaru]=aru[:] im[yaru2:h_aru+yaru2,xaru:w_aru+xaru]=aru[:] nMarginBorder = 70 #~ cv2.putText(im, str(nNumMark), (nMarginBorder+20, int(h-nMarginBorder-20) ), cv2.FONT_HERSHEY_SIMPLEX, 1.4, black, 1) cv2.putText(im, str(nNumMark), (w-nMarginBorder-20-140, int(h-nMarginBorder-30) ), cv2.FONT_HERSHEY_SIMPLEX, 1.4, lgrey, 2 ) if 1: nThickness = 44 # 30=> 3mm of plastic border after cut cv2.rectangle(im, (nThickness//2,nThickness//2),(w-1-nThickness//2,h-1-nThickness//2), llgrey, nThickness ) if 1: # too bad redoing that for each image!!! logo = cv2.imread("logo_sbr.jpg") hl,wl,dummy = logo.shape wlf = 400 hlf = (wlf*hl) // wl logo = cv2.resize( logo, (wlf,hlf) ) nAddedMargin = 30 #im[h-hlf-nMarginBorder-nAddedMargin:h-nMarginBorder-nAddedMargin,w-wlf-nMarginBorder-nAddedMargin:w-nMarginBorder-nAddedMargin] = logo im[h-hlf-nMarginBorder-nAddedMargin:h-nMarginBorder-nAddedMargin,nMarginBorder+nAddedMargin+10:nMarginBorder+nAddedMargin+wlf+10] = logo cv2.imwrite(strImgOut, im) if 0: im = cv2.resize( im, (w//4, h//4) ) cv2.imshow("", im) cv2.waitKey(0) return strImgOut
def __init__(self): self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) self.parameters = aruco.DetectorParameters_create()
ap = argparse.ArgumentParser() ap.add_argument("-c", "--camera", required=True, help="path to the input camera") args = vars(ap.parse_args()) # Initialize camera input cap = cv2.VideoCapture(int(args["camera"])) while (True): # Capture frame-by-frame ret, frame = cap.read() # print(frame.shape) #480x640 # Our operations on the frame come here aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() # Detect Markers and print the corners of the found ones corners, ids, rejectedImgPoints = aruco.detectMarkers( frame, aruco_dict, parameters=parameters) print(corners) # Draw the found markers in the image, and show the image. frame = aruco.drawDetectedMarkers(frame, corners) # Perspective transformation if corners != []: warped, M, _ = four_point_transform(frame, corners[0][0], 0.144) cv2.imshow('warped', warped)
def main(): global flFirst """ This functions loads the target surface image, """ homography = None # matrix of camera parameters (made up but works quite well for me) camera_parameters = np.array(mtx) dir_name = os.getcwd() obj2 = OBJ(os.path.join(dir_name, 'models1/rat.obj'), swapyz=True) obj = OBJ(os.path.join(dir_name, 'models1/fox.obj'), swapyz=True) # init video capture cap = cv2.VideoCapture(0) while True: # read the current frame ret, frame = cap.read() frame2 = np.copy(frame) if not ret: print("Unable to capture video") return gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) #res = aruco.detectMarkers(gray, aruco.getPredefinedDictionary(aruco.DICT_6X6_250)) '''for i in range(len(res[0])): print(res[0][i]) rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(res[0][i], 1, mtx, dist) #print(i, rvecs, tvecs) #sm = moveModel(frame, (135, 150, 110), corners) sm = moveModel2(frame, (135, 156, 118), corners, tvecs, rvecs) #frame = aruco.drawDetectedMarkers(frame.copy(), corners, ids)''' if corners: '''for i in range(len(ids)): rvecs, tvecs, _ 1.27= aruco.estimatePoseSingleMarkers(np.array([corners[0][i]]), 1, mtx, dist) #print('----------------------') H = mtx @ (rvecs[0].T @ rvecs[0]) #print(mtx @ (tvecs[0].T @ rvecs[0])) #print(rvecs, tvecs, sep='\n') v = glyp(rvecs, tvecs) #print(v)''' src_pts = np.array([[0, 0], [0, 200], [200, 200], [200, 0]]).reshape(-1, 1, 2) dst_pts = [] avCorn = [[0] * 2 for i in range(4)] k = 0 #print(corners[0][0]) for i in range(len(corners)): k += 1 for j in range(4): avCorn[j][0] += corners[i][0][j][0] avCorn[j][1] += corners[i][0][j][1] #cv2.circle(frame, (corners[i][0][j][0], corners[i][0][j][1]), 1, (255 * (j % 3 == 0),255 * (j % 2 == 0),255), 10) for i in range(len(avCorn)): avCorn[i][0] = avCorn[i][0] // k avCorn[i][1] = avCorn[i][1] // k '''for i in avCorn: cv2.circle(frame, (i[0], i[1]), 1, (255, 0, 125), 10)''' '''for i in corners[0][0]: dst_pts.append([i[0] + sm[0], i[1] + sm[1]])''' rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers( np.array([avCorn]), 1, mtx, dist) rvecsMat, _ = cv2.Rodrigues(rvecs) print('-----------') print(rvecs, tvecs) print(rvecsMat) for i in range(len(avCorn)): avCorn[i][0] = int(avCorn[i][0]) avCorn[i][1] = int(avCorn[i][1]) if flFirst: avCornOld = avCorn.copy() flFirst = False else: for i in range(len(avCorn)): for j in range(2): if abs(avCorn[i][j] - avCornOld[i][j]) <= 5: avCorn[i][j] = avCornOld[i][j] avCornOld = avCorn.copy() sm = moveModel2(frame, (135, 156, 118), np.array(avCorn), tvecs, rvecs) #print(np.array([avCorn])) #print('-----------------') for i in avCorn: dst_pts.append([i[0] + sm[0], i[1] + sm[1]]) dst_pts = np.array(dst_pts).reshape(-1, 1, 2) homography, mask = cv2.findHomography(src_pts, dst_pts) #print(homography) R_T = get_extended_RT(camera_parameters, homography) #print('aaaa',R_T) transformation = camera_parameters.dot(R_T) print(R_T) frame = render(frame, obj, transformation, False) #cv2.imshow('frame2', frame2) #cv2.imshow('mask', mask) cv2.imshow('frame', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows() return 0
""" Connect to the drone and calibrate the camera """ import time import tellopy import av import cv2.cv2 as cv2 # for avoidance of pylint error from cv2 import aruco import numpy as np aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) board = aruco.CharucoBoard_create(7, 5, 1, .8, aruco_dict) def make_chessboard(): """Make a "chess" aruco board, and save to file for printing.""" imboard = board.draw((2000, 2000)) cv2.imwrite('chessboard.tiff', imboard) def calibrate_camera(allCorners, allIds, imsize): """Calibrates the camera using the dected corners.""" print("CAMERA CALIBRATION") cameraMatrixInit = np.array([[1000., 0., imsize[0] / 2.], [0., 1000., imsize[1] / 2.], [0., 0., 1.]]) distCoeffsInit = np.zeros((5, 1)) flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO)
def frame_loop(cap, display=False): """capture aruco Arguments: cap {open cv capture object} -- the opencv device to view Keyword Arguments: display {bool} -- Print debug to stdout and display the OpenCV frame (default: {False}) Returns: All_Objects -- An All_Objects object containing the detected positions of each object """ positions = All_Objects() # Capture frame-by-frame ret, frame = cap.read() #print(frame.shape) # Uncomment to determine dimensions width = frame.shape[1] height = frame.shape[0] # Our operations on the frame come here gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) #print(corners) #print(ids) #It's working. # my problem was that the cellphone put black all around it. The alrogithm # depends very much upon finding rectangular black blobs for count, i in enumerate(corners): #x = (int(corners[i-1][0][0][0]) + int(corners[i-1][0][1][0]) + int(corners[i-1][0][2][0]) + int(corners[i-1][0][3][0])) / 4 #y = (int(corners[i-1][0][0][1]) + int(corners[i-1][0][1][1]) + int(corners[i-1][0][2][1]) + int(corners[i-1][0][3][1])) / 4 #print(x, y) j = i.ravel() print(j) x = (j[0] + j[2] + j[4] + j[6])/4 y = (j[1] + j[3] + j[5] + j[7])/4 if ids[count] == 1: print("monkey") positions.monkey.x=x positions.monkey.y=y elif ids[count] == 2: print("banana") positions.banana.x=x positions.banana.y=y elif ids[count] == 3: print("ramp") positions.ramp.x=x positions.ramp.y=y print(x, y) if display is True: gray = aruco.drawDetectedMarkers(gray, corners) #print(rejectedImgPoints) # Add indicatiors and grid for i in range(1, gridDimensions): cv2.line(gray, ((width/gridDimensions)*i, 0), ((width/gridDimensions)*i, height), (255, 255, 0), 1, 1) for i in range(1, gridDimensions): cv2.line(gray, (0, (height/gridDimensions)*i), (width, (height/gridDimensions)*i), (255, 255, 0), 1, 1) # Display the resulting frame cv2.imshow('frame',gray) return positions
ret2, mtx2, dist2, rvecs2, tvecs2 = cv2.calibrateCamera( objpoints2, imgpoints2, gray2.shape[::-1], None, None) retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate( objpoints1, imgpoints1, imgpoints2, mtx1, dist1, mtx2, dist2, gray1.shape[::-1], criteria) rset = [] tset = [] dists = [] objpoints3 = [] imgpoints3 = [] objpoints4 = [] imgpoints4 = [] ## set dictionary size depending on the aruco marker selected aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) #board = aruco.GridBoard_create(4, 5, markerLength, 0, aruco_dict) for (fname1, fname2) in zip(images3, images4): frame1 = cv2.imread(fname1) frame2 = cv2.imread(fname2) # operations on the frame gray1 = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY) gray2 = cv2.cvtColor(frame2, cv2.COLOR_BGR2GRAY) # detector parameters can be set here (List of detection parameters[3]) parameters = aruco.DetectorParameters_create() #parameters.adaptiveThreshConstant = 10 # lists of ids and the corners belonging to each id
def table_callback(self, rgb_img,ros_cloud): try: rgb_img = self.bridge.imgmsg_to_cv2(rgb_img,'bgr8') gray = cv2.cvtColor(rgb_img, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict, parameters=parameters) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.0001) for corner in corners: cv2.cornerSubPix(gray, corner, winSize = (3,3), zeroZone = (-1,-1), criteria = criteria) frame_markers = aruco.drawDetectedMarkers(rgb_img.copy(), corners, ids) markerLength = 0.8 flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO) rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, markerLength, cameramatrix, dist_coeffs) # result_matrix = np.zeros(shape=[4,4]) # rotation_matrix = eulerAnglesToRotationMatrix(rvecs[0][0]) # result_matrix[:3,:3] = rotation_matrix # result_matrix[:3,3] = tvecs[0] # result_matrix[3,3] = 1. length_of_axis = 0.6 total_center = np.zeros(shape = [4,1,2]) for index in range(len(ids)): center = np.array(np.mean(corners[index][0],axis = 0), dtype = np.int) total_center[ids[index]-1] = center imaxis = aruco.drawDetectedMarkers(rgb_img.copy(), corners, ids) for i in range(len(tvecs)): imaxis = aruco.drawAxis(imaxis, cameramatrix, dist_coeffs, rvecs[i], tvecs[i], length_of_axis) # Display the resulting frame self.ar_pub.publish(self.bridge.cv2_to_imgmsg(imaxis,'bgr8')) print('height: {}'.format(ros_cloud.height)) print('width: {}'.format(ros_cloud.width)) ## get only 1 point data from point clouds ## uvs must be iterable, so use 2d list. pose_array = PoseArray() for point_i in range(len(ids)): pixel_x = int(total_center[point_i][0][0]) pixel_y = int(total_center[point_i][0][1]) for p in pc2.read_points(ros_cloud, field_names = ("x", "y", "z"), uvs=[[pixel_x,pixel_y]]): # print('{} p.x: {}'.format(ids[point_i],p[0])) # print('{} p.y: {}'.format(ids[point_i],p[1])) # print('{} p.z: {}'.format(ids[point_i],p[2])) pose_msg = Pose() tx, ty,tz = transform_coor(p[0],p[1],p[2]) pose_msg.position.x = tx pose_msg.position.y = ty pose_msg.position.z = tz pose_msg.orientation.x = 0 pose_msg.orientation.y = 0 pose_msg.orientation.z = 0 pose_msg.orientation.w = ids[point_i] pose_array.poses.append(pose_msg) self.pose_pub.publish(pose_array) # When everything done, release the capture except TypeError as e : print(e) ## get only 1 point data from point clouds ## uvs must be iterable, so use 2d list. pose_array = PoseArray() for point_i in range(len(ids)): pixel_x = int(total_center[point_i][0][0]) pixel_y = int(total_center[point_i][0][1]) for p in pc2.read_points(ros_cloud, field_names = ("x", "y", "z"), uvs=[[pixel_x,pixel_y]]): # print('{} p.x: {}'.format(ids[point_i],p[0])) # print('{} p.y: {}'.format(ids[point_i],p[1])) # print('{} p.z: {}'.format(ids[point_i],p[2])) pose_msg = Pose() tx, ty,tz = transform_coor(p[0],p[1],p[2]) pose_msg.position.x = tx pose_msg.position.y = ty pose_msg.position.z = tz pose_msg.orientation.x = 0 pose_msg.orientation.y = 0 pose_msg.orientation.z = 0 pose_msg.orientation.w = ids[point_i] pose_array.poses.append(pose_msg) self.pose_pub.publish(pose_array) # When everything done, release the capture except TypeError as e : print(e)