def detect(self, image): gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(gray) for r in results: (ptA, ptB, ptC, ptD) = r.corners ptB = (int(ptB[0]), int(ptB[1])) ptC = (int(ptC[0]), int(ptC[1])) ptD = (int(ptD[0]), int(ptD[1])) ptA = (int(ptA[0]), int(ptA[1])) # extract the bounding box (x, y)-coordinates for the AprilTag # and convert each of the (x, y)-coordinate pairs to integers # draw the bounding box of the AprilTag detection cv2.line(image, ptA, ptB, (0, 255, 0), 2) cv2.line(image, ptB, ptC, (0, 255, 0), 2) cv2.line(image, ptC, ptD, (0, 255, 0), 2) cv2.line(image, ptD, ptA, (0, 255, 0), 2) # draw the center (x, y)-coordinates of the AprilTag (cX, cY) = (int(r.center[0]), int(r.center[1])) cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1) # draw the estimated corner of the paper (cX_up, cY_up) = self.estimatCorner(results) cv2.circle(image, (cX_up, cY_up), 9, (255, 0, 0), -1) (cX_dw, cY_dw) = (int(r.center[0]), int(r.center[1])) cv2.circle(image, (cX_up, cY_up), 9, (255, 0, 0), -1) # show the output image after AprilTag detection return image
def __init__(self, server_): self.frame = None self.server = server_ self.tag_name = "" self.target_name = "" self.width = int(self.server.video.width) self.height = int(self.server.video.height) self.quad_x = int(self.width / 2) self.quad_y = int(self.height / 2) self.theta = math.radians(62.2) # width angle of picam self.psi = math.radians(48.8) # height angle of picam self.pix_to_meter = 0 self.altitude = 1 options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=1, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=True, refine_pose=True, debug=False, quad_contours=True) self.detector = apriltag.Detector(options) self.x = None self.y = None
def __init__(self, args): # state self.input_camera = [None, None] self.april_detector = True self.rate = 0 self.kin_joints = JointState() self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"] self.kin_joints.position = [ -0.10471975803375244, 0.9031959176063538, 0.06871610879898071, 0.14394205808639526 ] self.kin_joints.effort = [0, 0, 0, 1] # init the robot Kinetic control. self.robot = miro.interface.PlatformInterface() # handle april if "apriltag" not in sys.modules: raise ValueError("April Tags library not available") options = apriltag.DetectorOptions( \ families='tag36h11', border=0, nthreads=128, quad_decimate=2.0, quad_blur=0.0, refine_edges=False, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) self.april_detector = apriltag.Detector(options) # ROS -> OpenCV converter self.image_converter = CvBridge() # robot name topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME") # subscribe self.sub_caml = rospy.Subscriber(topic_base_name + "/sensors/caml/compressed", CompressedImage, self.callback_caml, queue_size=1, tcp_nodelay=True) #self.sub_camr = rospy.Subscriber(topic_base_name + "/sensors/camr/compressed", CompressedImage, self.callback_camr, queue_size=1, tcp_nodelay=True) self.sub_kin = rospy.Subscriber(topic_base_name + "/sensors/kinematic_joints", JointState, self.callback_kin, queue_size=1, tcp_nodelay=False) self.kinematic_joints_pub = rospy.Publisher( topic_base_name + "/control/kinematic_joints", JointState, queue_size=1)
def tag_detect(self, data): # Convert ros gray img to cv2: cv2_frame = self.bridge.imgmsg_to_cv2(data, "mono8") # Tag detection: options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(cv2_frame) if len(results) > 0: return results[0].tag_id else: return -1
def __init__(self, tag_size=0.03, x_length=0.225, y_length=0.14): self.bridge = CvBridge() options = apriltag.DetectorOptions(families="tag16h5") self.detector = apriltag.Detector(options) # TODO: This needs to map from tag ids to corner locations. Should probably learn it on first image rather than specify self.corner_ids = {7: 0, 5: 1, 6: 2, 4: 3} self.tag_size = tag_size self.x_length = x_length self.y_length = y_length self.px_per_m = 5000 xPix = int(self.x_length * self.px_per_m) yPix = int(self.y_length * self.px_per_m) self.corner_coords = numpy.array( [[-self.x_length / 2, -self.y_length / 2, 0], [self.x_length / 2, -self.y_length / 2, 0], [-self.x_length / 2, self.y_length / 2, 0], [self.x_length / 2, self.y_length / 2, 0]]) self.pts_dst = numpy.array([[0, 0], [xPix, 0], [0, yPix], [xPix, yPix]]) offset = int(self.tag_size / 2 * self.px_per_m) self.output_shape = (xPix + 2 * offset, yPix + 2 * offset) self.pts_dst += offset self.corner_locations = numpy.empty((4, 2), numpy.float32) self.corner_locations[:] = numpy.nan self.corner_positions = numpy.empty((4, 3), numpy.float32) self.corner_positions[:] = numpy.nan if DEBUG: self.pc_pub = rospy.Publisher("/test_cloud", PointCloud2, queue_size=10) self.pc_pub2 = rospy.Publisher("/test_cloud2", PointCloud2, queue_size=10) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.transformed_workspace_pub = rospy.Publisher( 'transformed_workspace', Image, queue_size=10) self.camera_params = None
def detect_apriltag(gray, image): options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) #results = detector.detect(img=gray,True, camera_params=[544.021136,542.307110,308.111905,261.603373], tag_size=0.044) results = detector.detect(img=gray) if len(results) > 0: print("[INFO] {} total AprilTags detected".format(len(results))) else: print("No AprilTag Detected") return image = np.array(image) # loop over the AprilTag detection results for r in results: # extract the bounding box (x, y)-coordinates for the AprilTag # and convert each of the (x, y)-coordinate pairs to integers (ptA, ptB, ptC, ptD) = r.corners ptB = (int(ptB[0]), int(ptB[1])) ptC = (int(ptC[0]), int(ptC[1])) ptD = (int(ptD[0]), int(ptD[1])) ptA = (int(ptA[0]), int(ptA[1])) # draw the bounding box of the AprilTag detection cv2.line(image, ptA, ptB, (0, 255, 0), 2) cv2.line(image, ptB, ptC, (0, 255, 0), 2) cv2.line(image, ptC, ptD, (0, 255, 0), 2) cv2.line(image, ptD, ptA, (0, 255, 0), 2) # draw the center (x, y)-coordinates of the AprilTag (cX, cY) = (int(r.center[0]), int(r.center[1])) cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1) # draw the tag family on the image tagFamily = r.tag_family.decode("utf-8") cv2.putText(image, tagFamily, (ptA[0], ptA[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) #print("[INFO] tag family: {}".format(tagFamily)) M, e1, e2 = detector.detection_pose( r, [544.021136, 542.307110, 308.111905, 261.603373]) #w:QR code length w = 4.4 t = [M[0][3] * w, M[1][3] * w, M[2][3] * w] dist = (t[0]**2 + t[1]**2 + t[2]**2)**0.5 print("[INFO] dist:", dist, " tag pose:", t)
def __init__(self, sys): node.Node.__init__(self, sys, "detect_april") options = apriltag.DetectorOptions( \ families='tag16h5', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) self.detector = apriltag.Detector(options)
def extract_markers(frame, marker_family, scan_for_inverted_markers=False): """ Takes in a RGB color frame and extracts all of the apriltag markers present. Returns a list of markers. :param frame: The frame to search. :param marker_family: The marker family to search for. :param scan_for_inverted_markers: Determines if the image should be checked for markers that are inverted. :return: {[Marker]} List of markers """ log.info('Extracting apriltag markers from camera frame.' + (' Checking for inverted markers as well.' if scan_for_inverted_markers else '')) markers = [] options = apriltag.DetectorOptions(families=marker_family) detector = apriltag.Detector(options) gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) gray = Camera.blur_frame(gray, 2) results = detector.detect(gray) if scan_for_inverted_markers: results += detector.detect(Camera.invert_colors(gray)) for r in results: c0, c1, c2, c3 = r.corners c0 = np.array([ int(c0[0]), int(c0[1]) ]) c1 = np.array([ int(c1[0]), int(c1[1]) ]) c2 = np.array([ int(c2[0]), int(c2[1]) ]) c3 = np.array([ int(c3[0]), int(c3[1]) ]) fid = str(r.tag_id) markers.append( Marker( fid, [c0, c1, c2, c3] ) ) log.info(f"Found {len(markers)} markers: {Marker.get_fids_from_list(markers)}") return markers
def __init__(self): self.jdrc = None self.myCmdvel = None self.myCamera = None self.myExtra = None self.max_speed = 0.07 self.dead_band_x = 0.2 #Real_imagen #Real_relativas 0.4 #Virtual 0.2 self.dead_band_y = 0.15 #Real_imagen #Real_relativas 0.15 #Virtual 0.2 self.dead_band_z = 0.3 #Real_imagen #Real_relativas 0.3 #Virtual 0.2 self.min_z = 2.8 self.dead_band_w = 0.230 #Real_imagen #Real_relativas 0.230 #0.2 self.center = [0, 0] self.kp = 0.1 #Real_relativas 0.1 #Virtual 0.1 self.kd = 1500 #Real_imagen 0.01 #Real_relativas 0.7 #Virtual 0.03 self.ki = 0.01 #Real_imangen #Real_relativas0.01 #Virtual 0.005 self.kpw = 0.1 #Virtual 0.1 self.kdw = 1500 #Real 0.7 #Virtual 0 self.kiw = 0.01 #Real 0.01 #Virtual 0 self.kpz = 0.4 #Real 0.4 #Virtual 0.1 self.kdz = 1500 #Real 0.7#Virtual 0.01 self.kiz = 0.01 # Real 0.01# Virtual 0.001 self.cycle = 50 #in ms self.error_xy_anterior = [0, 0] self.error_z_anterior = 0 self.error_w_anterior = 0 self.vxi = 0 self.vyi = 0 self.vzi = 0 self.vwi = 0 #AprilTags self.options = apriltag.DetectorOptions() self.detector = apriltag.Detector(self.options) self.MARKER_SIZE = 0.28 #Pose self.x = 0.0 self.y = 0.0 self.z = 0.0 self.pitch = 0.0 #Beacons self.beacons = [4, 7] self.beaconCounter = 0 self.switchBeacon = False self.timeToLand = False self.connectProxies()
def __init__(self, args): # state self.input_camera = [None, None] self.april_detector = True # init the robot Kinetic control. self.robot = miro.interface.PlatformInterface() # handle april if "apriltag" not in sys.modules: raise ValueError("April Tags library not available") options = apriltag.DetectorOptions( \ families='tag16h5', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=True, quad_contours=True) self.april_detector = apriltag.Detector(options) # ROS -> OpenCV converter self.image_converter = CvBridge() # robot name topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME") # subscribe self.sub_caml = rospy.Subscriber(topic_base_name + "/sensors/caml/compressed", CompressedImage, self.callback_caml, queue_size=1, tcp_nodelay=True) self.sub_camr = rospy.Subscriber(topic_base_name + "/sensors/camr/compressed", CompressedImage, self.callback_camr, queue_size=1, tcp_nodelay=True) # report print "recording from 2 cameras, press CTRL+C to halt..."
def tag_detect(self, data): # Converter a imagem ros para imagem cv2 cv2_frame = self.bridge.imgmsg_to_cv2(data, "mono8") # cv2_frame_grey = cv2.cvtColor(cv2_frame, cv2.COLOR_BGR2GRAY) # Tag detection options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(cv2_frame) # for r in results: # (cX, cY) = (int(r.center[0]), int(r.center[1])) # cv2.circle(cv2_frame, (cX, cY), 5, (0,0,255), -1) if len(results) > 0: return results[0].tag_id # print('[info] tag detectada') else: return -1 print('[info] buscando por tags...')
def callback(self, data): # Converter a imagem ros para imagem cv2 cv2_frame = self.bridge.imgmsg_to_cv2(data, "bgr8") cv2_frame_grey = cv2.cvtColor(cv2_frame, cv2.COLOR_BGR2GRAY) # Tag detection options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(cv2_frame_grey) # for r in results: # (cX, cY) = (int(r.center[0]), int(r.center[1])) # cv2.circle(cv2_frame, (cX, cY), 5, (0,0,255), -1) if len(results) > 0: self.tag_id_pub.publish(results[0].tag_id) else: self.tag_id_pub.publish(-1) print('[info] buscando por tags...')
def __init__(self): self.jdrc = None self.myCmdvel = None self.myCamera = None self.max_speed = 5 self.dead_band_x = 0.0 #Real 0.4 #Virtual 0.2 self.dead_band_y = 0.0 #Virtual 0.2 self.dead_band_z = 0.0 #Real 0.4 #Virtual 0.2 self.min_z = 2.4 self.dead_band_w = 0.0 #0.2 self.center = [0, 0] self.kp = 0.2 #Real 0.05 #Virtual 0.1 self.kd = 0.05 #Real 0.0001 #Virtual 0.03 self.ki = 0.001 #Virtual 0.005 self.kpz = 0.01 self.kdz = 0.05 self.kiz = 0.001 self.kpw = 1 self.kdw = 0 self.kiw = 0 self.cycle = 50 #in ms self.error_xy_anterior = [0, 0] self.error_z_anterior = 0 self.error_w_anterior = 0 self.vxi = 0 self.vyi = 0 self.vzi = 0 self.vwi = 0 #AprilTags self.options = apriltag.DetectorOptions() self.detector = apriltag.Detector(self.options) #Pose self.x = 0.0 self.y = 0.0 self.z = 0.0 self.pitch = 0.0 self.MARKER_SIZE = 0.28 self.connectProxies()
def __init__(self, image): ###################################### # SETTING UP VARIABLES ###################################### # assuming that the image is already grayscale # self.image = image self.imageArray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) self.threshold = 70 self.camera_cal_matrix = np.array([[1.11379904e+03, 0.00000000e+00, 5.67239537e+02], [0.00000000e+00, 1.16158572e+03, 1.04091897e+02], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) options = apriltag.DetectorOptions(families='tag36h11', border=4, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=True, refine_pose=True, debug=True, quad_contours=False) self.StringToList = self.getResults()
def __init__(self, markers): self.options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) self.detector = apriltag.Detector(self.options) self.detector = apriltag.Detector( self.options, searchpath=apriltag._get_demo_searchpath()) self.kalman_filters = {} self.transformation_ka = Kalman(12) self.g_camera_base = None self.marker_pose_dict = {} self.markers = markers self.dimg = None # apriltage detection overlay matrix self.kalman_count = {} for tid in self.markers: self.kalman_count[tid] = 0
def start(): #img = cv2.imread('tag-standard-41h12.png', cv2.IMREAD_GRAYSCALE) img_dir = input('input image dir: ') img_name = input('input the image name: ') img = cv2.imread(img_dir + '/' + img_name, cv2.IMREAD_GRAYSCALE) options = apriltag.DetectorOptions(families='tagstandard41h12', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) detector = apriltag.Detector() results = detector.detect(img) print(results) if len(results) > 0: print(results[0].tag_family) print(results[0].center) print('number of detections: ', len(results))
def __init__(self): options = apriltag.DetectorOptions(refine_pose=True) self.detector = apriltag.Detector(options=options)
import apriltag import cv2 import time cam = cv2.VideoCapture(1) options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=True, debug=False, quad_contours=True) detector = apriltag.Detector() for i in range(40): ret, frame = cam.read() #cv2.imwrite("her.png", frame) #img = cv2.imread('her.png', cv2.IMREAD_GRAYSCALE) img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) '''scale_percent = 30 # percent of original size width = int(img.shape[1] * scale_percent / 100) height = int(img.shape[0] * scale_percent / 100) dim = (width, height)''' #img = cv2.resize(img, dim) result = detector.detect(img)
def apriltag_dection(): global ser while True: ret, image = video_capture.read() # if we are viewing a video and we did not grab a frame, # then we have reached the end of the video if image is None: break small_frame = cv2.resize(image, (0, 0), fx=frameSize, fy=frameSize) #print(small_frame.shape) gray = cv2.cvtColor(small_frame, cv2.COLOR_BGR2GRAY) #print("[INFO] detecting AprilTags...") options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(gray) #if there is no AprilTag in the frame, search for it if not len(results): print("No AprilTags detected, searching for...") ser.write("1 Right .2".encode()) print("Right\n"); #x_time = time.time() #time.sleep(robot_delay) #start_time = time.time() print(ser.readline().decode()) print(ser.readline().decode()) #print("Start - X: " + str(start_time-x_time) + "\n") #ser.write("Stop".encode()) #print("Stop\n"); #middle_time = time.time() #print(ser.readline().decode()) #print(ser.readline().decode()) #end_time = time.time() #print("END - MIDDLE: " + str(end_time - middle_time) + "\nMIDDLE - START: " + str(middle_time - start_time)) #time.sleep(robot_delay) #cv2.imshow("Image", small_frame) else: print("[INFO] {} total AprilTags detected".format(len(results))) # loop over the AprilTag detection results for r in results: print("tag_id:" + str(r.tag_id) + "\n") # extract the bounding box (x, y)-coordinates for the AprilTag # and convert each of the (x, y)-coordinate pairs to integers (ptA, ptB, ptC, ptD) = r.corners ptB = (int(ptB[0]), int(ptB[1])) ptC = (int(ptC[0]), int(ptC[1])) ptD = (int(ptD[0]), int(ptD[1])) ptA = (int(ptA[0]), int(ptA[1])) (cX, cY) = (int(r.center[0]), int(r.center[1])) #print the coordinate #print("Distance A_B" + str(distance(ptA,ptB))) #print("Distance A_D" + str(distance(ptA,ptD))) #print("Distance B_C" + str(distance(ptB,ptC))) #print("Distance C_D" + str(distance(ptC,ptD))) #compute the distance between webcam and the tag #limit: the apriltag has to be right in front the camera for the best approximation bound_length = [distance(ptA,ptB),distance(ptA,ptD),distance(ptB,ptC),distance(ptC,ptD)]; average_bound_length = statistics.mean(bound_length); print("distance: " + str(4791.1*average_bound_length**-1.042) +" cm") cv2.putText(small_frame, "distance: " + str(round(4791.1*average_bound_length**-1.042,3)) +" cm", (540,480), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) # draw the bounding box of the AprilTag detection cv2.line(small_frame, ptA, ptB, (0, 0, 255), 2) cv2.line(small_frame, ptB, ptC, (0, 0, 255), 2) cv2.line(small_frame, ptC, ptD, (0, 0, 255), 2) cv2.line(small_frame, ptD, ptA, (0, 0, 255), 2) # draw the center (x, y)-coordinates of the AprilTag cv2.circle(small_frame, (cX, cY), 5, (0, 0, 255), -1) print("Center: " + str(cX) + ", " + str(cY)) # draw the tag family on the image tagFamily = r.tag_family.decode("utf-8") cv2.putText(small_frame, tagFamily, (ptA[0], ptA[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) print("[INFO] tag family: {}".format(tagFamily)) # show the output image after AprilTag detection #cv2.imshow("Image", small_frame) if is_center(cX) == True: print("Apriltag Centered...") distance_to_tag = 4791.1*average_bound_length**-1.042; print(distance_to_tag); if(distance_to_tag > 60.0): print("Following the Tag...") ser.write("1 Forward .5".encode()) print(ser.readline().decode()) print(ser.readline().decode()) #time.sleep(robot_delay) #print(ser.readline().decode()) #ser.write("Stop".encode()) #print(ser.readline().decode()) #time.sleep(robot_delay) else: ser.write("Stop".encode()) print(ser.readline().decode()) print(ser.readline().decode()) else: print("Apriltag Not Centered..") if(cX < 350): print("Shift Left...") ser.write("1 Left .1".encode()) print(ser.readline().decode()) print(ser.readline().decode()) #time.sleep(robot_delay) #print(ser.readline().decode()) #ser.write("Stop".encode()) #print(ser.readline().decode()) #time.sleep(robot_delay) elif(cX > 610): print("Shift Right...") ser.write("1 Right .1".encode()) print(ser.readline().decode()) print(ser.readline().decode()) #time.sleep(robot_delay) #print(ser.readline().decode()) #ser.write("Stop".encode()) #print(ser.readline().decode()) #time.sleep(robot_delay) else: ser.write("Stop".encode()) print(ser.readline().decode()) print(ser.readline().decode()) #print(ser.readline().decode()) # Hit 'q' on the keyboard to quit! if cv2.waitKey(1) & 0xFF == ord('q'): break # Release handle to the webcam video_capture.release() cv2.destroyAllWindows()
import sys import Adafruit_PCA9685 import time import RPi.GPIO as GPIO GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) GPIO.setup(18, GPIO.OUT) pwm = Adafruit_PCA9685.PCA9685() pwm.set_pwm_freq(50) cap = cv.VideoCapture(0) cap.set(3, 640) cap.set(4, 480) at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11')) pwm_x_flag = 0 pwm_y_flag = 0 lastError_x = 0 lastError_y = 0 w_center = 320 h_center = 240 x_dutyCircle = 435 y_dutyCircle = 220 para_x = [lastError_x, x_dutyCircle] para_y = [lastError_y, y_dutyCircle] pwm.set_pwm(9, 0, 435) time.sleep(0.005) pwm.set_pwm(8, 0, 220)
import argparse import cv2 # Construct the argument parser and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("-i", "--image", required=True, help="Path to the input image containing the AprilTag") args = vars(ap.parse_args()) # Load the input image and convert the image to grayscale print("[INFO] Loading the image...") image = cv2.imread(args["image"]) gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # Define the AprilTag detector options and then detect the AprilTags in the input image print("[INFO] Detecting AprilTags...") options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(gray) print("[INFO] {} Total AprilTags Detected!".format(len(results))) # Loop over the AprilTags detection results for r in results: # Extract the bounding box (x, y) coordinates for the AprilTag and convert each one of the (x,y) coordinate # pairs to integers (ptA, ptB, ptC, ptD) = r.corners ptB = (int(ptB[0]), int(ptB[1])) ptC = (int(ptC[0]), int(ptC[1])) ptD = (int(ptD[0]), int(ptD[1])) ptA = (int(ptA[0]), int(ptA[1])) # Draw the bounding box of the AprilTag direction using the extracted (x, y) coordinates cv2.line(image, ptA, ptB, (0, 255, 0), 2)
def __init__(self): self.jdrc = None self.myCmdvel = None self.myCamera = None self.myExtra = None self.maxSpeed = 2 #Real_relativa 0.07 #Cambiar max_speed #Transitions Variables self.timeToLand = False self.timeToApril = False self.timeToColor = False #Xml Files self.xmlFileName = 'calibration_simulated.xml' #self.xmlFileName = 'calibration.xml' #Image Center self.center = [0, 0] #For PIDs self.cycle = 50 #in ms '''Color Beacon Variables''' #PID Constants self.kpxColor = 0.005 #0.012 #Virtual 0.005 self.kdxColor = 0.016 #Virtual 0.01 self.kixColor = 0.00004 #Virtual 0.00006 self.kpyColor = 0.0025 #Virtual self.kdyColor = 0.008 #Virtual self.kiyColor = 0.00006 #Virtual self.kpzColor = 0 #Virtual self.kdzColor = 0 #Virtual self.kizColor = 0 #Virtual self.initialize = True self.stableTime = 0 self.errYPrevColor = 0 self.errXPrevColor = 0 self.yanteriorTot = 0 self.xanteriorTot = 0 #self.x_img=0 #self.y_img=0 #self.landed=True #Cambiar #self.turnland=0 #Cambiar #self.numIteracionesOrange=0 #Revisar #self.numIteracionesGreen=0 #Revisar #Color Filter self.minArea = 700 #Gazebo 700 self.PH_max = 180 self.PS_max = 255 self.PV_max = 255 self.PH_min = 0 self.PS_min = 0 self.PV_min = 0 self.PErode = 0 self.PDilate = 0 self.SH_max = 180 self.SS_max = 255 self.SV_max = 255 self.SH_min = 0 self.SS_min = 0 self.SV_min = 0 self.SErode = 0 self.SDilate = 0 self.alpha = 0.6 self.vxiColor = 0 self.vyiColor = 0 self.vziColor = 0 self.xDistanceDeadZone = 0 #Virtual 0 self.yDistanceDeadZone = 0 #Virtual 0 self.xDeadZone = False self.yDeadZone = False self.calibration = True self.timeLimitToLand = 3 #In seconds '''Apriltag Beacon Variables''' self.dead_band_x = 0.2 #Real_imagen #Real_relativas 0.4 #Virtual 0.2 self.dead_band_y = 0.15 #Real_imagen #Real_relativas 0.15 #Virtual 0.2 self.dead_band_z = 0.3 #Real_imagen #Real_relativas 0.3 #Virtual 0.2 self.min_z = 2.8 self.dead_band_w = 0.230 #Real_imagen #Real_relativas 0.230 #0.2 self.targetCenter = [0, 0] #CAMBIAR self.kp = 0.1 #Real_relativas 0.1 #Virtual 0.1 self.kd = 1500 #Real_imagen 0.01 #Real_relativas 0.7 #Virtual 0.03 self.ki = 0.01 #Real_imangen #Real_relativas0.01 #Virtual 0.005 self.kpw = 0.1 #Virtual 0.1 self.kdw = 1500 #Real 0.7 #Virtual 0 self.kiw = 0.01 #Real 0.01 #Virtual 0 self.kpz = 0.4 #Real 0.4 #Virtual 0.1 self.kdz = 1500 #Real 0.7#Virtual 0.01 self.kiz = 0.01 # Real 0.01# Virtual 0.001 self.error_xy_anterior = [0, 0] self.error_z_anterior = 0 self.error_w_anterior = 0 self.vxi = 0 self.vyi = 0 self.vzi = 0 self.vwi = 0 #AprilTags self.options = apriltag.DetectorOptions() self.detector = apriltag.Detector(self.options) self.MARKER_SIZE = 0.28 #Image Processing self.cameraMatrix = [187.336, 0, 160, 0, 187.336, 120, 0, 0, 1] #Gazebo simulator #self.cameraMatrix = [731.37257937739332, 0, 322.35647387552422, 0, 734.23757205576192, 246.30430666269825, 0, 0, 1] #ArDrone 2 #Distorsion Coefficients self.distCoeffs = np.zeros((5, 1), np.uint8) #Gazebo #self.distCoeffs = np.array([-0.07304604105914128, 0.50646907582979650, 0.00024443957708413687, 0.00074556540195940392,-1.0762308065763191]) #ArDrone 2 #Pose self.x = 0.0 self.y = 0.0 self.z = 0.0 self.pitch = 0.0 #Beacons self.beacons = [4, 7] self.beaconCounter = 0 self.switchBeacon = False #Performance monitor self.minf = 10000 self.maxf = 0 self.avgf = 0 self.listf = [0, 0, 0] self.c = 0 self.connectProxies()
def get_coordinates(self, cvQueue: Queue): # To get the coordinate, we rotate on our axis some X number of times to form images that compose a complete # 360 degree view of our surroundings. We use each image (as long as there are april tags in it) to get a (x, # z) coordinate value, and then we choose which (x,z) coordinate to return based off of which we deem the # most correct/reliable (this decision is shown in the code below) # When turning to search for the desiredTag, we specify time to turn, and time to wait after each semi-turn. # We do this because we want a stable photo/shot at each searchingTimeToTurn = 0.5 # seconds searchingTimeToHalt = 2.0 # seconds # Note that refine_pose is set to True (takes more work/processing but hopefully gets better coordinates) options = apriltag.DetectorOptions( families='tag36h11', border=1, nthreads=1, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=True, refine_pose=True, debug=False, quad_contours=True) det = apriltag.Detector(options) # Load camera data with open('cameraParams.json', 'r') as f: data = json.load(f) cameraMatrix = np.array(data['cameraMatrix'], dtype=np.float32) distCoeffs = np.array(data['distCoeffs'], dtype=np.float32) # Load world points world_points = {} with open('worldPoints.json', 'r') as f: data = json.load(f) for k, v in data.items(): world_points[int(k)] = np.array(v, dtype=np.float32).reshape((4, 3, 1)) # Variables for final decision coordinates_list = [] iterationNumber = 1 numIterations = 10 while True: # Rotate camera by going left by some amount self.send_serial_command(Direction.LEFT, b'l') time.sleep(searchingTimeToTurn) self.send_serial_command(Direction.STOP, b'h') time.sleep(searchingTimeToHalt) # Now lets read the frame (while the robot is halted so that image is clean) frame = self.vs.read() if frame is None: print("ERROR - frame read a NONE") break # Use grayscale image for detection gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) res = det.detect(gray) # Check how many tags we see... if it's 0 then ignore this frame and move on to capturing the next frame numTagsSeen = len(res) print("\nNumber of tags seen", numTagsSeen) # TODO remove if numTagsSeen > 0: poses = [] # Store poses from each tag to average them over tagRadiusList = [] # Store tag radius' to determine the largest for r in res: # Iterate over each tag in the frame corners = r.corners tag_id = r.tag_id corners = np.array(corners, dtype=np.float32).reshape((4, 2, 1)) cornersList = [] for c in corners: cornersList.append([int(x) for x in c]) cornersList = np.array(cornersList, dtype=np.int32) # Turn into numpy array (openCV wants this) # Draw circle around tag using its corners & get radius of that tag ((x, y), radius) = cv2.minEnclosingCircle(cornersList) filteredPtsRadius = [radius] # Solve pose ((x,z) coordinates) r, rot, t = cv2.solvePnP(world_points[tag_id], corners, cameraMatrix, distCoeffs) # get rotation and translation vector using solvePnP rot_mat, _ = cv2.Rodrigues(rot) # convert to rotation matrix R = rot_mat.transpose() # Use rotation matrix to get pose = -R * t (matrix mul w/ @) pose = -R @ t weight = self.calc_weight(pose, world_points[tag_id][0]) poses.append((pose, weight)) tagRadiusList.append(filteredPtsRadius) # Done iterating over the tags that're seen in the frame... # Now get the average pose across the tags and get the largest tag radius that we saw. # We will store the (x,z) coordinate that we calculate, and we'll also # store the largest radius for a tag that we've seen in this frame. avgPose = sum([x * y for x, y in poses]) / sum([x[1] for x in poses]) largestTagRadius = max(tagRadiusList) coordinates = (avgPose[0][0], avgPose[2][0], largestTagRadius) print(str(coordinates)) # TODO remove this coordinates_list.append(coordinates) # Display frame cv2.imshow('frame', frame) # If we've completed our numIterations, then choose the coordinate # and return (do closing operations too) if iterationNumber == numIterations: if len(coordinates_list) > 0: # TODO 2 things we can try here ... # 1) The coordinate to return is the one with the smallest z-coordinate # (which essentially means it's closest to those tags that it used) # BUT this value seems to vary a lot and I don't think it's reliable # 2) I have saved the largest radius for a tag seen for each of these # coordinates, so I can use that (which I bet is more reliable) # I will go with approach number 2 # coordinateToReturn = min(coordinates_list, key=lambda x: x[1]) # Approach (1) coordinateToReturn = max(coordinates_list, key=lambda x: x[2]) # Approach (2) coordinateToReturn = ( int(coordinateToReturn[0]), int(coordinateToReturn[1])) # This stays regardless else: coordinateToReturn = (0, -1) # TODO set to some default outside the door # Cleanup and return cv2.destroyAllWindows() print("Value to return:") # TODO remove print(coordinateToReturn) # TODO remove return coordinateToReturn else: # Still have iterations to go, increment the value iterationNumber += 1 # Q to quit key = cv2.waitKey(1) & 0xFF if (key == ord("q")) or (not cvQueue.empty()): self.send_serial_command(Direction.STOP, b'h') cv2.destroyAllWindows() break
def run_cam_thread(): global name # Initialize some variables face_locations = [] face_encodings = [] face_names = [] process_this_frame = True prev_direction = None while True: # Grab a single frame of video ret, frame = video_capture.read() # Resize frame of video to 1/4 size for faster face recognition processing small_frame = cv2.resize(frame, (0, 0), fx=frameSize, fy=frameSize) if (function_index == 1): # Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses) rgb_small_frame = small_frame[:, :, ::-1] # Only process every other frame of video to save time if process_this_frame: # Find all the faces and face encodings in the current frame of video face_locations = face_recognition.face_locations( rgb_small_frame) face_encodings = face_recognition.face_encodings( rgb_small_frame, face_locations) name = "Unknown" face_names = [] for face_encoding in face_encodings: # See if the face is a match for the known face(s) matches = face_recognition.compare_faces( known_face_encodings, face_encoding) # Or instead, use the known face with the smallest distance to the new face face_distances = face_recognition.face_distance( known_face_encodings, face_encoding) best_match_index = np.argmin(face_distances) if matches[best_match_index]: name = known_face_names[best_match_index] else: name = "Unknown" face_names.append(name) process_this_frame = not process_this_frame # Display the results for (top, right, bottom, left), name in zip(face_locations, face_names): # Draw a box around the face cv2.rectangle(small_frame, (left, top), (right, bottom), (0, 0, 255), 2) # Draw a label with a name below the face cv2.rectangle(small_frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED) font = cv2.FONT_HERSHEY_DUPLEX cv2.putText(small_frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1) elif (function_index == 2): print("tracking tag...") gray = cv2.cvtColor(small_frame, cv2.COLOR_BGR2GRAY) options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) results = detector.detect(gray) #if there is no AprilTag in the frame, search for it if not len(results): print("No AprilTags detected, searching for...") if (prev_direction == None or prev_direction == 'Right'): ser.write("1 Right .2".encode()) print("Right\n") print(ser.readline().decode()) else: ser.write("1 Left .2".encode()) print("Left\n") print(ser.readline().decode()) else: print("[INFO] {} total AprilTags detected".format( len(results))) # loop over the AprilTag detection results for r in results: print("tag_id:" + str(r.tag_id) + "\n") # extract the bounding box (x, y)-coordinates for the AprilTag # and convert each of the (x, y)-coordinate pairs to integers (ptA, ptB, ptC, ptD) = r.corners ptB = (int(ptB[0]), int(ptB[1])) ptC = (int(ptC[0]), int(ptC[1])) ptD = (int(ptD[0]), int(ptD[1])) ptA = (int(ptA[0]), int(ptA[1])) (cX, cY) = (int(r.center[0]), int(r.center[1])) #compute the distance between webcam and the tag #limit: the apriltag has to be right in front the camera for the best approximation bound_length = [ distance(ptA, ptB), distance(ptA, ptD), distance(ptB, ptC), distance(ptC, ptD) ] average_bound_length = statistics.mean(bound_length) print("distance: " + str(2148.2 * average_bound_length**-1.02) + " cm") cv2.putText( small_frame, "distance: " + str( (round(2148.2 * average_bound_length**-1.02))) + " cm", (540, 480), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) # draw the bounding box of the AprilTag detection cv2.line(small_frame, ptA, ptB, (0, 0, 255), 2) cv2.line(small_frame, ptB, ptC, (0, 0, 255), 2) cv2.line(small_frame, ptC, ptD, (0, 0, 255), 2) cv2.line(small_frame, ptD, ptA, (0, 0, 255), 2) # draw the center (x, y)-coordinates of the AprilTag cv2.circle(small_frame, (cX, cY), 5, (0, 0, 255), -1) print("Center: " + str(cX) + ", " + str(cY)) # draw the tag family on the image tagFamily = r.tag_family.decode("utf-8") cv2.putText(small_frame, tagFamily, (ptA[0], ptA[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) print("[INFO] tag family: {}".format(tagFamily)) if is_center(cX) == True: print("Apriltag Centered...") distance_to_tag = (2148.2 * average_bound_length**-1.02) print(distance_to_tag) if (distance_to_tag > 60.0): print("Following the Tag...") ser.write("1 Forward .5".encode()) print(ser.readline().decode()) else: ser.write("Stop".encode()) print(ser.readline().decode()) else: print("Apriltag Not Centered..") if (cX < 150): print("Shift Left...") ser.write("1 Left .1".encode()) print(ser.readline().decode()) elif (cX > 350): print("Shift Right...") ser.write("1 Right .1".encode()) print(ser.readline().decode()) else: ser.write("Stop".encode()) print(ser.readline().decode()) if cX < 240: prev_direction = 'Left' else: prev_direction = 'Right' cv2.imshow('Video', small_frame) # Hit 'q' on the keyboard to quit! if cv2.waitKey(1) & 0xFF == ord('q'): break video_capture.release() cv2.destroyAllWindows()
def RotateByX(Cy, Cz, thetaX): rx = thetaX*math.pi/180.0 outY = math.cos(rx)*Cy - math.sin(rx)*Cz outZ = math.sin(rx)*Cy + math.cos(rx)*Cz return outY, outZ cameraParams_Intrinsic = [591,591,321,245] # camera_fx, camera_fy, camera_cx, camera_cy camera_matrix = np.array(([591.280996, 0, 321.754492], [0, 591.853914, 245.866165], [0, 0, 1.0]), dtype=np.double) cap = cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480) cap.set(cv2.CAP_PROP_FRAME_WIDTH,640) # cap.set(cv2.CAP_PROP_BRIGHTNESS, 5) tag_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11')) # Build a detector for apriltag while( cap.isOpened() ): ret, img = cap.read() gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # The image must be a grayscale image of type numpy.uint8 key = cv2.waitKey(45) if key & 0x00FF == 27: break tags = tag_detector.detect( gray) for tag in tags: cv2.circle(img, tuple(tag.corners[0].astype(int)), 4,(0,0,255), 2) # left-top cv2.circle(img, tuple(tag.corners[1].astype(int)), 4,(0,0,255), 2) # right-top cv2.circle(img, tuple(tag.corners[2].astype(int)), 4,(0,0,255), 2) # right-bottom cv2.circle(img, tuple(tag.corners[3].astype(int)), 4,(0,0,255), 2) # left-bottom cv2.circle(img, tuple(tag.center.astype(int)), 4,(0,0,255), 2) #center #print(position_center) cv2.putText(img,str(tag.tag_id),tuple(tag.center.astype(int)),cv2.FONT_HERSHEY_COMPLEX, 2, (0, 255, 0), 5)
_colorlist = {} # Open and read in the datafile with the colorlist with open(args.datafile, 'r') as f: data = yaml.load(f, Loader=yaml.Loader) # Organize colorlist so that we can access the color by tag number as index for obj in data['colorlist']: _colorlist[obj['tag']] = obj['color'] # print(_colorlist) # Open Camera _cam = cv2.VideoCapture(0) apriltag.DetectorOptions(families='tag36h11') _detector = apriltag.Detector() _calibrationRunning = True _calibrationStep = 0 _data = {} while _calibrationRunning: ret_val, img = _cam.read() grayscale = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Rough guessing of where the screen is based on image differences if _calibrationStep == 0: if 'meanBright' not in _data: _data['meanBright'] = []
def __init__(self, args): # state self.input_camera = [None, None] self.april_detector = True self.kin_joints = JointState() self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"] self.kin_joints.position = [ 0.0, math.radians(5), math.radians(1), math.radians(7) ] # init the robot Kinetic control. self.robot = miro.interface.PlatformInterface() # handle april if "apriltag" not in sys.modules: raise ValueError("April Tags library not available") options = apriltag.DetectorOptions( \ families='tag36h11', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=True, quad_contours=True) self.april_detector = apriltag.Detector(options) # ROS -> OpenCV converter self.image_converter = CvBridge() # robot name topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME") # subscribe self.sub_caml = rospy.Subscriber(topic_base_name + "/sensors/caml/compressed", CompressedImage, self.callback_caml, queue_size=1, tcp_nodelay=True) self.sub_camr = rospy.Subscriber(topic_base_name + "/sensors/camr/compressed", CompressedImage, self.callback_camr, queue_size=1, tcp_nodelay=True) self.sub_lightsens = rospy.Subscriber(topic_base_name + "/sensors/package", miro.msg.sensors_package, self.callback_lightsense, queue_size=1, tcp_nodelay=True) self.kinematic_joints_pub = rospy.Publisher( topic_base_name + "/control/kinematic_joints", JointState, queue_size=0) # report print "recording from 2 cameras, press CTRL+C to halt..."
def __init__(self, args): # state self.mode = None self.input_camera = [None, None] self.april_detector = None # handle arguments for arg in args: if arg in ["show", "record"]: self.mode = arg continue if arg == "--april": self.april_detector = True continue print "argument unrecognised:", arg exit() # check mode if self.mode is None: error("mode not set") # handle april if not self.april_detector is None: if "apriltag" not in sys.modules: raise ValueError("April Tags library not available") self.mode = self.mode.replace("-april", "") options = apriltag.DetectorOptions( \ families='tag16h5', border=1, nthreads=4, quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) self.april_detector = apriltag.Detector(options) # ROS -> OpenCV converter self.image_converter = CvBridge() # robot name topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME") # subscribe self.sub_caml = rospy.Subscriber(topic_base_name + "/sensors/caml/compressed", CompressedImage, self.callback_caml, queue_size=1, tcp_nodelay=True) self.sub_camr = rospy.Subscriber(topic_base_name + "/sensors/camr/compressed", CompressedImage, self.callback_camr, queue_size=1, tcp_nodelay=True) # report print "recording from 2 cameras, press CTRL+C to halt..."
def main(argv=None): if argv is None: argv = sys.argv #img_path="./image/" #img_path = argv[1] img_path = Path("./image/") pose_path = Path("./pose") mask_path = Path("./mask") distortion = np.array(distortion_v8) intrinsicMatrix = np.array(intrinsicMatrix_v8) i_img_succeed = 0 all_files = [ os.path.basename(img_path_) for i_img, img_path_ in enumerate(img_path.files('*.jpg')) ] all_files.sort(key=lambda x: int(x[:-4])) img_num = len(all_files) for i_img, img_file_name in enumerate(all_files): img_file = img_path + img_file_name image_src = np.array(io.imread(img_file, True) * 255, np.uint8) print("segemet image: %s.. %d/%d" % (os.path.basename(img_file), i_img + 1, img_num)) image_src = deblur(image_src) options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=8, quad_decimate=1.0, quad_blur=1, refine_edges=True, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) detector = apriltag.Detector(options) tags = detector.detect(image_src) #get camera pose if len(tags) < 4: print("Tags detection failed, drop image", i_img) continue ret, rvec, tvec = detectionCameraPose(i_img, tags, intrinsicMatrix, distortion) #get tag mark area tag_mask = np.zeros(image_src.shape, np.uint8) tag_corners = [ tags[0].center, tags[1].center, tags[3].center, tags[2].center ] area_center_src = (tags[0].center + tags[1].center + \ tags[3].center + tags[2].center) / 4 area = np.array([tag_corners], dtype=np.int32) #get roi area_rect = cv2.boundingRect(area) x = area_rect[0] y = area_rect[1] dx = area_rect[2] dy = area_rect[3] image_ROI = image_src[y:y + dy, x:x + dx] area_center_roi = area_center_src - [x, y] # for tag in tags: # cv2.circle(image_src, (int(tag.corners[3][0]), int(tag.corners[3][1])), # radius=5, color=255, thickness=40) #cv2.circle( # image_ROI, (int(area_center_roi[0]), int(area_center_roi[1])), radius=5, color=255, thickness=20) #showimg(image_ROI) shape0_res = patch_size - image_ROI.shape[0] % patch_size shape1_res = patch_size - image_ROI.shape[1] % patch_size # add 4 255 row image_add = np.zeros( (image_ROI.shape[0] + shape0_res, image_ROI.shape[1] + shape1_res), np.uint8) image_add[:-shape0_res, :-shape1_res] = image_ROI # for 8x8 patch calculate grecomatrix image = image_add // 32 image_block = view_as_blocks(image, block_shape=(patch_size, patch_size)) M = np.shape(image_block)[0] N = np.shape(image_block)[1] #print("enter") pool = [] if _MULTIPLUE_THREAD_: cores = multiprocessing.cpu_count() pool = multiprocessing.pool.Pool(processes=cores) result_contrast = np.zeros(shape=(M, N)) # Single thresh # for i in range(M): # for j in range(N): # result_contrast[i][j]=pool_f(i,j) for i in range(M): if _MULTIPLUE_THREAD_: result_contrast[i] = np.array( pool.map(pool_f, list(image_block[i]))).flatten() else: result_contrast[i] = np.array([ pool_f(block) for block in list(image_block[i]) ]).flatten() if _MULTIPLUE_THREAD_: pool.close() pool.join() #print("finish") result_contrast *= (255.0 / result_contrast.max()) img_contrast = result_contrast.astype(np.uint8) #showimg(img_contrast) # how to set this threshold from skimage.filters import threshold_minimum from skimage.filters import threshold_otsu data = img_contrast.ravel() data = data[data != 0] img_gaussianblur = cv2.GaussianBlur(data, (5, 5), 0) otsu_thresh = threshold_otsu(data) #otsu_thresh, binary = cv2.threshold( #img_gaussianblur, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU) binary = img_contrast < otsu_thresh * 3 / 5 #binary = cv2.adaptiveThreshold( # img_contrast, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 51, 5) tag_mask = tag_mask < 128 img_binary = binary.astype(np.uint8) * 255 # find object reagion by finding max countour c_x = img_binary.shape[0] // 2 c_y = img_binary.shape[1] // 2 # You need to choose 4 or 8 for connectivity type connectivity_ = 8 # Perform the operation output = cv2.connectedComponentsWithStats(img_binary, connectivity=connectivity_, stats=cv2.CV_32S) # Get the results # The first cell is the number of labels num_labels = output[0] # The second cell is the label matrix labels = output[1] # The third cell is the stat matrix stats = output[2] # The fourth cell is the centroid matrix centroids = output[3] connected_areas = [[stats[i, cv2.CC_STAT_AREA]] for i in range(num_labels)] #find out second largest area whose centroids in center reagion area_center_downsampled = area_center_roi / patch_size area_center_distance = np.array( [[np.linalg.norm(centroids[i] - area_center_downsampled)] for i in range(num_labels)]) area_is_center = area_center_distance < 20 * patch_size / 32 #rule out outsider by condidering if reagion point inside measurement area area_downsampled = (area - [x, y]) // patch_size #insider_point = np.zeros(num_labels) index_counter = np.zeros(num_labels) index_interval = np.array(connected_areas) // sample_num for ly in range(labels.shape[0]): for lx in range(labels.shape[1]): label = labels[ly][lx] if area_is_center[label] == False: continue if index_counter[label] == index_interval[label]: index_counter[label] = 0 dist = cv2.pointPolygonTest(area_downsampled, (lx, ly), True) if dist < -15: if show_outlier: cv2.circle(img_contrast, (lx, ly), radius=3, color=255, thickness=1) showimg(img_contrast) area_is_center[label] = False break index_counter[label] = index_counter[label] + 1 #nlargest=heapq.nlargest(2, range(len(connected_areas)), key=connected_areas.__getitem__) largest_label, object_label, findsucceed = findSecondLargeIndexWithMask( connected_areas, area_is_center) #TODO: check aera if largest_label == object_label: print("Segmentation failed, drop image", i_img) continue #showimg(labels) binary = labels == object_label binary = binary.repeat(patch_size, axis=0) binary = binary.repeat(patch_size, axis=1) #binary = binary & tag_mask # delete final 4 row binary = binary[:-shape0_res, :-shape1_res] img_binary = binary.astype(np.uint8) * 255 #median = cv2.medianBlur(img_binary, 33) median = img_binary binary = median > 128 result_img = np.zeros(image_ROI.shape, np.uint8) result_img[binary] = image_ROI[binary] mask_src = np.zeros(image_src.shape, np.uint8) mask_src[y:y + dy, x:x + dx] = binary.astype(int) * 255 # # open and close # from skimage.morphology import opening, closing, erosion, dilation # from skimage.morphology import square # selem = square(30) # selem_open = square(15) # # close # closed = closing(binary, selem) # # open # opened = opening(closed, selem_open) # # close # closed = closing(opened, selem) write_pose(pose_path, i_img_succeed, intrinsicMatrix, rvec, tvec) write_mask(mask_path, i_img_succeed, mask_src) i_img_succeed = i_img_succeed + 1 if show_img: #image_ROI = image_add[y:y+dy, x:x+dx] final_img = np.ones(image_src.shape, np.uint8) final_img[y:y + dy, x:x + dx] = result_img plt.figure(figsize=(30, 90)) plt.subplot(2, 2, 1) plt.imshow(img_contrast, cmap=plt.cm.gray) plt.title('contrast') plt.subplot(2, 2, 2) plt.imshow(labels, cmap=plt.cm.gray) plt.title('labels') plt.subplot(2, 2, 3) image_src = cv2.cvtColor(image_src, cv2.COLOR_BGR2RGB) plt.imshow(image_src, cmap=plt.cm.gray) plt.title('Original') plt.subplot(2, 2, 4) final_img = cv2.cvtColor(final_img, cv2.COLOR_BGR2RGB) plt.imshow(final_img, cmap=plt.cm.gray) plt.title('Result') plt.show() stoppoint = 0 stoppoint = stoppoint + 1
def detect_apriltag(gray, image): global imgCount options = apriltag.DetectorOptions(families="tag36h11") detector = apriltag.Detector(options) #results = detector.detect(img=gray,True, camera_params=[544.021136,542.307110,308.111905,261.603373], tag_size=0.044) results = detector.detect(img=gray) if len(results) > 0: print("[INFO] {} total AprilTags detected".format(len(results))) else: print("No AprilTag Detected") return image, "No AprilTag Detected" info = [] # loop over the AprilTag detection results for r in results: # extract the bounding box (x, y)-coordinates for the AprilTag # and convert each of the (x, y)-coordinate pairs to integers (ptA, ptB, ptC, ptD) = r.corners ptB = (int(ptB[0]), int(ptB[1])) ptC = (int(ptC[0]), int(ptC[1])) ptD = (int(ptD[0]), int(ptD[1])) ptA = (int(ptA[0]), int(ptA[1])) # draw the bounding box of the AprilTag detection cv2.line(image, ptA, ptB, (0, 255, 0), 2) cv2.line(image, ptB, ptC, (0, 255, 0), 2) cv2.line(image, ptC, ptD, (0, 255, 0), 2) cv2.line(image, ptD, ptA, (0, 255, 0), 2) # draw the center (x, y)-coordinates of the AprilTag (cX, cY) = (int(r.center[0]), int(r.center[1])) #cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1) # draw the tag family on the image tagFamily = r.tag_family.decode("utf-8") #print("[INFO] tag family: {}".format(tagFamily)) M, e1, e2 = detector.detection_pose( r, [544.021136, 542.307110, 308.111905, 261.603373]) #w:QR code length w = 4.4 t = [M[0][3] * w, M[1][3] * w, M[2][3] * w] dist = (t[0]**2 + t[1]**2 + t[2]**2)**0.5 showStr = "dist:" + str(dist) cv2.putText(image, showStr, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) print("[INFO] dist:", dist, " tag pose:", t) info.append({"dist": dist, "pose_t": t}) #save some photo ''' if imgCount<50: cv2.imwrite("/home/gry/AprilDistImg/"+str(imgCount)+".jpg",image) imgCount=imgCount+1 print("image saved") ''' return image, str(info)