def main(serial):
# Read camera parameters
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(serial)
# Set up webcam
cap = utils.get_video_cap(serial, image_width, image_height)
# Set up aruco dict
aruco_dict = cv2.aruco.Dictionary_get(utils.get_marker_dict_id())
while True:
if cv2.waitKey(1) == 27: # Esc key
break
image = None
while image is None:
_, image = cap.read()
# Undistort image and detect markers
image = cv2.undistort(image, camera_matrix, dist_coeffs)
corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict)
# Show detections
image_copy = image.copy()
if ids is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners, ids)
cv2.imshow('out', image_copy)
cap.release()
cv2.destroyAllWindows()
def __init__(self, serial):
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(serial)
self._map_x, self._map_y = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, camera_matrix, (image_width, image_height), cv2.CV_32FC1)
self._cap = utils.get_video_cap(serial, image_width, image_height)
self._queue = Queue(maxsize=1)
self._thread = Thread(target=self._worker)
self._thread.start()
def main(args):
# Read camera parameters
camera_params_file_path = utils.get_camera_params_file_path(
args.camera_name)
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(
camera_params_file_path)
# Set up webcam
cap = utils.get_video_cap(image_width, image_height, args.camera_id)
# Set up aruco dict
params = utils.get_marker_parameters()
aruco_dict = cv2.aruco.Dictionary_get(params['dict_id'])
# Enable corner refinement
#detector_params = cv2.aruco.DetectorParameters_create()
#detector_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
while True:
if cv2.waitKey(1) == 27: # Esc key
break
_, image = cap.read()
if image is None:
continue
# Undistort image and detect markers
image = cv2.undistort(image, camera_matrix, dist_coeffs)
#corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict, parameters=detector_params)
corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict)
# Show detections
image_copy = image.copy()
if ids is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners, ids)
cv2.imshow('out', image_copy)
cap.release()
cv2.destroyAllWindows()
Ping pong game
"""
import sys
import cv2
import numpy as np
import obj_loader
from utils import (init_game, draw_game, update_game, load_ref_images,
get_camera_params, get_homographies_contour)
if __name__ == "__main__":
REF_IMAGES, REF_DSC = load_ref_images()
VID_FEED = cv2.VideoCapture(-1)
CAM_MAT = get_camera_params()
GAME_STATE = None
flag_g = True
flag_b = True
MATCH_DATA = [None, None]
CORNERS = [None, None]
while True:
RET, FRAME = VID_FEED.read()
if not RET:
print("Unable to capture video")
sys.exit()
elif GAME_STATE is None:
SIZE = FRAME.shape
GAME_STATE = init_game(SIZE)
def extract_features_labelled(drc, config_file, dir_labels, mhad_color_map,
subjects, actions, recordings):
# Fetch Camera parameters
fx1, fy1, cx1, cy1, fx2, fy2, cx2, cy2, h1, h2 = get_camera_params(
config_file)
for sub in range(1, subjects + 1):
for act in range(1, actions + 1):
for rec in range(1, recordings + 1):
try:
skeleton_file = drc + 'Skeletons/skl_s%02d_a%02d_r%02d.mat' % (
sub, act, rec)
im_mc, im_mc2, skel_jnt, center, stand = get_skeleton_info(
skeleton_file)
print '\nExtraction of PCL in subject ' + str(
sub) + ' action ' + str(act) + ' recording ' + str(rec)
for i in tqdm(range(min(im_mc.shape[1], im_mc2.shape[1]))):
try:
# if not os.path.exists(out_pc_name):
d1 = misc.imread(
'%s/Kinect/Kin01/S%02d/A%02d/R%02d/kin_k01_s%02d_a%02d_r%02d_depth_%05d.pgm'
% (drc, sub, act, rec, sub, act, rec, i))
d2 = misc.imread(
'%s/Kinect/Kin02/S%02d/A%02d/R%02d/kin_k02_s%02d_a%02d_r%02d_depth_%05d.pgm'
% (drc, sub, act, rec, sub, act, rec, i))
# Apply inverse projection to fetch point clouds in mm and corresponding indices
pts1, indices1, mask1 = map_range_2_pc(d1,
fx1,
fy1,
cx1,
cy1,
h1,
center,
stand,
thresh=5)
pts2, indices2, mask2 = map_range_2_pc(d2,
fx2,
fy2,
cx2,
cy2,
h2,
center,
stand,
thresh=5)
skel = skel_jnt.T
jnt = np.reshape(skel[int(im_mc[2, i]) + 1, :],
(35, 3)).T
# For combined PC
# out_pc_name = os.path.join(OUT_PC_DIR,
# 'sub_%02d_act_%02d_rec_%02d_cap_%05d.pkl' % (
# sub, act, rec, i))
# For single view
out_pc1_name = os.path.join(
OUT_PC1_DIR,
'sub_%02d_act_%02d_rec_%02d_cap_cam01_%05d.pkl'
% (sub, act, rec, i))
out_pc2_name = os.path.join(
OUT_PC2_DIR,
'sub_%02d_act_%02d_rec_%02d_cap_cam02_%05d.pkl'
% (sub, act, rec, i))
# To store the GT Joint locations
# out_gt_name = os.path.join(OUT_GT_DIR,
# 'sub_%02d_act_%02d_rec_%02d_cap_%05d.pkl' % (
# sub, act, rec, i))
# pickle.dump(jnt, open(out_gt_name, 'wb'))
# plot_basic_object(pts1[[1, 2, 0], :], jnt)
# plot_basic_object(pts2[[1, 2, 0], :], jnt)
#
# pickle.dump(pts1[[1, 2, 0], :], open(out_pc1_name, 'wb'))
# pickle.dump(pts2[[1, 2, 0], :], open(out_pc2_name, 'wb'))
# full_pc = np.hstack([pts1, pts2])
# full_pc = full_pc[[1, 2, 0], :]
#
# pickle.dump(full_pc, open(out_pc_name, 'wb'))
except (IndexError, IOError) as ee:
print '\nNumber ' + str(i) + ' image is empty!'
except IOError:
print '\nFile does not exist!'
return 0
def extract_features_raw(drc, config_file, subjects, actions, recordings):
feature_dictionary = dict(
(str(k).decode(), []) for k in range(1, subjects + 1))
gt_dictionary = dict((str(k).decode(), []) for k in range(1, subjects + 1))
# Fetch Camera parameters
fx1, fy1, cx1, cy1, fx2, fy2, cx2, cy2, h1, h2 = utils.get_camera_params(
config_file)
for sub in range(1, subjects + 1):
# Declare Feature and ground truth lists
feature, gt = np.array([]), np.array([])
for act in range(1, actions + 1):
for rec in range(1, recordings + 1):
try:
skeleton_file = drc + 'Skeletons/skl_s%02d_a%02d_r%02d.mat' % (
sub, act, rec)
im_mc, im_mc2, skel_jnt, center, stand = utils.get_skeleton_info(
skeleton_file)
print '\nExtraction of features in subject ' + str(
sub) + ' action ' + str(act) + ' recording ' + str(rec)
for i in tqdm(range(min(im_mc.shape[1], im_mc2.shape[1]))):
try:
# Read Depth data from two kinects
d1 = misc.imread(
'%s/Kinect/Kin01/S%02d/A%02d/R%02d/kin_k01_s%02d_a%02d_r%02d_depth_%05d.pgm'
% (drc, sub, act, rec, sub, act, rec, i))
d2 = misc.imread(
'%s/Kinect/Kin02/S%02d/A%02d/R%02d/kin_k02_s%02d_a%02d_r%02d_depth_%05d.pgm'
% (drc, sub, act, rec, sub, act, rec, i))
# Apply inverse projection to fetch point clouds in cm
pts1, __, __ = utils.map_range_2_pc(d1,
fx1,
fy1,
cx1,
cy1,
h1,
center,
stand,
thresh=5)
pts2, __, __ = utils.map_range_2_pc(d2,
fx2,
fy2,
cx2,
cy2,
h2,
center,
stand,
thresh=5)
full_pc = np.hstack([pts1, pts2])
full_pc = full_pc[[1, 2, 0], :]
plotxyz(full_pc.T, color='b', hold=False)
skel = skel_jnt.T
jnt = np.reshape(skel[int(im_mc[2, i]) + 1, :],
(35, 3)).T
plotxyz(jnt.T, color='r', hold=False)
# nearest neighbour
diff = full_pc.T - np.reshape(jnt, (3, 1, 35)).T
dfsq = np.sqrt(np.sum((diff**2), axis=2))
label_idx = np.argmin(dfsq, axis=0)
full_pcl = np.vstack([
full_pc,
np.reshape(label_idx, (1, label_idx.shape[0]))
])
# store joint-wise features
feature_image = np.array([])
for joint in range(jnt.shape[1]):
ind_seg = full_pcl[3, :] == joint
pcl_joint = full_pcl[0:3, ind_seg]
pcl_joint[np.isnan(pcl_joint)] = 0
# extract moments
med_joint = np.median(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
med_joint[np.isnan(med_joint)] = 0
std_joint = np.std(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
std_joint[np.isnan(std_joint)] = 0
min_joint = np.min(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
min_joint[np.isnan(min_joint)] = 0
max_joint = np.max(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
max_joint[np.isnan(max_joint)] = 0
cov_joint = np.cov(
pcl_joint) if pcl_joint.size else np.zeros(
(3, 3))
cov_joint[np.isnan(cov_joint)] = 0
eig_joint = np.linalg.eigvals(cov_joint)
eig_joint[np.isnan(eig_joint)] = 0
feature_joint = np.concatenate([
np.ravel(med_joint),
np.ravel(std_joint),
np.ravel(min_joint),
np.ravel(max_joint),
np.ravel(eig_joint),
np.ravel(cov_joint),
],
axis=0)
feature_image = np.concatenate([np.ravel(feature_image), np.ravel(feature_joint)]) \
if feature_image.size else feature_joint
feature = np.vstack([
feature, feature_image
]) if feature.size else feature_image
# print feature.shape
gt = np.vstack([gt, np.ravel(jnt)
]) if gt.size else np.ravel(jnt)
except IndexError:
print '\nNumber ' + str(i) + ' image is empty!'
except IOError:
print '\nFile does not exist!'
feature_dictionary[str(sub)].append(feature)
gt_dictionary[str(sub)].append(gt)
return feature_dictionary, gt_dictionary
def extract_features_labelled(drc, config_file, dir_labels, mhad_color_map,
subjects, actions, recordings):
# Fetch Camera parameters
fx1, fy1, cx1, cy1, fx2, fy2, cx2, cy2, h1, h2 = utils.get_camera_params(
config_file)
for sub in range(1, subjects + 1):
feature_dictionary = {str(sub).decode(): []}
gt_dictionary = {str(sub).decode(): []}
# Declare Feature and ground truth lists
feature, gt = np.array([]), np.array([])
for act in range(1, actions + 1):
for rec in range(1, recordings + 1):
try:
skeleton_file = drc + 'Skeletons/skl_s%02d_a%02d_r%02d.mat' % (
sub, act, rec)
im_mc, im_mc2, skel_jnt, center, stand = utils.get_skeleton_info(
skeleton_file)
print '\nExtraction of features in subject ' + str(
sub) + ' action ' + str(act) + ' recording ' + str(rec)
for i in tqdm(range(min(im_mc.shape[1], im_mc2.shape[1]))):
try:
# Read Depth data from two kinects
d1 = misc.imread(
'%s/Kinect/Kin01/S%02d/A%02d/R%02d/kin_k01_s%02d_a%02d_r%02d_depth_%05d.pgm'
% (drc, sub, act, rec, sub, act, rec, i))
d2 = misc.imread(
'%s/Kinect/Kin02/S%02d/A%02d/R%02d/kin_k02_s%02d_a%02d_r%02d_depth_%05d.pgm'
% (drc, sub, act, rec, sub, act, rec, i))
# read in the color data
il1 = misc.imread(
'%s/Kin01/S%02d/A%02d/R%02d/kin_k01_s%02d_a%02d_r%02d_depth_%05d.png'
%
(dir_labels, sub, act, rec, sub, act, rec, i))
il2 = misc.imread(
'%s/Kin02/S%02d/A%02d/R%02d/kin_k02_s%02d_a%02d_r%02d_depth_%05d.png'
%
(dir_labels, sub, act, rec, sub, act, rec, i))
# Apply inverse projection to fetch point clouds in mm and corresponding indices
pts1, indices1, mask1 = utils.map_range_2_pc(
d1,
fx1,
fy1,
cx1,
cy1,
h1,
center,
stand,
thresh=5)
pts2, indices2, mask2 = utils.map_range_2_pc(
d2,
fx2,
fy2,
cx2,
cy2,
h2,
center,
stand,
thresh=5)
idx_mask1, idx_mask2 = (mask1 == 0), (mask2 == 0)
il1[idx_mask1], il2[idx_mask2] = 0, 0
# get pixel level classes for all the body parts (+ Background)
lbl1 = utils.get_pixel_level_classes(
il1, mhad_color_map)
lbl2 = utils.get_pixel_level_classes(
il2, mhad_color_map)
label_idx1 = np.zeros(indices1.shape[0])
label_idx2 = np.zeros(indices2.shape[0])
for index in range(indices1.shape[0]):
label_idx1[index] = lbl1[indices1[index, 1],
indices1[index, 0]]
for index in range(indices2.shape[0]):
label_idx2[index] = lbl2[indices2[index, 1],
indices2[index, 0]]
full_pc = np.hstack([pts1, pts2])
full_pc = full_pc[[1, 2, 0], :]
# plotxyz(full_pc.T, color='b', hold=False)
skel = skel_jnt.T
jnt = np.reshape(skel[int(im_mc[2, i]) + 1, :],
(35, 3)).T
# plotxyz(jnt.T, color='r', hold=False)
label_idx = np.hstack([label_idx1, label_idx2])
full_pcl = np.vstack([
full_pc,
np.reshape(label_idx, (1, label_idx.shape[0]))
])
# store joint-wise features
feature_image = np.array([])
for joint in range(jnt.shape[1]):
ind_seg = full_pcl[3, :] == joint
pcl_joint = full_pcl[0:3, ind_seg]
pcl_joint[np.isnan(pcl_joint)] = 0
# extract moments
med_joint = np.median(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
med_joint[np.isnan(med_joint)] = 0
std_joint = np.std(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
std_joint[np.isnan(std_joint)] = 0
min_joint = np.min(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
min_joint[np.isnan(min_joint)] = 0
max_joint = np.max(
pcl_joint,
1) if pcl_joint.size else np.zeros(3)
max_joint[np.isnan(max_joint)] = 0
cov_joint = np.cov(
pcl_joint) if pcl_joint.size else np.zeros(
(3, 3))
cov_joint[np.isnan(cov_joint)] = 0
eig_joint = np.linalg.eigvals(cov_joint)
eig_joint[np.isnan(eig_joint)] = 0
feature_joint = np.concatenate([
np.ravel(med_joint),
np.ravel(std_joint),
np.ravel(min_joint),
np.ravel(max_joint),
np.ravel(eig_joint),
np.ravel(cov_joint),
],
axis=0)
feature_image = np.concatenate([np.ravel(feature_image), np.ravel(feature_joint)]) \
if feature_image.size else feature_joint
feature = np.vstack([
feature, feature_image
]) if feature.size else feature_image
# print feature.shape
gt = np.vstack([gt, np.ravel(jnt)
]) if gt.size else np.ravel(jnt)
except (IndexError, IOError) as ee:
print '\nNumber ' + str(i) + ' image is empty!'
except IOError:
print '\nFile does not exist!'
feature_dictionary[str(sub)].append(feature)
pickle.dump(feature_dictionary, pickle_fv_inferred)
gt_dictionary[str(sub)].append(gt)
pickle.dump(gt_dictionary, pickle_gt_inferred)
return feature_dictionary, gt_dictionary
import obj_loader
from utils import (draw_harris_kps, draw_rectangle, get_camera_params,
get_homographies_contour, get_matrix, load_ref_images,
render, draw_corners)
RECTANGLE = False # Display bounding rectangle or not
DRAW_MATCHES = False # Draw matches
DRAW_HARRIS = False
if __name__ == "__main__":
OBJ_PATH = sys.argv[1]
OBJ = obj_loader.OBJ(OBJ_PATH, swapyz=True)
REF_IMAGES, REF_DSC = load_ref_images(1)
CAM_PARAMS = get_camera_params()
VID_FEED = cv2.VideoCapture(-1)
MATCH_DATA = [None, None]
while True:
RET, FRAME = VID_FEED.read()
if not RET:
print("Unable to capture video")
sys.exit()
if DRAW_HARRIS:
FRAME = draw_harris_kps(FRAME)
# MATCH_DATA = find_homographies(REF_DSC, FRAME)
MATCH_DATA, _ = get_homographies_contour(FRAME, REF_IMAGES, MATCH_DATA,
None)
def main(args):
# Read camera parameters
camera_params_file_path = utils.get_camera_params_file_path(
args.camera_name)
image_width, image_height, camera_matrix, dist_coeffs = utils.get_camera_params(
camera_params_file_path)
# Set up webcam
cap = utils.get_video_cap(image_width, image_height, args.camera_id)
# Board and marker params
boards = [{
'name': 'robots',
'corner_offset_mm': 36
}, {
'name': 'cubes',
'corner_offset_mm': 12
}]
marker_params = utils.get_marker_parameters()
room_length_mm = 1000 * args.room_length
room_width_mm = 1000 * args.room_width
room_length_pixels = int(room_length_mm * marker_params['pixels_per_mm'])
room_width_pixels = int(room_width_mm * marker_params['pixels_per_mm'])
# Set up aruco dicts
for board in boards:
aruco_dict = cv2.aruco.Dictionary_get(marker_params['dict_id'])
aruco_dict.bytesList = aruco_dict.bytesList[utils.get_marker_ids(
'corners_{}'.format(board['name']))]
board['board_dict'] = aruco_dict
aruco_dict = cv2.aruco.Dictionary_get(marker_params['dict_id'])
aruco_dict.bytesList = aruco_dict.bytesList[utils.get_marker_ids(
board['name'])]
board['marker_dict'] = aruco_dict
# Board warping
for board in boards:
corner_offset_pixels = board['corner_offset_mm'] * marker_params[
'pixels_per_mm']
board['src_points'] = None
board['dst_points'] = [
[-corner_offset_pixels, -corner_offset_pixels],
[room_length_pixels + corner_offset_pixels, -corner_offset_pixels],
[
room_length_pixels + corner_offset_pixels,
room_width_pixels + corner_offset_pixels
],
[-corner_offset_pixels, room_width_pixels + corner_offset_pixels]
]
# Enable corner refinement
detector_params = cv2.aruco.DetectorParameters_create()
detector_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
# Set up server
address = ('localhost', 6000)
listener = Listener(address, authkey=b'secret password')
conn = None
def process_image(image):
# Undistort image
image = cv2.undistort(image, camera_matrix, dist_coeffs)
data = {}
for board in boards:
board_name = board['name']
data[board_name] = {}
if board['src_points'] is None:
# Detect board markers (assumes board won't move since this is only done once)
#board_corners, board_indices, _ = cv2.aruco.detectMarkers(image, board['board_dict'], parameters=detector_params)
board_corners, board_indices, _ = cv2.aruco.detectMarkers(
image, board['board_dict'])
# Show detections
if args.debug:
image_copy = image.copy()
if board_indices is not None:
cv2.aruco.drawDetectedMarkers(image_copy,
board_corners,
board_indices)
cv2.imshow('{}_board_corners'.format(board_name),
image_copy)
# Ensure board was found
if board_indices is None or len(board_indices) < 4:
data[
board_name] = None # None rather than {} to signify board was not detected
continue
board['src_points'] = []
for marker_index, corner in sorted(
zip(board_indices, board_corners)):
board['src_points'].append(
corner.squeeze(0).mean(axis=0).tolist())
else:
# Warp the board
M = cv2.getPerspectiveTransform(
np.asarray(board['src_points'], dtype=np.float32),
np.asarray(board['dst_points'], dtype=np.float32))
warped_image = cv2.warpPerspective(
image, M, (room_length_pixels, room_width_pixels))
# Detect markers in warped image
corners, indices, _ = cv2.aruco.detectMarkers(
warped_image,
board['marker_dict'],
parameters=detector_params)
# Show detections
if args.debug:
image_copy = warped_image.copy()
if indices is not None:
cv2.aruco.drawDetectedMarkers(image_copy, corners,
indices)
image_copy = cv2.resize(image_copy,
(int(image_copy.shape[1] / 2),
int(image_copy.shape[0] / 2)))
cv2.imshow(board_name, image_copy)
if indices is None:
continue
# Compute poses
board_data = {}
for marker_index, corner in zip(indices, corners):
marker_index = marker_index.item()
marker_corners = corner.squeeze(0)
marker_center = marker_corners.mean(axis=0)
# Compute heading estimates for each of the four marker corners
diffs = [c - marker_center for c in marker_corners]
angles = np.array(
[np.arctan2(-diff[1], diff[0]) for diff in diffs])
angles = angles + np.radians([-135, -45, 45, 135])
angles = np.mod(angles + np.pi, 2 * np.pi) - np.pi
# If multiple markers are detected on same cube, use the marker on top (which should have the lowest angle_std)
angle_std = angles.std()
if board_name == 'cubes' and marker_index in board_data and angle_std > board_data[
marker_index]['angle_std']:
continue
# Compute position and heading
position = [
(marker_center[0] / marker_params['pixels_per_mm'] -
room_length_mm / 2) / 1000,
(room_width_mm / 2 -
(marker_center[1] / marker_params['pixels_per_mm'])) /
1000
]
heading = angles.mean()
marker_data = {'position': position, 'heading': heading}
if board_name == 'cubes':
marker_data['angle_std'] = angle_std
board_data[marker_index] = marker_data
data[board_name] = board_data
return data
while True:
if cv2.waitKey(1) == 27: # Esc key
break
if conn is None:
print('Waiting for connection...')
conn = listener.accept()
print('Connected!')
_, image = cap.read()
if image is None:
continue
data = process_image(image)
try:
conn.send(data)
except:
conn = None
cap.release()
cv2.destroyAllWindows()
