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)
"""
