def __init__(self): self.targetclass = opt.targetclass self.switch = 1 self.__target_class_sub_ = rospy.Subscriber('detect_item', std_msgs.msg.Int32, self.targetClassCB, queue_size=5) self.__target_position_pub_ = rospy.Publisher( 'detect_item_result', geometry_msgs.msg.PointStamped, queue_size=5) self.__apriltag_switch_sub_ = rospy.Subscriber('apriltag_request', std_msgs.msg.Int32, self.apriltag_switch, queue_size=5) self.__apriltag_pose_pub_ = rospy.Publisher( 'apriltag_pose_result', geometry_msgs.msg.PoseStamped, queue_size=5) self.__pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) # Start streaming self.__pipe_profile = self.__pipeline.start(config) self.at_detector = Detector(families='tag36h11', nthreads=4, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.tags = []
def __init__(self, camera_resolution=(640, 480), camera_framerate=32): self.camera = PiCamera() self.camera.resolution = camera_resolution self.camera.framerate = camera_framerate self.capture = PiRGBArray(self.camera, size=camera_resolution) self.detector = Detector(families="tag36h11") self.show_image = False self.tag_size = params['vision_settings']['tag_size'] self.lens_size = params['vision_settings']['lens_size'] self.camera_x_center = params['vision_settings']['camera_x_center'] self.camera_y_center = params['vision_settings']['camera_y_center']
class Estimator: def __init__(self, intrinsics, tag_size): self._detector = Detector( families="tag36h11", nthreads=4, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.5, debug=0, ) self._camera_params = [ intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy, ] self._tag_size = tag_size def cube_pose(self, image): tags = self._detector.detect( image, estimate_tag_pose=True, camera_params=self._camera_params, tag_size=self._tag_size, ) if len(tags) == 1: euler = mat2euler(tags[0].pose_R) return tags[0].pose_t, euler[2] return None
def __init__(self, intrinsics, tag_size): self._detector = Detector( families="tag36h11", nthreads=4, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.5, debug=0, ) self._camera_params = [ intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy, ] self._tag_size = tag_size
class Vision: def __init__(self, camera_resolution=(640, 480), camera_framerate=32): self.camera = PiCamera() self.camera.resolution = camera_resolution self.camera.framerate = camera_framerate self.capture = PiRGBArray(self.camera, size=camera_resolution) self.detector = Detector(families="tag36h11") self.show_image = False self.tag_size = params['vision_settings']['tag_size'] self.lens_size = params['vision_settings']['lens_size'] self.camera_x_center = params['vision_settings']['camera_x_center'] self.camera_y_center = params['vision_settings']['camera_y_center'] def capture_continuous(self, format="bgr", use_video_port=True): def frame_generator(): for frame in self.camera.capture_continuous(self.capture, format=format, use_video_port=use_video_port): image = frame.array image = cv2.flip(image, -1) self.capture.truncate(0) results = self.detect(image) # for result in results: # (ptA, ptB, ptC, ptD) = result.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(result.center[0]), int(result.center[1])) # cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1) # cv2.imshow("Fiducial Detection Image", image) yield results return frame_generator() def detect(self, img): gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) return self.detector.detect(gray, estimate_tag_pose=True, camera_params=(self.lens_size, self.lens_size, self.camera_x_center, self.camera_y_center), tag_size=self.tag_size)
def computePlane(calMatrix, distCoeffs, img, board, debug=False): at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # Fine imgpoints_fine, objpoints_fine, tagids_fine = detect_aprilboard( img, board, at_detector) if debug: print("fine: {} imgpts, {} objpts".format(len(imgpoints_fine), len(objpoints_fine))) orig = img if len(orig.shape) == 3: img = cv2.cvtColor(orig, cv2.COLOR_RGB2GRAY) else: img = orig # compute normalized image coordinates (equivalent to K^{-1}*x) imgpts_fine_norm = cv2.undistortPoints(imgpoints_fine, calMatrix, distCoeffs) # homographies from each board to normalized image pts H_fine, _ = cv2.findHomography(objpoints_fine[:, :2], imgpts_fine_norm) # extract rotation and translation from homography: fineboard H_fine = 2*H_fine / \ (np.linalg.norm(H_fine[:, 0]) + np.linalg.norm(H_fine[:, 1])) R_fine = np.hstack( (H_fine[:, :2], np.atleast_2d(np.cross(H_fine[:, 0], H_fine[:, 1])).T)) rvec_fine, _ = cv2.Rodrigues(R_fine) R_fine, _ = cv2.Rodrigues(rvec_fine) tvec_fine = H_fine[:, 2:3] # get plane equation which is [r3, -r3.T@T] r3_fine = R_fine[:, 2:3] prod_fine = -r3_fine.T @ tvec_fine plane_fine = np.concatenate((r3_fine, prod_fine), axis=0) return plane_fine
def main(): at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) arm = UArm() arm.connect() time.sleep(2) z_height = 50 for i in range(-300, -200, 50): for j in range(0, 250, 50): print(f"Tring to go to ({i},{j},{z_height}).") arm.set_servo_attach(wait=True) arm.set_position(0, 200, 200, wait=True) arm.set_position(i, j, z_height, wait=True) success = input("Enter y if succesful") if success != 'y': print(f"Skipping point") continue # look for the fiducial tag1 = find_fid(at_detector) if tag1 is False: continue measured_move = tag1.center # now turn off the servos and ask the person to corect the move input("Dropping arm, hit enter when ready.") arm.set_servo_detach() input("Move arm to correct location. Hit enter when done.") print("measuring current location.") tag2 = find_fid(at_detector) if tag2 is False: continue target = tag2.center target_introspective = arm.get_position() data[f"{i}_{j}_{z_height}"] = (target, measured_move, target_introspective) write_data()
def main(): at_detector = Detector(families='tag36h11',nthreads=1,quad_decimate=1.0,quad_sigma=0.0,refine_edges=1,decode_sharpening=0.25,debug=0) arm = UArm() arm.connect() time.sleep(2) for i in range(-300,-200,50): for j in range(0,250,50): z_height=70 print(f"Tring to go to ({i},{j},{z_height}).") arm.set_servo_attach(wait=True) arm.set_position(0,200,200,wait=True) arm.set_position(i,j,z_height,wait=True) success = input("Enter y if succesful") if success != 'y': print(f"Skipping point") continue #drop the height until it reaches the ground while True: stop = input("enter y when the tip is touching the ground") if stop == 'y': break else: z_height -= 5 arm.set_position(i,j,z_height, wait=True) measured_move = (i,j,z_height) # now turn off the servos and ask the person to corect the move input("Dropping arm, hit enter when ready.") arm.set_servo_detach() input("Move arm to correct location. Hit enter when done.") target_introspective = arm.get_position() data.append((measured_move, target_introspective)) write_data()
def __init__(self): rospy.init_node('camera_calib_node') self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.base_frame = "panda_link0" self.eef_frame = "panda_hand" self.pcmsg = None self.imgmsg = None self.tf = None self.bridge = CvBridge() self.detector = Detector() # image_sub = message_filters.Subscriber('/camera/color/image_raw', Image) image_sub = message_filters.Subscriber( '/camera/color/image_rect_color', Image) pc_sub = message_filters.Subscriber('/camera/depth_registered/points', PointCloud2) rospy.sleep(2.0) # subscriber to topic announcing new pose for AT recording s1 = rospy.Subscriber('/record_apriltag', Bool, self.record_new_pose) # save all saved AT positions and hand poses for camera/hand calibration s2 = rospy.Subscriber('/apriltags_done', Bool, self.poses_done) self.flag_record = False # self.pose_pub = rospy.Publisher('/pose', Float64MultiArray, queue_size=1) ts = message_filters.TimeSynchronizer([image_sub, pc_sub], 10) ts.registerCallback(self.callback) self.points = None self.tfs = None
class DetectObject: def __init__(self): self.targetclass = opt.targetclass self.switch = 1 self.__target_class_sub_ = rospy.Subscriber('detect_item', std_msgs.msg.Int32, self.targetClassCB, queue_size=5) self.__target_position_pub_ = rospy.Publisher( 'detect_item_result', geometry_msgs.msg.PointStamped, queue_size=5) self.__apriltag_switch_sub_ = rospy.Subscriber('apriltag_request', std_msgs.msg.Int32, self.apriltag_switch, queue_size=5) self.__apriltag_pose_pub_ = rospy.Publisher( 'apriltag_pose_result', geometry_msgs.msg.PoseStamped, queue_size=5) self.__pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) # Start streaming self.__pipe_profile = self.__pipeline.start(config) self.at_detector = Detector(families='tag36h11', nthreads=4, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) self.tags = [] def __del__(self): # Close the camera self.__pipeline.stop() def targetClassCB(self, msg): # receive target class index and start to detect self.targetclass = msg.data rospy.loginfo('got target class index:%d', self.targetclass) with torch.no_grad(): self.__detect() return def __detect(self, save_img=False): # Declare pointcloud object, for calculating pointclouds and texture mappings pc = rs.pointcloud() # Create an align object # rs.align allows us to perform alignment of depth frames to others frames # The "align_to" is the stream type to which we plan to align depth frames. align_to = rs.stream.color align = rs.align(align_to) frames = self.__pipeline.wait_for_frames() aligned_frames = align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() # time.sleep(8) img_color = np.array(color_frame.get_data()) img_depth = np.array(depth_frame.get_data()) cv2.imwrite(opt.source, img_color) # detection section img_size = ( 320, 192 ) if ONNX_EXPORT else opt.img_size # (320, 192) or (416, 256) or (608, 352) for (height, width) out, source, weights, half, view_img, save_txt = opt.output, opt.source, opt.weights, opt.half, opt.view_img, opt.save_txt webcam = source == '0' or source.startswith( 'rtsp') or source.startswith('http') or source.endswith('.txt') # Initialize device = torch_utils.select_device( device='cpu' if ONNX_EXPORT else opt.device) if os.path.exists(out): shutil.rmtree(out) # delete output folder os.makedirs(out) # make new output folder # Initialize model model = Darknet(opt.cfg, img_size) # Load weights attempt_download(weights) if weights.endswith('.pt'): # pytorch format model.load_state_dict( torch.load(weights, map_location=device)['model']) else: # darknet format load_darknet_weights(model, weights) # Second-stage classifier classify = False if classify: modelc = torch_utils.load_classifier(name='resnet101', n=2) # initialize modelc.load_state_dict( torch.load('weights/resnet101.pt', map_location=device)['model']) # load weights modelc.to(device).eval() # Eval mode model.to(device).eval() # Fuse Conv2d + BatchNorm2d layers # model.fuse() # Export mode if ONNX_EXPORT: model.fuse() img = torch.zeros((1, 3) + img_size) # (1, 3, 320, 192) f = opt.weights.replace(opt.weights.split('.')[-1], 'onnx') # *.onnx filename torch.onnx.export(model, img, f, verbose=False, opset_version=11) # Validate exported model import onnx model = onnx.load(f) # Load the ONNX model onnx.checker.check_model(model) # Check that the IR is well formed print(onnx.helper.printable_graph(model.graph) ) # Print a human readable representation of the graph return # Half precision half = half and device.type != 'cpu' # half precision only supported on CUDA if half: model.half() # Set Dataloader vid_path, vid_writer = None, None if webcam: view_img = True torch.backends.cudnn.benchmark = True # set True to speed up constant image size inference dataset = LoadStreams(source, img_size=img_size) else: save_img = True dataset = LoadImages(source, img_size=img_size) # Get names and colors names = load_classes(opt.names) colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))] # Run inference t0 = time.time() _ = model(torch.zeros( (1, 3, img_size, img_size), device=device)) if device.type != 'cpu' else None # run once for path, img, im0s, vid_cap in dataset: img = torch.from_numpy(img).to(device) img = img.half() if half else img.float() # uint8 to fp16/32 img /= 255.0 # 0 - 255 to 0.0 - 1.0 if img.ndimension() == 3: img = img.unsqueeze(0) # Inference t1 = torch_utils.time_synchronized() pred = model(img, augment=opt.augment)[0] t2 = torch_utils.time_synchronized() # to float if half: pred = pred.float() # Apply NMS pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, multi_label=False, classes=opt.classes, agnostic=opt.agnostic_nms) # Apply Classifier if classify: pred = apply_classifier(pred, modelc, img, im0s) # Process detections for i, det in enumerate(pred): # detections per image if webcam: # batch_size >= 1 p, s, im0 = path[i], '%g: ' % i, im0s[i] else: p, s, im0 = path, '', im0s save_path = str(Path(out) / Path(p).name) s += '%gx%g ' % img.shape[2:] # print string position_result = geometry_msgs.msg.PointStamped() if det is not None and len(det): # Rescale boxes from img_size to im0 size det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round() # Print results for c in det[:, -1].unique(): n = (det[:, -1] == c).sum() # detections per class s += '%g %ss, ' % (n, names[int(c)]) # add to string # stop_num = 0 # print('c = ',len(det)) target_detected = False # Write results for *xyxy, conf, cls in det: # c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3])) # c0 = ((int(xyxy[0])+int(xyxy[2]))/2, (int(xyxy[1])+int(xyxy[3]))/2) if save_txt: # Write to file with open(save_path + '.txt', 'a') as file: file.write( ('%g ' * 6 + '\n') % (*xyxy, cls, conf)) if save_img or view_img: # Add bbox to image label = '%s %.2f' % (names[int(cls)], conf) plot_one_box(xyxy, im0, label=label, color=colors[int(cls)]) # print(c0) if int(cls) == self.targetclass: x_center = int((int(xyxy[0]) + int(xyxy[2])) / 2) y_center = int((int(xyxy[1]) + int(xyxy[3])) / 2) # Intrinsics and Extrinsics depth_intrin = depth_frame.profile.as_video_stream_profile( ).intrinsics color_intrin = color_frame.profile.as_video_stream_profile( ).intrinsics depth_to_color_extrin = depth_frame.profile.get_extrinsics_to( color_frame.profile) # Depth scale: units of the values inside a depth frame, how to convert the value to units of 1 meter depth_sensor = self.__pipe_profile.get_device( ).first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # Map depth to color depth_pixel = [240, 320] # Random pixel depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, depth_pixel, depth_scale) color_point = rs.rs2_transform_point_to_point( depth_to_color_extrin, depth_point) color_pixel = rs.rs2_project_point_to_pixel( color_intrin, color_point) pc.map_to(color_frame) points = pc.calculate(depth_frame) vtx = np.array(points.get_vertices()) tex = np.array(points.get_texture_coordinates()) pix_num = 1280 * y_center + x_center point_cloud_value = [ np.float(vtx[pix_num][0]), np.float(vtx[pix_num][1]), np.float(vtx[pix_num][2]) ] print('point_cloud_value:', point_cloud_value, names[int(cls)], int(cls)) if np.float(vtx[pix_num][2]) > 0.11: position_result.header.frame_id = 'camera_color_optical_frame' position_result.point.x = np.float( vtx[pix_num][0]) position_result.point.y = np.float( vtx[pix_num][1]) position_result.point.z = np.float( vtx[pix_num][2]) self.__target_position_pub_.publish( position_result) # publish the result target_detected = True rospy.loginfo('The target has been detected!') break # only the target class # os.system('rostopic pub -1 /goal_pose geometry_msgs/PointStamped [0,[0,0],zed_left_camera_optical_frame] [%s,%s,%s]' %(np.float(vtx[pix_num][0]),np.float(vtx[pix_num][1]),np.float(vtx[pix_num][2])) # os.system('rostopic pub -1 /start_plan std_msgs/Bool 1') # else: # os.system('rostopic pub -1 /start_plan std_msgs/Bool 0') # print("Can't estimate point cloud at this position.") # stop_num += 1 # if stop_num >= len(det): # os.system('rostopic pub -1 /start_plan std_msgs/Bool 0') if not target_detected: position_result.header.frame_id = 'empty' self.__target_position_pub_.publish( position_result) # publish failure topic rospy.logwarn('Fail to detect the target!') else: position_result.header.frame_id = 'empty' self.__target_position_pub_.publish( position_result) # publish failure topic rospy.logwarn('Fail to detect any target!') # Print time (inference + NMS) print('%sDone. (%.3fs)' % (s, t2 - t1)) # Stream results if view_img: cv2.imshow(p, im0) if cv2.waitKey(1) == ord('q'): # q to quit raise StopIteration # Save results (image with detections) if save_img: if dataset.mode == 'images': cv2.imwrite(save_path, im0) else: if vid_path != save_path: # new video vid_path = save_path if isinstance(vid_writer, cv2.VideoWriter): vid_writer.release( ) # release previous video writer fps = vid_cap.get(cv2.CAP_PROP_FPS) w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH)) h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) vid_writer = cv2.VideoWriter( save_path, cv2.VideoWriter_fourcc(*opt.fourcc), fps, (w, h)) vid_writer.write(im0) if save_txt or save_img: print('Results saved to %s' % os.getcwd() + os.sep + out) if platform == 'darwin': # MacOS os.system('open ' + out + ' ' + save_path) print('Done. (%.3fs)' % (time.time() - t0)) return def apriltag_switch(self, msg): self.switch = msg.data if self.switch == 12: rospy.loginfo('begin to detect small tag') self.__apriltag_detect(0.03) # print('finish a detection') else: rospy.loginfo('begin to detect big tag') self.__apriltag_detect() return def __apriltag_detect(self, tag_size=0.083): # Wait for a coherent pair of color frame frames = self.__pipeline.wait_for_frames() color_frame = frames.get_color_frame() color_image = np.asanyarray(color_frame.get_data()) # get the camera intrinsics color_intrin = color_frame.profile.as_video_stream_profile().intrinsics color_intrin_part = [ color_intrin.fx, color_intrin.fy, color_intrin.ppx, color_intrin.ppy ] gray_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2GRAY) self.tags = self.at_detector.detect(gray_image, estimate_tag_pose=True, camera_params=color_intrin_part, tag_size=tag_size) if self.tags: is_break = False for tag in self.tags: if tag.tag_id == self.switch: r = Rota.from_matrix(tag.pose_R) r_quat = r.as_quat() # r_euler = r.as_euler('zxy', degrees=True) r_pose = tag.pose_t tag_id = tag.tag_id # print(r_pose, tag_id) p1 = geometry_msgs.msg.PoseStamped() # br = tf.TransformBroadcaster() p1.header.frame_id = 'apriltag_' + str(tag_id) p1.pose.orientation.w = r_quat[3] p1.pose.orientation.x = r_quat[0] p1.pose.orientation.y = r_quat[1] p1.pose.orientation.z = r_quat[2] p1.pose.position.x = r_pose[0] p1.pose.position.y = r_pose[1] p1.pose.position.z = r_pose[2] self.__apriltag_pose_pub_.publish(p1) rospy.loginfo('The target april_tag has been detected!') # br.sendTransform((r_pose[0], r_pose[1], r_pose[2]), # p1.pose.orientation(), # rospy.Time.now(), # 'apriltag_' + str(tag_id), # "world") # time.sleep(1) is_break = True break else: continue if not is_break: p2 = geometry_msgs.msg.PoseStamped() p2.header.frame_id = 'empty' p2.pose.orientation.w = 1 p2.pose.orientation.x = 0 p2.pose.orientation.y = 0 p2.pose.orientation.z = 0 p2.pose.position.x = 0 p2.pose.position.y = 0 p2.pose.position.z = 0 self.__apriltag_pose_pub_.publish(p2) rospy.logwarn('Fail to detect target april_tag!') else: p3 = geometry_msgs.msg.PoseStamped() p3.header.frame_id = 'empty' p3.pose.orientation.w = 1 p3.pose.orientation.x = 0 p3.pose.orientation.y = 0 p3.pose.orientation.z = 0 p3.pose.position.x = 0 p3.pose.position.y = 0 p3.pose.position.z = 0 self.__apriltag_pose_pub_.publish(p3) rospy.logwarn('Fail to detect any april_tags!') # print('finish tags', id(tags)) # Release memory is necessary, to solve the segment fault #del at_detector, tags #gc.collect() self.tags.clear() return
''' This script detects an AprilTag on the ground, detects it pose (rotation and translation) w.r.t. camera. Then, homography matrix is constructed using this R and T, which is used to get the ortho view using warpPerspective. ''' import cv2 import matplotlib.pyplot as plt from pupil_apriltags import Detector import numpy as np img = cv2.imread('left0002.jpg', cv2.IMREAD_GRAYSCALE) detector = Detector() # Astra's intrinsic parameters from ROS # result = detector.detect(img, estimate_tag_pose = True, camera_params = [570.34222, 570.34222, 319.50000, 239.50000], tag_size = 0.08) # Astra's intrinsic parameters given by Aman result = detector.detect( img, estimate_tag_pose=True, camera_params=[518.56666108, 518.80466479, 329.45801792, 237.05589955], tag_size=0.08) R = result[0].pose_R # scaling is required in the translation T = result[0].pose_t * 1000 # creating intermediate homography matrix H' by replacing 3rd column of R with T R[:, 2] = np.transpose(T)
left_name = file_name[i] # find right right_name = file_name[i + len(file_name) // 2] img_left = cv2.imread(left_name, cv2.IMREAD_GRAYSCALE) img_right = cv2.imread(right_name, cv2.IMREAD_GRAYSCALE) # append to pair list img_pair.append((img_left, img_right)) src = [] # left dst = [] # right detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) for pair in img_pair: left = pair[0] right = pair[1] tag_left = detector.detect(left) tag_right = detector.detect(right) if len(tag_left) != len(tag_right): continue print("%d apriltags have been detected." % len(tag_left)) for tag in tag_left: # cv2.circle(left, tuple(tag.corners[0].astype(int)), 4, (255, 0, 0), 2) # left-top # cv2.circle(left, tuple(tag.corners[1].astype(int)), 4,(255,0,0), 2) # right-top # cv2.circle(left, tuple(tag.corners[2].astype(int)), 4,(255,0,0), 2) # right-bottom
def detect_tags( frames_path: str, aperture=11, visualize=False) -> Tuple[List[List[Dict[str, Any]]], Dict[int, int]]: """Detect all tags (Apriltags3) found in a folder of PNG files and return (1) a list of tag objects for preprocessing and (2) a dictionary containing the frequency that each tag ID appeared Args: frames_path (str): path to the directory containing PNG images aperture (int): visualize (bool): Returns: frames (List[Dict[str, Any]]): list of objects containing id (int), centroid (np.array[int]) and corners (np.array[int]) tag_ds (Dict[int, int]): dictionary mapping tag IDs to frequency of tag across all images """ # Initialize variables frames = [] tag_ids = defaultdict(int) at_detector = Detector() # Sort by index in.../frame<index>.png all_images = sorted(glob(f'{frames_path}/*.png'), key=lambda f: int(os.path.basename(f)[5:-4])) # Deleted last image after out of range error popped up # TODO: but not analyzing last 2 frames? all_images = all_images[:-1] num_images = len(all_images) print_progress_bar(0, num_images, prefix='Progress:', suffix='Complete', length=50) # Iterate thru all PNG images in frames_path for i, img_path in enumerate(all_images): # Create a grayscale 2D NumPy array for Detector.detect() img = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE) if type(img) == np.ndarray: tags_in_framex = [] for tag in at_detector.detect(img): # Increment frequency tag_ids[tag.tag_id] += 1 # r = np.roll(np.float32(img), tag.homography + 1, axis=0) # Add to frames in following format - feel free to adjust tags_in_framex.append({ 'id': tag.tag_id, 'id_confidence': tag.decision_margin, 'soft_id': tag.tag_id, 'perimeter': 100, #cv2.arcLength(img, closed=True), 'centroid': tag.center, 'verts': tag.corners, 'frames_since_true_detection': 0 }) # {'id': msg, 'id_confidence': id_confidence, 'verts': r.tolist(), 'soft_id': soft_msg, # 'perimeter': cv2.arcLength(r, closed=True), 'centroid': centroid.tolist(), # "frames_since_true_detection": 0} frames.append(tags_in_framex) time.sleep(0.01) print_progress_bar(i + 1, num_images, prefix='Progress:', suffix='Complete', length=50) return frames, dict(tag_ids)
def camera_callback(self, data): cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) twist_object = Twist() # initial cmd_vel twist_object.linear.x = 0 twist_object.angular.z = 0 # We select bgr8 because its the OpneCV encoding by default cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding="bgr8") height, width, channels = cv_image.shape # tss = (width/1280)*2.1 # itss = int((width/1280)*2.5) # mll = int(height/4) #Mask uper limit from bottom ## AR TAG DETECTION AND CONTROL height, width, channels = cv_image.shape # frame = cv_image[int(height/2):int(height)][1:int(width)] frame = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # April tag detector # detector = apriltag.Detector() detector = Detector() result = detector.detect(frame) tag_detected = False cx = 0 cy = 0 if len(result) == 1: tag_detected = True tag = result[0].tag_family cx = result[0].center[0] cy = result[0].center[1] corner_points = result[0].corners print(cx, cy) if len(result) > 1: print("Multiple tags detected") if tag_detected: print("Tag detected") global kp, kd, pre_error, temp # Control err = 0 if tag_detected: err = cx - width / 2 + 250 if err > -5 and err < 5: #avoid steady state oscillation err = 0 twist_object.angular.z = np.clip( (-float(err) * kp / 100 + kd * (-err + pre_error) / 100), -0.2, 0.2) twist_object.linear.x = 0.1 pre_error = temp print("Mode: Tag following") # cv_image = cv2.putText(cv_image,'Mode: Tag detected',(int(20*tss),int(23*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.5*tss,(0,0,255),1*itss, cv2.LINE_AA) if tag_detected == False: twist_object.linear.x = 0 twist_object.angular.z = 0 print("Mode: Tag finding") # cv_image = cv2.putText(cv_image,'Mode: Tag finding',(int(20*tss),int(23*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.5*tss,(0,0,255),1*itss, cv2.LINE_AA) # time global t1 t0 = float(rospy.Time.now().to_sec()) timestep = (-t1 + t0) t1 = t0 # Show image # cv_image = cv2.rectangle(cv_image,(int(10*tss),int(5*tss)),(int(290*tss),int(100*tss)),(0,250, 0),2*int(tss)) if tag_detected: tlx_point = int(corner_points[0, 0]) tly_point = int(corner_points[0, 1]) brx_point = int(corner_points[2, 0]) bry_point = int(corner_points[2, 1]) cv_image = cv2.rectangle(cv_image, (tlx_point, tly_point), (brx_point, bry_point), (0, 165, 255), 1 * int(1000)) side_length = np.sqrt( ((tlx_point - brx_point) * (tlx_point - brx_point) + (tly_point - bry_point) * (tly_point - bry_point)) / 2) z_distance = (75 * 160 / side_length) * 75 / 44 #(160/270) msg5 = str("Z distance is " + str(int(z_distance)) + "cm") # cv_image = cv2.putText(cv_image,msg5,(int(tlx_point-side_length),int(tly_point-20)),cv2.FONT_HERSHEY_SIMPLEX,0.4*tss,(0,0,255),1*itss, cv2.LINE_AA) if z_distance < 15: twist_object.linear.x = np.clip((float(z_distance - 38) / 100), -0.1, 0.1) if z_distance < 5: twist_object.linear.x = 0 twist_object.angular.z = 0 print(" Too close to tag, stopped turtlebot") # cv_image = cv2.putText(cv_image,'Too close to tag',(int(20*tss),int(120*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.5*tss,(0,0,255),1*itss, cv2.LINE_AA) # msg1 = str("Lane error " + str(int(err*100/width))+" %") # cv_image = cv2.putText(cv_image,msg1,(int(20*tss),int(45*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.4*tss,(0,255,0),1*itss, cv2.LINE_AA) # msg2 = str("Linear velocity " + str(float(int(1000*twist_object.linear.x))/1000)) # cv_image = cv2.putText(cv_image,msg2,(int(20*tss),int(60*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.4*tss,(0,255,0),1*itss, cv2.LINE_AA) # msg3 = str("Angular velocity " + str(float(int(1000*twist_object.angular.z))/1000)) # cv_image = cv2.putText(cv_image,msg3,(int(20*tss),int(75*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.4*tss,(0,255,0),1*itss, cv2.LINE_AA) # msg4 = str("Update time (ms) " + str((int(1000*timestep)))) # cv_image = cv2.putText(cv_image,msg4,(int(20*tss),int(90*tss)),cv2.FONT_HERSHEY_SIMPLEX,0.4*tss,(0,255,0),1*itss, cv2.LINE_AA) # Print # print("\n Angular value sent=> "+str(float(int(1000*twist_object.angular.z))/1000)) # print(" Linear value sent=> "+str(float(int(1000*twist_object.linear.x))/1000)) # print(" Lane error => "+str(int(err*100/width))+" %") # print(" cx => "+str(float(int(100*cx))/100)+"\n cy => "+str(float(int(100*cy))/100)) # print(" Timestep (sec) => "+str(float(int(1000*timestep))/1000)) # print(" Update rate => "+str(int(1/timestep))) if tag_detected: print(corner_points) print(" Tag side length = ", side_length) print(" Tag z disatnce = ", z_distance) print(" \n") cv2.namedWindow("Apriltag follower", cv2.WINDOW_NORMAL) cv2.imshow("Apriltag follower", cv_image) cv2.resizeWindow("Apriltag follower", (500, 500)) cv2.waitKey(1) # Debug #twist_object.linear.x = 0 #twist_object.angular.z = 0 # Make it start turning self.moveTurtlebot3_object.move_robot(twist_object) print("\n")
from pupil_apriltags import Detector import cv2 import time t1 = time.time() img = cv2.imread("DockTag2.PNG") print(img.shape) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=2.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) results = at_detector.detect(gray, estimate_tag_pose=True, camera_params=[800.5335,801.2600,313.2403,231.1194], tag_size=0.20) 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(img, ptA, ptB, (0, 255, 0), 2) cv2.line(img, ptB, ptC, (0, 255, 0), 2) cv2.line(img, ptC, ptD, (0, 255, 0), 2)
import cv2 import matplotlib.pyplot as plt from pupil_apriltags import Detector import numpy as np cap = cv2.VideoCapture('apriltag_ground/output.avi') fourcc = cv2.VideoWriter_fourcc(*'XVID') writer = cv2.VideoWriter('out.avi', fourcc, 15, (2000, 2000)) detector = Detector() while (cap.isOpened()) : success, img = cap.read() if success : # using the homography matrix calculated using AprilTag on the ground H = np.array([[-6.13630839e+02, -2.81610381e+01, 2.51096776e+05], [-1.43915902e+02, 1.58572394e+02, 2.71513537e+05], [-5.75208061e-01, 8.16663809e-01, 6.86058534e+02]]) # translation to get proper view t = np.array([[1, 0, 1500], [0, 1, 350], [0, 0, 1]]) out = cv2.warpPerspective(img, [email protected](H), (2000, 2000)) writer.write(out) else : break writer.release() cap.release() cv2.destroyAllWindows()
def test(): at_detector = Detector( families="tag36h11", nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.5, debug=0, ) camera_params = [ 425.40704345703125, 425.40704345703125, 421.0938720703125, 246.84486389160156 ] tag_size = 0.030 centers = [] angles = [] for i in range(200): image = cv2.imread( f"/scratch/azimov/igor/data/physics/tmp/{i:04d}.png", 0) tags = at_detector.detect( image, estimate_tag_pose=True, camera_params=camera_params, tag_size=tag_size, ) if len(tags) == 1: centers.append(tags[0].pose_t) euler = mat2euler(tags[0].pose_R) print(euler) angles.append(euler[2]) # print(tags) print(f"{i:04d} / {200:04d}: {len(tags)}") for i in range(1, len(angles)): a0 = angles[i - 1] a1 = angles[i] if a0 - a1 > 270 - 30: angles[i] += 270 elif a0 - a1 > 180 - 30: angles[i] += 180 elif a0 - a1 > 90 - 30: angles[i] += 90 elif a1 - a0 > 270 - 30: angles[i] -= 270 elif a1 - a0 > 180 - 30: angles[i] -= 180 elif a1 - a0 > 90 - 30: angles[i] -= 90 times = [i / 90.0 for i in rage(200)] plt.figure() plt.subplot(2, 1, 1) plt.plot(times, [c[0] for c in centers], '-o', label="x") plt.plot(times, [c[1] for c in centers], '-o', label="y") # plt.plot([c[2] for c in centers], '-*', label="z") plt.legend() plt.subplot(2, 1, 2) plt.plot(times, angles, '-o', label="angle") plt.legend() plt.show() data = dict(time=times, center=centers, angle=np.rad2deg(angles).tolist()) json_name = file_name.replace('.bag', '.json') with open(json_name, 'w') as f: json.dump(data, f)
if sub in events: #Recoit des messages pour vider le cache msg = sub.recv_multipart() print(msg) topic = sub.recv_string() payload = unpackb(sub.recv(), raw=False) extra_frames = [] while sub.get(zmq.RCVMORE): extra_frames.append(sub.recv()) if extra_frames: payload['__raw_data__'] = extra_frames return topic, payload at_detector = Detector(families='tag36h11',nthreads=1,quad_decimate=1.0,quad_sigma=0.2, refine_edges=1, decode_sharpening=0.25, debug=0) recent_world = None recent_eye0 = None recent_eye1 = None i=0 indice=0 try: while True: topic, msg = recv_from_sub() if topic == 'frame.world': recent_world = np.frombuffer(msg['__raw_data__'][0], dtype=np.uint8,count=msg['height']*msg['width']).reshape(msg['height'], msg['width']) elif topic == 'frame.eye.0':
def detect_tags_and_surfaces(frames_path: str, createSurfaceFrame=False, tags=None, tags_corner_attribute=None): """Detect all tags (Apriltags3) & surfaces found in a folder of PNG files and return (1) a list of tag objects for preprocessing, (2) a dictionary containing the frequency that each tag ID appeared, (3) a dataframe consisting of all surface coordinates associated with each frame Args: frames_path (str): path to the directory containing PNG images createSurfaceFrame (bool): True to create surface_frames directory with annotated frames tags(List[int]): ids from bottom-left corner, counter-clockwise --> 1 surface tags_corner_attribute (List[bool]): order corresponds to tags, True if tag is corner Returns: frames (List[Dict[str, Any]]): list of objects containing id (int), centroid (np.array[int]) and corners (np.array[int]) tag_ids (Dict[int, int]): dictionary mapping tag IDs to frequency of tag across all images coordinates_df (DataFrame): dataframe that lists the coordinates of the corners & center """ if tags_corner_attribute is None: tags_corner_attribute = [ True, False, False, True, False, True, False, False, True, False ] if tags is None: tags = [2, 3, 5, 6, 7, 8, 9, 11, 0, 1] if len(tags) != len(tags_corner_attribute): logging.warning( 'tags_corner_attribute variable should match tags to describe if corner' ) # Initialize variables frames = [] tag_ids = defaultdict(int) at_detector = Detector() img_n_total = [] starting_frame = False img_n = [] norm_top_left_corner_x = [] norm_top_right_corner_x = [] norm_bottom_left_corner_x = [] norm_bottom_right_corner_x = [] norm_center_x = [] norm_top_left_corner_y = [] norm_top_right_corner_y = [] norm_bottom_left_corner_y = [] norm_bottom_right_corner_y = [] norm_center_y = [] # Sort by index in.../frame<index>.png all_images = sorted(glob(f'{frames_path}/*.png'), key=lambda f: int(os.path.basename(f)[5:-4])) # Deleted last image after out of range error popped up if len(all_images) > 1: all_images = all_images[:-1] # Iterate thru all PNG images in frames_path for i, img_path in enumerate(all_images): # Create a grayscale 2D NumPy array for Detector.detect() img = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE) if type(img) == np.ndarray: tags_in_framex = [] for tag in at_detector.detect(img): tags_in_framex = detect_tags_in_framex(tag_ids, tag, tags_in_framex) frames.append(tags_in_framex) # all frame numbers head, tail = os.path.split(img_path) img_n_total.append(int(''.join(list(filter(str.isdigit, tail))))) # detect surfaces once the first frame with all tags are identified if len(tags_in_framex) == len(tags): starting_frame = True if starting_frame: # frame number img_n.append(int(''.join(list(filter(str.isdigit, tail))))) # define surface norm_tl, norm_tr, norm_bl, norm_br, norm_c = norm_surface_coordinates( frames, img_path, tags, tags_corner_attribute, createSurfaceFrame) norm_top_left_corner_x.append(norm_tl[0]) norm_top_right_corner_x.append(norm_tr[0]) norm_bottom_left_corner_x.append(norm_bl[0]) norm_bottom_right_corner_x.append(norm_br[0]) norm_center_x.append(norm_c[0]) norm_top_left_corner_y.append(norm_tl[1]) norm_top_right_corner_y.append(norm_tr[1]) norm_bottom_left_corner_y.append(norm_bl[1]) norm_bottom_right_corner_y.append(norm_br[1]) norm_center_y.append(norm_c[1]) time.sleep(0.01) print_progress_bar(i + 1, len(all_images), prefix='Progress:', suffix='Complete', length=50) # coordinates dataframe output surface_coords = { 'image': img_n, 'timestamp': detect_timestamp_to_frame(frames_path, img_n_total, img_n), 'norm_top_left_x': norm_top_left_corner_x, 'norm_bottom_left_x': norm_bottom_left_corner_x, 'norm_bottom_right_x': norm_bottom_right_corner_x, 'norm_top_right_x': norm_top_right_corner_x, 'norm_center_x': norm_center_x, 'norm_top_left_y': norm_top_left_corner_y, 'norm_bottom_left_y': norm_bottom_left_corner_y, 'norm_bottom_right_y': norm_bottom_right_corner_y, 'norm_top_right_y': norm_top_right_corner_y, 'norm_center_y': norm_center_y } coordinates_df = pd.DataFrame(surface_coords) return frames, dict(tag_ids), coordinates_df
thickness=3) return color_img # Create a Tag object TAG_SIZE = .123 FAMILIES = "tagStandard41h12" tags = Tag(TAG_SIZE, FAMILIES) # Add information about tag locations # Function Arguments are id,x,y,z,theta_x,theta_y,theta_z tags.add_tag(1, 115., 31.5, 0., 0., 0., 0.) tags.add_tag(2, 95.75, 50., 0., 0., 0., 0.) # Create Detector detector = Detector(families=tags.family, nthreads=4) camera_info = {} # Camera Resolution camera_info["res"] = RES # Camera Intrinsic Matrix (3x3) camera_info["K"] = np.array([[313.11130800756115, 0.0, 336.11351317641487], [0.0, 310.34427179740504, 239.24222723346466], [0.0, 0.0, 1.0]]) # The non-default elements of the K array, in the AprilTag specification camera_info["params"] = [313.111, 310.344, 336.114, 239.242] # Fisheye Camera Distortion Matrix camera_info["D"] = np.array([[-0.03574382363559852], [0.0028133336786254765], [-0.007814648102960479], [0.003381442340208307]]) # Fisheye flag camera_info["fisheye"] = True
# Tag information TAG_SIZE = .123 FAMILIES = "tagStandard41h12" tags = Tag(TAG_SIZE, FAMILIES) # Add information about tag locations # Function Arguments are id,x,y,z,theta_x,theta_y,theta_z tags.add_tag(0, 76.25, 30.5, 0., 0., 0., 0.) tags.add_tag(1, 115., 31.5, 0., 0., 0., 0.) tags.add_tag(2, 95.75, 50., 0., 0., 0., 0.) tags.add_tag(3, 0., 41., 38.75, 0., -np.pi / 2, 0.) tags.add_tag(4, 0., 54., 19.25, 0., -np.pi / 2, 0.) starting_position = np.array([[.635], [1.0668], [2.7432]]) # Create Apriltags Detector detector = Detector(families=tags.family, nthreads=4) # Create the camera object with PiCamera() as camera: camera.resolution = RES camera.framerate = FPS # Reduce the shutter speed to reduce blur, given in microseconds camera.shutter_speed = int(1000000 / (3 * FPS)) # Create the stream object stream = Stream(detector, camera_info, tags, starting_position, args.run_name) sleep(1) print("Starting") try: # Start recording frames to the stream object camera.start_recording(stream, format='yuv')
def calibrateCamera(video, board, debug=False): # initialize 3D object points and 2D image points calObjPoints = [] calImgPoints = [] # set up april tag detector (I use default parameters; seems to be OK) at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # pick out 10 frames for camera calibration frames = np.random.choice(video.shape[2], NUM_REFERENCE_FRAMES, replace=False) if debug: # create figure to plot detected tags fig, axs = plt.subplots(np.ceil(NUM_REFERENCE_FRAMES / 2).astype(int), 2, figsize=(15, 50)) for count, ind in enumerate(frames): img = video[:, :, ind] if debug: # show image axs.reshape(-1)[count].imshow(img / 255.0, cmap="gray") axs.reshape(-1)[count].set_axis_off() axs.reshape(-1)[count].set_title("Image {}".format(count)) # detect apriltags and report number of detections imgpoints, objpoints, tagIDs = detect_aprilboard( img, board, at_detector) if debug: print("{} {}: {} imgpts, {} objpts".format(count, ind, len(imgpoints), len(objpoints))) # append detections if some are found if len(imgpoints) and len(objpoints): if debug: # display detected tag centers axs.reshape(-1)[count].scatter(imgpoints[:, 0], imgpoints[:, 1], marker='o', color='#ff7f0e') # append points detected in all images, (there is only one image now) calObjPoints.append(objpoints.astype('float32')) calImgPoints.append(imgpoints.astype('float32')) if debug: plt.show() # convert to numpy array calObjPoints = np.array(calObjPoints) calImgPoints = np.array(calImgPoints) # calibrate the camera reprojerr, calMatrix, distCoeffs, calRotations, calTranslations = cv2.calibrateCamera( calObjPoints, calImgPoints, img.shape, # image H,W for initialization of the principal point None, # no initial guess for the remaining entries of calMatrix None, # initial guesses for distortion coefficients are all 0 flags=None) # default contstraints (see documentation) if debug: # Print reprojection error. calibrateCamera returns the root mean square (RMS) re-projection error in pixels. # Bad calibration if this value if too big print('RMSE of reprojected points:', reprojerr) print('Distortion coefficients:', distCoeffs) print('Intrinsic camera matrix', calMatrix) return (reprojerr, calMatrix, distCoeffs, calRotations, calTranslations)
import numpy as np import cv2 parser = argparse.ArgumentParser( description='Detect apriltags and save the detection in a file') parser.add_argument('-i', '--input', help='input folder', default='undistorted') args = parser.parse_args() at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) imgs = glob.glob(args.input + '/*.jpg') if not os.path.exists(args.input + '/detected'): os.makedirs(args.input + '/detected') for img_f in imgs: img = cv2.imread(img_f, cv2.IMREAD_GRAYSCALE) tags = at_detector.detect(img, estimate_tag_pose=False, camera_params=None, tag_size=None)
class EyeTracker(): ## # # @param self Le pointeur vers l'objet EyeTracker # @param tracker_id int : identifiant du tracker au sein du Human # @param human_id int : identifiant du Human porteur du tracker # @param port int : numéro du port de communication du tracker # # @brief Constructeur de classe def __init__(self, tracker_id, human_id, port): # Verification des types assert (type(tracker_id) == int) assert (type(human_id) == int) assert (type(port) == int) # Creation des attributs self.id = tracker_id self.human_id = human_id self.port = port self.detected_tags = {} self.gaze_in_cam = Vector(0.0, 0.0, 1.0) #Parametres de la caméra frontale du tracker # mtx : matrice intrinsèque # h : hauteur de l'image en nombre de pixels # w : largeur de l'image en nombre de pixels # dist : paramètres de distorsion de l'image self.__mtx = Matrix([[1.600e+03, 0.00000000e+00, 6.40e+02], [0.00000000e+00, 1.600e+03, 3.60e+02], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) #self.__mtx = Matrix([[829.3510515270362, 0.0, 659.9293047259697],[0.0, 799.5709408845464, 373.0776462356668],[0.0, 0.0, 1.0]]) self.__h = 720 self.__w = 1280 self.__dist = np.array([[ -0.43738542863224966, 0.190570781428104, -0.00125233833830639, 0.0018723428760170056, -0.039219091259637684 ]]) # Communication avec le tracker self.__context = zmq.Context() self.__addr = '127.0.0.1' # remote ip ou localhost self.__req = self.__context.socket(zmq.REQ) self.__req.connect("tcp://{}:{}".format(self.__addr, str(self.port))) self.__req.send_string('SUB_PORT') # Demande du port de requete self.__sub_port = self.__req.recv_string() # Stockage port de requete # Démarrer le frame publisher au format Noir et Blanc self.notify({ 'subject': 'start_plugin', 'name': 'Frame_Publisher', 'args': { 'format': 'gray' } }) # Ouvert d'un port de souscription(sub) pour ecouter le tracker (frames) self.__sub_frame = self.__context.socket(zmq.SUB) self.__sub_frame.connect("tcp://{}:{}".format(self.__addr, self.__sub_port)) # Recevoir uniquement les notifications concernant les frames self.__sub_frame.setsockopt_string(zmq.SUBSCRIBE, 'frame.world') # Ouvert d'un port de souscription(sub) pour ecouter le tracker (gaze) self.__sub_gaze = self.__context.socket(zmq.SUB) self.__sub_gaze.connect("tcp://{}:{}".format(self.__addr, self.__sub_port)) # Recevoir uniquement les notifications concernant les frames self.__sub_gaze.setsockopt_string(zmq.SUBSCRIBE, 'gaze.2d.0') # Thread lecteur de frames en continu self.__frames_reader = threading.Thread(target=self.always_recv_frame, args=(), daemon=True) self.__frames_lock = threading.Lock() self.__available_frame = False self.__frames_reader.start() # Thread lecteur de gaze en continu self.__gaze_reader = threading.Thread(target=self.always_recv_gaze, args=(), daemon=True) self.__gaze_lock = threading.Lock() self.__available_gaze = False self.__gaze_reader.start() # Détecteur de tags self.__at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) ## # # @param self Le pointeur vers l'objet EyeTracker # # @brief Lit la dernièere frame reçue par le 'sub' et la stocke def always_recv_frame(self): # Vide le cache : on lit 10 frames sans les traiter # Nombre a adapter aux performances de l'ordinateur print("start reader") while (True): try: topic = self.__sub_frame.recv_string() payload = unpackb(self.__sub_frame.recv(), raw=False) extra_frames = [] while self.__sub_frame.get(zmq.RCVMORE): extra_frames.append(self.__sub_frame.recv()) if extra_frames: payload['_raw_data_'] = extra_frames with self.__frames_lock: self.__payload = payload self.__available_frame = True except: pass ## # # @param self Le pointeur vers l'objet EyeTracker # # @brief Lit le dernier message regard reçu par le 'sub' et le stocke def always_recv_gaze(self): # Vide le cache : on lit 10 frames sans les traiter # Nombre a adapter aux performances de l'ordinateur print("start reader") while (True): try: topic, payload = self.__sub_gaze.recv_multipart() message = msgpack.loads(payload) with self.__gaze_lock: self.__normalized_pos = [ message[b'norm_pos'][0], message[b'norm_pos'][1] ] self.__available_gaze = True except: pass ## # # @param self Le pointeur vers l'objet EyeTracker # @param notification dict : Le message à envoyer # # @brief Envoie un message à l'API de l'Eye Tracker def notify(self, notification): topic = 'notify.' + notification['subject'] payload = packb(notification, use_bin_type=True) self.__req.send_string(topic, flags=zmq.SNDMORE) self.__req.send(payload) return self.__req.recv_string() ## # # @param self Le pointeur vers l'objet EyeTracker # # @brief Verifie la connection avec l'eye-tracker def checkConnection(self): pass ## # # @param self Le pointeur vers l'objet EyeTracker # @param size_tag float : taille réelle d'un tag (en mètre) # # @brief Analyse l'image actuelle # # Capture l'image frontale récente, détecte les tags présents et met # à jour le dict "detected_tags" def updateFrame(self, size_tag): # Récupération de la frame with self.__frames_lock: if (self.__available_frame == True): msg = self.__payload else: return with self.__gaze_lock: if (self.__available_gaze == True): norm_pos = self.__normalized_pos else: return # Position regard dans caméra fx = self.__mtx[0, 0] fy = self.__mtx[1, 1] cx = self.__mtx[0, 2] cy = self.__mtx[1, 2] self.gaze_in_cam = Vector(norm_pos[0] * self.__w / fx - cx / fx, (1 - norm_pos[1]) * self.__h / fy - cy / fy, 1.0) #Image caméra frontale en noir et blanc recent_world = np.frombuffer(msg['_raw_data_'][0], dtype=np.uint8, count=msg['height'] * msg['width']).reshape( msg['height'], msg['width']) tags = self.__at_detector.detect(recent_world, estimate_tag_pose=True, camera_params=[ self.__mtx[0, 0], self.__mtx[1, 1], self.__mtx[0, 2], self.__mtx[1, 2] ], tag_size=size_tag) # Remplissage du dictionnaire de tags self.detected_tags = {} id_in_dict = 0 for tag in tags: # Caractéristiques du tag détecté tag_family = str(tag.tag_family) tag_family_id = tag.tag_id translation = tag.pose_t rotation = tag.pose_R # Ajout du tag dans le dictionnaire self.detected_tags[id_in_dict] = DetectedTag( tag_family, tag_family_id, id_in_dict, Vector(translation[0, 0], translation[1, 0], translation[2, 0]), Matrix(rotation.tolist())) # Incrémentation de l'indice id_in_dict += 1 ## # # @param self Le pointeur vers l'objet EyeTracker # # @brief Affichage de la configuration actuelle de l'EyeTracker # # Affichage du port et de la liste des AprilTags détectés def showEyeTracker(self): print("") print("Eye Tracker - ", self.id) print("Human id - ", self.human_id) print("Port - ", self.port) print("Detected Tags - ", self.detected_tags)
'tag_id': detection['tag_id'], 'hamming': detection['hamming'], 'decision_margin': detection['decision_margin'], 'homography': detection['homography'].tolist(), 'center': detection['center'].tolist(), 'corners': detection['corners'].tolist(), 'pose_R': detection['pose_R'].tolist(), 'pose_t': detection['pose_t'].tolist(), 'pose_err': detection['pose_err'], } at_detector = Detector( #searchpath=['apriltags/lib', 'apriltags/lib64'], families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # img = cv2.imread('test_image_multiple_01.png', cv2.IMREAD_GRAYSCALE) # tags = at_detector.detect(img, estimate_tag_pose=True, camera_params=camera_params, tag_size=0.08) # print(tags) # tags = [conver_detection_to_dict(detection) for detection in tags] # print(tags) context = zmq.Context() sockImage = context.socket(zmq.SUB) sockImage.connect('tcp://192.168.50.111:5560') sockImage.setsockopt(zmq.RCVHWM, 1) sockImage.subscribe('')
def __init__(self, tracker_id, human_id, port): # Verification des types assert (type(tracker_id) == int) assert (type(human_id) == int) assert (type(port) == int) # Creation des attributs self.id = tracker_id self.human_id = human_id self.port = port self.detected_tags = {} self.gaze_in_cam = Vector(0.0, 0.0, 1.0) #Parametres de la caméra frontale du tracker # mtx : matrice intrinsèque # h : hauteur de l'image en nombre de pixels # w : largeur de l'image en nombre de pixels # dist : paramètres de distorsion de l'image self.__mtx = Matrix([[1.600e+03, 0.00000000e+00, 6.40e+02], [0.00000000e+00, 1.600e+03, 3.60e+02], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) #self.__mtx = Matrix([[829.3510515270362, 0.0, 659.9293047259697],[0.0, 799.5709408845464, 373.0776462356668],[0.0, 0.0, 1.0]]) self.__h = 720 self.__w = 1280 self.__dist = np.array([[ -0.43738542863224966, 0.190570781428104, -0.00125233833830639, 0.0018723428760170056, -0.039219091259637684 ]]) # Communication avec le tracker self.__context = zmq.Context() self.__addr = '127.0.0.1' # remote ip ou localhost self.__req = self.__context.socket(zmq.REQ) self.__req.connect("tcp://{}:{}".format(self.__addr, str(self.port))) self.__req.send_string('SUB_PORT') # Demande du port de requete self.__sub_port = self.__req.recv_string() # Stockage port de requete # Démarrer le frame publisher au format Noir et Blanc self.notify({ 'subject': 'start_plugin', 'name': 'Frame_Publisher', 'args': { 'format': 'gray' } }) # Ouvert d'un port de souscription(sub) pour ecouter le tracker (frames) self.__sub_frame = self.__context.socket(zmq.SUB) self.__sub_frame.connect("tcp://{}:{}".format(self.__addr, self.__sub_port)) # Recevoir uniquement les notifications concernant les frames self.__sub_frame.setsockopt_string(zmq.SUBSCRIBE, 'frame.world') # Ouvert d'un port de souscription(sub) pour ecouter le tracker (gaze) self.__sub_gaze = self.__context.socket(zmq.SUB) self.__sub_gaze.connect("tcp://{}:{}".format(self.__addr, self.__sub_port)) # Recevoir uniquement les notifications concernant les frames self.__sub_gaze.setsockopt_string(zmq.SUBSCRIBE, 'gaze.2d.0') # Thread lecteur de frames en continu self.__frames_reader = threading.Thread(target=self.always_recv_frame, args=(), daemon=True) self.__frames_lock = threading.Lock() self.__available_frame = False self.__frames_reader.start() # Thread lecteur de gaze en continu self.__gaze_reader = threading.Thread(target=self.always_recv_gaze, args=(), daemon=True) self.__gaze_lock = threading.Lock() self.__available_gaze = False self.__gaze_reader.start() # Détecteur de tags self.__at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0)
#imports import pyuarm import numpy as np import cv2 import numpy as np from PIL import Image, ImageOps, ImageDraw, ImageFont from pupil_apriltags import Detector import time import math import subprocess from picamera import PiCamera from undistort import undistort_img import pickle as pkl at_detector = Detector(families='tag36h11',nthreads=1,quad_decimate=1.0,quad_sigma=0.0,refine_edges=1,decode_sharpening=0.25,debug=0) class HIRO(): pixel_scalar = 0.00115964489 #0.001209 pixel_intercept = 0.062 move_calibration_file = "random_forest_locomotion_regressor.pkl" view_calibration_file = "random_forest_view_regressor.pkl" def __init__(self, mute=False, projector=True): #uArm self.arm = pyuarm.UArm(debug=False,mac_address='FC:45:C3:24:76:EA', ble=False) self.arm.connect() self.speed = 200 # speed limit self.ground = 30 # z value to touch suction cup to ground self.position = np.array([[0],[150],[150]]) # default start position
import cv2 import numpy as np from pupil_apriltags import Detector img = cv2.imread('images/track.jpeg') img = cv2.resize(img, (0, 0), fx=0.2, fy=0.2) at_detector = Detector(families='tagStandard41h12', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) results = at_detector.detect(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)) # img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) result = results[1] points = np.array(result.corners) pts1 = np.float32([points[3], points[2], points[1], points[0]]) # [top left, top right, bottom left, bottom right] pts2 = np.float32([[0, 50], [0, 0], [50, 0], [50, 50]]) # [top left, top right, bottom left, bottom right] # for i, pt in enumerate(pts1): # img = cv2.putText(img, str(i), tuple(pt.astype(np.int)), cv2.FONT_HERSHEY_COMPLEX, 0.5, (255, 0, 0), 1) cv2.imshow('img1', img) h = cv2.getPerspectiveTransform(pts1, pts2) img3 = cv2.warpPerspective(img, np.dot(np.array([[1, 0, 25], [0, 1, 25], [0, 0, 1]]), h), (2000, 2000), cv2.INTER_CUBIC) cv2.imshow('img3', img3) """