def aruco_read(img_copy): """ Função que processa uma imagem, buscando os QR codes. Retorna: img_copy: copia da imagem da camera do robo, com o contorno dos QRcodes desenhados por cima, e com os IDs dos QRcodes sobrescritos na imagem. ids: lista dos IDs dos QRcodes, que a funcao esta detectando na imagem. """ aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250) # parameters = aruco.DetectorParameters_create() # parameters.minDistanceToBorder = 0 # parameters.adaptiveThreshWinSizeMax = 1000 gray = cv2.cvtColor(img_copy, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict) #, parameters=parameters) # if ids is None: # return None # for i in range(len(ids)): # print('ID: {}'.format(ids[i])) # for c in corners[i]: # for canto in c: # print("Corner {}".format(canto)) aruco.drawDetectedMarkers(img_copy, corners, ids) return img_copy, ids
def detect_Aruco( img): #returns the detected aruco list dictionary with id: corners aruco_list = {} gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.getPredefinedDictionary( aruco.DICT_4X4_1000 ) #aruco.Dictionary_get(aruco.DICT_4X4_50) #creating aruco_dict with 5x5 bits with max 250 ids..so ids ranges from 0-249 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 example_calibrate_folder(folder_in, folder_out,marker_length_mm = 3.75, marker_space_mm = 0.5,dct = aruco.DICT_6X6_1000): tools_IO.remove_files(folder_out) num_cols, num_rows = 4, 5 board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct)) filenames = numpy.unique(tools_IO.get_filenames(folder_in, '*.jpg,*.png'))[:3] counter, corners_list, id_list, first = [], [], [], True for filename_in in filenames: base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1] image = cv2.imread(folder_in + filename_in) img_gray = cv2.cvtColor(image,cv2.COLOR_RGB2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers(img_gray, aruco.getPredefinedDictionary(dct)) if first == True: corners_list = corners id_list = ids first = False else: corners_list = numpy.vstack((corners_list, corners)) id_list = numpy.vstack((id_list,ids)) counter.append(len(ids)) image_temp = tools_image.desaturate(image.copy()) aruco.drawDetectedMarkers(image_temp, corners) cv2.imwrite(folder_out+base_name+'.png',image_temp) print(base_name) counter = numpy.array(counter) ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners_list, id_list, counter, board, img_gray.shape, None, None ) print(camera_matrix) return
def __init__(self): global take_off # --- Get the camera calibration path self.twist = Twist() self.workspace_path = '/home/vishal/vishal_testing' self.camera_matrix = np.loadtxt( self.workspace_path + '/drone-simulation-ws/src/drone_control/src/cameraMatrix.txt', delimiter=',') self.camera_distortion = np.loadtxt( self.workspace_path + '/drone-simulation-ws/src/drone_control/src/cameraDistortion.txt', delimiter=',') # --- 180 deg rotation matrix around the x axis self.r_flip = np.zeros((3, 3), dtype=np.float32) self.r_flip[0, 0] = 1.0 self.r_flip[1, 1] = -1.0 self.r_flip[2, 2] = -1.0 # --- Define the aruco dictionary self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL) self.parameters = aruco.DetectorParameters_create() # --- Capture the camera video (this may also be a video or a picture) self.capture = cv_bridge.CvBridge() # -- Font for the text in the image self.font = cv2.FONT_HERSHEY_PLAIN self.height = 0 self.downward = 0 self.upword = 0 self.height_sub = rospy.Subscriber('/sonar_height', Range, callback=self.height_callback) self.camera_image_sub = rospy.Subscriber('/bottom/camera/image', Image, queue_size=5, callback=self.main) self.drone_status_pub = rospy.Publisher('land', String, queue_size=1) self.velocity_command_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
def startWebCam(cameraMatrix, distCoeffs,arucoSquareDimension): markerCorners=[] #markerCorners=[] markerDictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) cap = cv2.VideoCapture(cam_1_no) while(1): _,frame = cap.read() frame,new_matrix_1,_=undistortImage(frame,cam_matrix_1,distortion_coefficients_1) markerCorners,markerID, rejectedCandidates= aruco.detectMarkers(frame,markerDictionary) r_vect,t_vect, _ =aruco.estimatePoseSingleMarkers(markerCorners,arucoSquareDimension,cameraMatrix,distCoeffs) if(markerID is not None): for i in range(len(markerID)): #aruco.draw aruco.drawAxis(frame,cameraMatrix,distCoeffs,r_vect[i],t_vect[i],0.1) aruco.drawDetectedMarkers(frame,markerCorners) # print("Corners",markerCorners[0][0]) #focalLength = (abs(markerCorners[0][0][1][0]-markerCorners[0][0][2][0]) * KNOWN_DISTANCE) / KNOWN_WIDTH #print("Focal length: ",focalLength) print("Length: ",distance_to_camera(KNOWN_WIDTH,FOCAL,abs(markerCorners[0][0][1][0]-markerCorners[0][0][2][0]))) cv2.imshow('Webcam',frame) if(cv2.waitKey(30)>=0): break return 1
def __init__(self, squaresX, squaresY, square_length, marker_length, marker_bits=4, dict_size=50, aruco_dict=None, manually_verify=False): self.squaresX = squaresX self.squaresY = squaresY self.square_length = square_length self.marker_length = marker_length self.manually_verify = manually_verify dkey = (marker_bits, dict_size) self.dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey]) self.board = aruco.CharucoBoard_create(squaresX, squaresY, square_length, marker_length, self.dictionary) total_size = (squaresX - 1) * (squaresY - 1) objp = np.zeros((total_size, 3), np.float64) objp[:, :2] = np.mgrid[0:(squaresX - 1), 0:(squaresY - 1)].T.reshape(-1, 2) objp *= square_length self.objPoints = objp self.empty_detection = np.zeros((total_size, 1, 2)) * np.nan self.total_size = total_size
def identifica_id(cv_image): gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # --- Define the aruco dictionary aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250) # parameters = aruco.DetectorParameters_create() # parameters.minDistanceToBorder = 0 # parameters.adaptiveThreshWinSizeMax = 1000 corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict) # , parameters=parameters) aruco.drawDetectedMarkers(cv_image, corners, ids) if ids is None: ids = [[0]] centro = (0, 0) else: ids = ids.tolist() corners = corners[0][0] media_x = int( (corners[0][0] + corners[1][0] + corners[2][0] + corners[3][0]) / 4) media_y = int( (corners[0][1] + corners[1][1] + corners[2][1] + corners[3][1]) / 4) centro = (media_x, media_y) return centro, ids[0][0], ids[0]
def get_calibration_board(config): board_size = config['calibration']['board_size'] board_type = config['calibration']['board_type'].lower() if board_type in ['aruco', 'charuco']: dkey = (config['calibration']['board_marker_bits'], config['calibration']['board_marker_dict_number']) dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey]) if board_type == 'aruco': board = aruco.GridBoard_create( board_size[0], board_size[1], config['calibration']['board_marker_length'], config['calibration']['board_marker_separation_length'], dictionary) elif board_type == 'charuco': board = aruco.CharucoBoard_create( board_size[0], board_size[1], config['calibration']['board_square_side_length'], config['calibration']['board_marker_length'], dictionary) elif board_type == 'checkerboard': board = Checkerboard(board_size[0], board_size[1], config['calibration']['board_square_side_length']) else: raise ValueError( "board_type should be one of " "'aruco', 'charuco', or 'checkerboard' not '{}'".format( board_type)) return board
def __init__(self, marker_size, camera_matrix, camera_distortion): #The CvBridge command is only needed if the images are coming in via ROS #self.bridge = CvBridge() self.marker_size = marker_size self.camera_matrix = camera_matrix self.camera_distortion = camera_distortion self.R_flip = np.zeros((3, 3), dtype=np.float32) self.R_flip[0, 0] = 1.0 self.R_flip[1, 1] = -1.0 self.R_flip[2, 2] = -1.0 #The following commands establish which Aruco dictionary will be in use. self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_100) self.parameters = aruco.DetectorParameters_create() #initialize video stream self.vid = cv2.VideoCapture(0) #initialize an empty frame within the class self.frame = np.zeros([960, 1280, 3], dtype=np.uint8) self.gray = np.zeros([960, 1280, 3], dtype=np.uint8) self.font = cv2.FONT_HERSHEY_PLAIN
def create_board(cor, a_id): board_corners = [np.array(cor, dtype=np.float32)] board_ids = np.array([[a_id]], dtype=np.int32) board = aruco.Board_create( board_corners, aruco.getPredefinedDictionary(aruco.DICT_5X5_250), board_ids) return board
def main(): # establish global constants (hardcode in file or make optional param to fn call) # KNOWN_WIDTH is the width of the AR tags to detect in URC competition KNOWN_WIDTH = 20 # unit is cm # initialize the camera and grab a reference to the raw camera capture camera = PiCamera() camera.resolution = (640, 480) camera.framerate = 32 rawCapture = PiRGBArray(camera, size=(640, 480)) # allow the camera to warmup time.sleep(2) # capture frames from the camera for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): # print("Captured Image") # grab the raw NumPy array representing the image image = np.array(frame.array) # Our operations on the frame come here gray_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) #TODO: change "cv2.aruco.DICT_5x5" to rover aruco lib aruco_dict = aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_50) parameters = aruco.DetectorParameters_create() # print("Parameters:", parameters) # Lists of ids and the corners belonging to each id corners, ids, rejectedImgPoints = aruco.detectMarkers( gray_img, aruco_dict, parameters=parameters) # print("Corners:", corners) # print(type(id)) if (len(corners) > 0): for i in range(len(corners)): # used to be id # call function to calculate ar tag heading relative to rover depth = get_depth(i, corners[i]) #used to be id[i] heading = global_coord_trans(i, corners[i], depth) # id[i] # this is where we would send heading over LCM channel # note: global coord trans could happen somewhere # outside of this script. It doesn't, but it could print("Depth and Heading of", i, "are:", depth, "&", heading) gray_img = aruco.drawDetectedMarkers(gray_img, corners) #print(rejectedImgPoints) # Display the resulting frame cv2.imshow('frame', gray_img) if cv2.waitKey(1) & 0xFF == ord('q'): break rawCapture.truncate(0) # When everything done, release the capture # cap.release() cv2.destroyAllWindows() return
def callback(data): global theID, thePoint, cam_tf, cam_pub ####for real tello #camera_matrix = np.array([[921.170702, 0.000000, 459.904354], [ # 0.000000, 919.018377, 351.238301], [0.000000, 0.000000, 1.000000]]) #dist_coeffs = np.array( # [-0.033458, 0.105152, 0.001256, -0.006647, 0.000000]) ####for sim camera_matrix = np.array([[562, 0.000000, 480.5], [0.000000, 562, 360.5], [0.000000, 0.000000, 1.000000]]) dist_coeffs = np.array([0, 0, 0, 0, 0]) aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) board_corners = [np.array(thePoint, dtype=np.float32)] board_ids = np.array([theID], dtype=np.int32) board = aruco.Board_create( board_corners, aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL), board_ids) i = 0 corners = [] ids = np.array([data.id], dtype=np.int32) corners = np.array([[[data.points[0].x, data.points[0].y], [data.points[1].x, data.points[1].y], [data.points[2].x, data.points[2].y], [data.points[3].x, data.points[3].y]]], dtype=np.float32) retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, camera_matrix, dist_coeffs, None, None) if retval == 0: return 0, 0, 0 a = rvec b = tvec rr, _ = cv2.Rodrigues(np.array([a[0][0], a[1][0], a[2][0]])) tt = np.array([b[0][0], b[1][0], b[2][0] + 0.21]) cam_r = rr.transpose() cam_t = -cam_r.dot(tt) cam_x = cam_t[0] cam_y = cam_t[1] cam_z = cam_t[2] cam_q = quaternion_from_matrix(cam_r) tq = [-0.5, 0.5, 0.5, 0.5] q3 = qmulq(tq, cam_q) cam_tf.sendTransform((cam_x, cam_y, cam_z), cam_q, rospy.Time.now(), "Tello_cam_from_marker" + str(theID), "world") cam_tf_t.sendTransform((0, 0, 0), (0.5, -0.5, 0.5, 0.5), rospy.Time.now(), "Tello_cam_from_marker_world" + str(theID), "Tello_cam_from_marker" + str(theID)) pubmsg = PoseStamped() pubmsg.pose.position.x = cam_x pubmsg.pose.position.y = cam_y pubmsg.pose.position.z = cam_z pubmsg.pose.orientation.w = q3[3] pubmsg.pose.orientation.x = q3[0] pubmsg.pose.orientation.y = q3[1] pubmsg.pose.orientation.z = q3[2] pubmsg.header = data.header pubmsg.header.frame_id = "world" cam_pub.publish(pubmsg)
def __init__(self, videoPort, cameraMatrix, distortionCoefficients): #カメラ読み込み self.cap = cv2.VideoCapture(videoPort) #カメラの内部パラメータ読み込み self.cameraMatrix = np.load(cameraMatrix) self.distortionCoefficients = np.load(distortionCoefficients) #arucoの辞書読み込み self.dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
def __init__(self): self._cap = cv2.VideoCapture(VIDEO_SOURCE_ID) self._dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) self._font = cv2.FONT_HERSHEY_SIMPLEX rospy.Subscriber("/neato01/pose", PoseStamped, self.tracked_neato01, queue_size=10) cv2.namedWindow('frame', cv2.WINDOW_NORMAL) cv2.resizeWindow('frame', 1280, 720) self._start_time = time.time()
def detect_Aruco(image): img=image aruco_list = {} gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_250) parameters = aruco.DetectorParameters_create() _, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters) return ids
def configure_rigid_bodies(configuration): """ reads configuration and creates a list of rigid bodies together with a list of dictionaries used. """ dictionary_name = configuration.get("aruco dictionary", 'DICT_4X4_50') try: ar_dictionary_name = getattr(aruco, dictionary_name) except AttributeError: raise ImportError(('Failed when trying to import {} from cv2.' 'aruco. Check dictionary exists.' ).format(dictionary_name)) from AttributeError ar_dicts = [] ar_dict_names = [] ar_dicts.append(aruco.getPredefinedDictionary(ar_dictionary_name)) ar_dict_names.append(dictionary_name) rigid_bodies = [] rigid_body_configs = configuration.get('rigid bodies', []) for rigid_body_config in rigid_body_configs: rigid_body = ArUcoRigidBody(rigid_body_config.get('name', 'no name')) filename = rigid_body_config.get('filename', None) if filename is None: raise ValueError('rigid body configuration must include filename') dictionary_name = rigid_body_config.get('aruco dictionary', 'DICT_ARUCO_ORIGINAL') try: ar_dictionary_name = getattr(aruco, dictionary_name) except AttributeError: raise ImportError(('Failed when trying to import {} from cv2.' 'aruco. Check dictionary exists.' ).format(dictionary_name)) from AttributeError rigid_body.load_3d_points(filename, dictionary_name) tag_width = rigid_body_config.get('tag width', None) if tag_width is not None: rigid_body.scale_3d_tags(tag_width) rigid_bodies.append(rigid_body) if dictionary_name not in ar_dict_names: ar_dict_names.append(dictionary_name) ar_dicts.append(aruco.getPredefinedDictionary(ar_dictionary_name)) return ar_dicts, ar_dict_names, rigid_bodies
def show_camera(self): flag, self.image = self.cap.read() if self.aruco_flag is True: if self.camera_matrix is None: error_dialog = QErrorMessage() error_dialog.showMessage( 'Oh no you do not have load the matrix\n' 'Please sure you have clicked the load mtx button\n' 'Program will close in 1 sec\n') error_dialog.exec_() exit() # load the matrix in Camera Matrix aruco_dict = aruco.getPredefinedDictionary( aruco.DICT_ARUCO_ORIGINAL) parameters = aruco.DetectorParameters_create() gray = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY) ret, out1 = cv2.threshold(gray, 90, 255, cv2.THRESH_BINARY) corners, ids, rejected = aruco.detectMarkers( image=out1, dictionary=aruco_dict, parameters=parameters, cameraMatrix=self.camera_matrix, distCoeff=self.camera_distortion) if ids is not None: # -- Find the marker's 6-DOF pose rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers( corners, 140, self.camera_matrix, self.camera_distortion) # print(rvecs) for i in range(rvecs.shape[0]): aruco.drawAxis(self.image, self.camera_matrix, self.camera_distortion, rvecs[i, :, :], tvecs[i, :, :], 100) aruco.drawDetectedMarkers(self.image, corners) id_str = "id:{}".format(ids[0][0]) rvecs_str = "rvecs:Rx: {:.2f},Ry: {:.2f},Rz: {:.2f}".format( rvecs[0][0][0], rvecs[0][0][1], rvecs[0][0][2]) tvecs_str = "tvecs:Tx: {:.3f},Ty: {:.3f},Tz: {:.3f}".format( tvecs[0][0][0], tvecs[0][0][1], tvecs[0][0][2]) center_point_str = f"center point {corners}" # cv2.putText(self.image, id_str, (0, 30), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5) # cv2.putText(self.image, rvecs_str, (0, 130), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5) # cv2.putText(self.image, tvecs_str, (0, 230), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5) # cv2.putText(self.image, center_point_str, (0, 330), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 255, 0), 5) self.tvec_label.setText(tvecs_str) self.rvec_label.setText(rvecs_str) else: pass show = cv2.resize(self.image, (1920, 1080)) show = cv2.cvtColor(show, cv2.COLOR_BGR2RGB) showImage = QImage(show.data, show.shape[1], show.shape[0], QImage.Format_RGB888) self.label_show_camera.setPixmap(QPixmap.fromImage(showImage)) self.label_show_camera.resize(self.widget.size()) self.label_show_camera.setScaledContents(True)
def createArucoMarkers(): outputMarker = [] markerDictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) for i in range(50): outputMarker = aruco.drawMarker(markerDictionary, i, 500) imageName = "4x4Marker_" + str(i) + ".jpg" #f=open(imageName,"w") cv2.imwrite(imageName, outputMarker)
def test_get_dictionary(self): """ compare public attributes of dictionary to confirm equality """ err_msg = "get_dictionary failed" dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) self.assertTrue( FiducialMarker.dictionary_equal(dictionary, FiducialMarker.get_dictionary()))
def detecter(): cap = cv2.VideoCapture(2) Setcamera(cap) cameraMartix = np.array([[720.2120, 2.0761, 335.1827], [0, 721.1528, 268.9376], [0, 0, 1]]) distCoeffs = np.array([-0.1622, 0.1400, 0.00234976, -0.0008216553]) dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250) pub = rospy.Publisher('visual', Visual_msg, queue_size=10) rospy.init_node('detecter', anonymous=True) rate = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): ret, frame = cap.read() img = frame corners, ids, reject = aruco.detectMarkers(frame, dict) if ids is not None: img = aruco.drawDetectedMarkers(img, corners, ids) r, t = aruco.estimatePoseSingleMarkers(corners, 0.95, cameraMartix, distCoeffs) position = xyz(r, t) # rospy.loginfo("r:\n",r) # rospy.loginfo("t:\n",t) for i, id in enumerate(ids): img = aruco.drawAxis(img, cameraMartix, distCoeffs, r[i], t[i], 1) msg = Visual_msg() msg.id = id marker_r = r[i][0] marker_t = t[i][0] msg.rvec.x = marker_r[0] msg.rvec.y = marker_r[1] msg.rvec.z = marker_r[2] msg.tvec.x = position[0] msg.tvec.y = position[1] msg.tvec.z = position[2] rospy.loginfo("detect marker") pub.publish(msg) rate.sleep() cv2.imshow("img", img) cv2.waitKey(1) else: cv2.imshow("img", frame) cv2.waitKey(1) msg = Visual_msg() msg.id = 100 msg.rvec.x = 0 msg.rvec.y = 0 msg.rvec.z = 0 msg.tvec.x = 0 msg.tvec.y = 0 msg.tvec.z = 0 rospy.loginfo("detect nothing") rospy.loginfo(msg) pub.publish(msg)
def main_boi(): while (True): ret, frame = cap.read() #gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.getPredefinedDictionary(0) parameters = aruco.DetectorParameters_create() # draw boundary cv2.rectangle(frame, (50, 50), (1230, 670), (255, 0, 0), 2) corners, ids, rejectedImgPoints = aruco.detectMarkers( frame, aruco_dict, parameters=parameters) for box_index in range(len(corners)): x_avg = get_avg_coord(corners[box_index][0])[0] y_avg = get_avg_coord(corners[box_index][0])[1] cv2.rectangle(frame, (int(x_avg), int(y_avg)), (int(x_avg + 5), int(y_avg + 5)), (0, 255, 0), 2) # if start if ids[box_index][0] == start_id: start = corners[box_index][0] print "start: " + str(get_avg_coord(start)) # if goal elif ids[box_index][0] == goal_id: goal = corners[box_index][0] print "goal: " + str(get_avg_coord(goal)) # if obs for i in range(len(obs_id)): if ids[box_index][0] in obs_id: obs[i] = get_avg_coord(corners[box_index][0]) print "obs: " + str(obs) # if robot for i in range(len(robot_id)): if ids[box_index][0] in robot_id: robot[i] = get_avg_coord(corners[box_index][0]) print "robot: " + str(robot) cv2.putText( frame, "id=" + str(ids[box_index][0]) + " (" + str(int(x_avg)) + "," + str(int(y_avg)) + ")", (int(x_avg), int(y_avg)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) gray = aruco.drawDetectedMarkers(frame, corners) draw_grid(frame, num_rows, num_cols, corners, ids) # Display the resulting frame cv2.imshow('frame', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
def detect_aruco(self): self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250) image = self.image_repository.get_next_image() gray = cv2.cvtColor(image.frame, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, self.aruco_dict, parameters=aruco.DetectorParameters_create()) return corners
def init(debug=False): global sock sock = SocketClient(timeout=1) global capture capture = CaptureThread(debug) capture.start() global markerDict markerDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
def aruco_detect1(): aruco_list = {} rgb = image1[...,::-1] gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB) aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_1000)#aruco.Dictionary_get(aruco.DICT_4X4_50) #creating aruco_dict with 5x5 bits with max 250 ids..so ids ranges from 0-249 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 img = cv2.imread(path_to_image) #give the name of the image with the complete path id_aruco_trace = 0 det_aruco_list = {} img2 = img[0:x,0:y,:] #separate out the Aruco image from the whole image det_aruco_list = detect_Aruco(img2) if det_aruco_list: img3 = mark_Aruco(img2,det_aruco_list) id_aruco_trace = calculate_Robot_State(img3,det_aruco_list) print(id_aruco_trace) cv2.imshow('image',img2) cv2.waitKey(0) cv2.destroyAllWindows()
def show_aruco(self): aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250) image = aruco.drawMarker(aruco_dict, 3, 700) photo = ImageTk.PhotoImage(image=PIL.Image.fromarray(image)) self.projector_window.canvas.create_image(500, 400, image=photo) return np.array( [[150, 50, 0], [850, 50, 0], [850, 750, 0], [150, 750, 0]], dtype=np.float32)
def gen_aruco(n): while(n): img = np.zeros((100,100,3), np.uint8) Dictionary = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL) img=aruco.drawMarker(Dictionary,n,100,img,1) #cv2.imwrite('C:\\Users\\ailab\\PycharmProjects\\drone\\train\\'+str(n)+'.jpg',img) cv2.imwrite('C:\\Users\\ailab\\PycharmProjects\\drone\\train\\class_0\\' + 'aruco_0_'+str(n) + '.jpg', img) print('gen_aruco: '+'aruco_0_'+str(n) + '.jpg') n = n - 1 return 0
def get_markers(frame): '''Função que pega os marcadores da frame passada como argumento e retorna três listas corners (com as coordenadas de cada ID detectado), ids (com o ID detectado) e rejectedImgPoints (com as coordenadas rejeitada)''' aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( frame, aruco_dict, parameters=parameters) return corners, ids, rejectedImgPoints
def example_calibrate_aruco_markers(filename_in, marker_length_mm = 3.75, marker_space_mm = 0.5, dct = aruco.DICT_6X6_1000): image = cv2.imread(filename_in) gray = tools_image.desaturate(image) scale = (marker_length_mm / 2, marker_length_mm / 2, marker_length_mm / 2) num_cols, num_rows = 4,5 board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct)) image_AR, image_cube = gray.copy(), gray.copy() base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1] #board_width_px = int((num_cols * marker_length_mm + (num_cols - 1) * marker_space_mm)) #board_height_px= int((num_rows * marker_length_mm + (num_rows - 1) * marker_space_mm)) #image_board = aruco.drawPlanarBoard(board, (board_width_px, board_height_px)) camera_matrix = None#tools_pr_geom.compose_projection_mat_3x3(image.shape[1], image.shape[0]) corners, ids, _ = aruco.detectMarkers(cv2.cvtColor(image, cv2.COLOR_BGR2GRAY), aruco.getPredefinedDictionary(dct)) if len(corners)>0: if len(corners)==1:corners,ids = numpy.array([corners]),numpy.array([ids]) else:corners, ids = numpy.array(corners), numpy.array(ids) counters = numpy.array([len(ids)]) ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners,ids,counters, board, gray.shape[:2],None, None) image_markers = [tools_image.saturate(aruco.drawMarker(aruco.getPredefinedDictionary(dct), id, 100)) for id in ids] #!!! #camera_matrix = tools_pr_geom.compose_projection_mat_3x3(4200,4200) for i,image_marker in enumerate(image_markers): rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners[i], marker_length_mm, camera_matrix, numpy.zeros(5)) image_AR = tools_render_CV.draw_image(image_AR,image_marker, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(), numpy.array(tvecs).flatten(),scale) image_cube = tools_render_CV.draw_cube_numpy(image_cube, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(),numpy.array(tvecs).flatten(), scale) cv2.imwrite(folder_out + base_name+'_AR.png', image_AR) cv2.imwrite(folder_out + base_name+'_AR_cube.png', image_cube) print(camera_matrix) return camera_matrix
def create_markers(): j = 2 markers_dict_1 = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) markers_dict1 = custom_dictionary_from(j, 6, markers_dict_1) markers_dict_2 = aruco.getPredefinedDictionary(aruco.DICT_7X7_50) markers_dict2 = custom_dictionary_from(j, 7, markers_dict_2) markers1 = list( aruco.drawMarker(markers_dict1, i + 1, 200) for i in range(1)) markers2 = list( aruco.drawMarker(markers_dict2, i + 1, 200) for i in range(1)) for i in range(len(markers1)): cv2.imwrite( os.path.join('markers', 'marker6_') + str(i + 1) + '.png', markers1[i]) for i in range(len(markers2)): cv2.imwrite( os.path.join('markers', 'marker7_') + str(i + 1) + '.png', markers2[i]) return markers_dict1, markers_dict2
def __threadfunc__(self): markerDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250) while self.working: ok, frame = self.capture.read() self.current_frame = frame if self.debug: (corners, ids, rejects) = aruco.detectMarkers(frame, markerDict) frame = aruco.drawDetectedMarkers(frame, corners, ids) cv2.imshow("Camera", cv2.flip(imutils.resize(frame, width=480), 1)) cv2.waitKey(1)