def get_joint_coordinates(list_of_joints, reference_join, reference_join_up, reference_join_left, reference_join_right, joint_indexes = [], n_time = 0.0, local=False): t0 = time.time() # get joints to compute if(joint_indexes): joints_to_compute = [] for index in joint_indexes: joints_to_compute.append(cp.deepcopy(list_of_joints[index])) else: joints_to_compute = list_of_joints coords = np.zeros(len(joints_to_compute) * 3) if local is False: for idx,joint in enumerate(joints_to_compute): coords[idx*3] = joint.get_WorldJoint()[0] coords[(idx*3)+1] = joint.get_WorldJoint()[1] coords[(idx*3)+2] = joint.get_WorldJoint()[2] else: #translation translation_vector = np.array([-reference_join.get_WorldJoint()[0], -reference_join.get_WorldJoint()[1], -reference_join.get_WorldJoint()[2]]) # print(translation_vector) for joint in joints_to_compute: point = np.array(joint.get_WorldJoint()) transformed_point = point + translation_vector joint.set_WorldJoint(transformed_point.item(0),transformed_point.item(1),transformed_point.item(2)) # # X = right_hip to left_hip # # Y = bottom_center to spine # (allmost perpendicular to alpha) # x_vector = np.array(reference_join_left.get_WorldJoint()) - np.array(reference_join_right.get_WorldJoint()) y_vector = np.array(reference_join_up.get_WorldJoint()) - np.array(reference_join.get_WorldJoint()) # calculate z z_vector = np.cross(x_vector,y_vector) # recalculate y so it is perpendicular to XZ plane y_vector = np.cross(z_vector,x_vector) for idx,joint in enumerate(joints_to_compute): x,y,z = transform_coordinate_linear(x_vector,y_vector,z_vector,joint.get_WorldJoint()) coords[idx*3] = x coords[(idx*3)+1] = y coords[(idx*3)+2] = z t1 = time.time() n_time += t1 - t0 return coords,n_time
def compute_hoj3d(list_of_joints,
reference_join,
reference_join_up,
reference_join_left,
reference_join_right,
joint_indexes=[],
use_triangle_function=False,
n_time=0.0):
t0 = time.time()
# bins for alpha, the horizontal angle, starting from
alpha_bin = [0, 30, 60, 90, 120, 150, 180, 210, 240, 270, 300, 330, 360]
# bins tor theta, the vertical angle, starting from north pole
theta_bin = [-15, 15, 45, 75, 105, 135, 165, 195]
# the historamm of joints 3D
hoj3d = [[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
], [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
], [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
], [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
], [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
], [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
], [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]]
# get joints to compute
if (joint_indexes):
joints_to_compute = []
for index in joint_indexes:
joints_to_compute.append(cp.deepcopy(list_of_joints[index]))
else:
joints_to_compute = list_of_joints
# assign probability function
probability_function = p_function
if (use_triangle_function):
probability_function = trinangle_function
# calculate radius for the inner zylinder
# sholder elbow
left_upper_arm = np.array(list_of_joints[4].get_WorldJoint()) - np.array(
list_of_joints[5].get_WorldJoint())
# elbow hand
left_under_arm = np.array(list_of_joints[5].get_WorldJoint()) - np.array(
list_of_joints[6].get_WorldJoint())
left_arm = ma.sqrt((left_upper_arm * left_upper_arm).sum()) + ma.sqrt(
(left_under_arm * left_under_arm).sum())
# sholder elbow
right_upper_arm = np.array(list_of_joints[8].get_WorldJoint()) - np.array(
list_of_joints[9].get_WorldJoint())
# elbow hand
right_under_arm = np.array(list_of_joints[9].get_WorldJoint()) - np.array(
list_of_joints[10].get_WorldJoint())
right_arm = ma.sqrt((right_upper_arm * right_upper_arm).sum()) + ma.sqrt(
(right_under_arm * right_under_arm).sum())
cut_radius = ((left_arm + right_arm) / 2)
# print('cut_radius='+str(cut_radius))
#translation
translation_vector = np.array([
-reference_join.get_WorldJoint()[0],
-reference_join.get_WorldJoint()[1],
-reference_join.get_WorldJoint()[2]
])
# print(translation_vector)
for joint in joints_to_compute:
point = np.array(joint.get_WorldJoint())
transformed_point = point + translation_vector
joint.set_WorldJoint(transformed_point.item(0),
transformed_point.item(1),
transformed_point.item(2))
#
# X = right_hip to left_hip
#
# Y = bottom_center to spine
# (allmost perpendicular to alpha)
#
x_vector = np.array(reference_join_left.get_WorldJoint()) - np.array(
reference_join_right.get_WorldJoint())
y_vector = np.array(reference_join_up.get_WorldJoint()) - np.array(
reference_join.get_WorldJoint())
# calculate z
z_vector = np.cross(x_vector, y_vector)
# recalculate y so it is perpendicular to XZ plane
y_vector = np.cross(z_vector, x_vector)
for joint in joints_to_compute:
# Catch the zero division raised by NaNs in the skeleton files
with warnings.catch_warnings():
warnings.filterwarnings('error')
try:
r, flat_r, alpha, theta = transform_coordinate(
x_vector, y_vector, z_vector, joint.get_WorldJoint())
except Warning:
return [], -1
inner, outer = r_function(r, cut_radius)
j = 0
for t in theta_bin:
if (t >= 180):
break
# find theta-bins to calculate
if ((theta - t > 60) or (theta - t < -30)):
j += 1
continue
i = 0
for a in alpha_bin:
if (a == 360):
break
# find alpha-bins to calculate
if ((alpha - a <= 60) and (alpha - a >= -30)):
probability = abs((probability_function(
alpha, (alpha_bin[i + 1] + alpha_bin[i]) / 2))) * abs(
(probability_function(
theta, (theta_bin[j + 1] + theta_bin[j]) / 2)))
# print(probability)
hoj3d[j][(i * 2)] += probability * inner
hoj3d[j][(i * 2) + 1] += probability * outer
# wrap around the sphere
if ((alpha + 360 - a <= 60) or (alpha - 360 - a >= -30)):
if (alpha < 30):
probability = abs((probability_function(
alpha + 360,
(alpha_bin[i + 1] + alpha_bin[i]) / 2))) * abs(
(probability_function(
theta,
(theta_bin[j + 1] + theta_bin[j]) / 2)))
# print(probability)
hoj3d[j][(i * 2)] += probability * inner
hoj3d[j][(i * 2) + 1] += probability * outer
elif (alpha > 330):
probability = abs((probability_function(
alpha - 360,
(alpha_bin[i + 1] + alpha_bin[i]) / 2))) * abs(
(probability_function(
theta,
(theta_bin[j + 1] + theta_bin[j]) / 2)))
# print(probability)
hoj3d[j][(i * 2)] += probability * inner
hoj3d[j][(i * 2) + 1] += probability * outer
i += 1
j += 1
# debug print
#np.set_printoptions(precision = 3,suppress = True)
#print(np.array(hoj3d))
#hoj = np.array(hoj3d)
#print(hoj.sum())
t1 = time.time()
n_time += t1 - t0
return hoj3d, n_time
def compute_hoj3d(list_of_joints,
reference_join,
reference_join_up,
reference_join_left,
reference_join_right,
joint_indexes=[],
use_triangle_function=False,
n_time=0.0):
t0 = time.time()
# bins for alpha, the horizontal angle, starting from
alpha_bin = [0, 30, 60, 90, 120, 150, 180, 210, 240, 270, 300, 330, 360]
# bins tor theta, the vertical angle, starting from north pole
theta_bin = [-15, 15, 45, 75, 105, 135, 165, 195]
# the historamm of joints 3D
hoj3d = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
# get joints to compute
if (joint_indexes):
joints_to_compute = []
for index in joint_indexes:
joints_to_compute.append(list_of_joints[index])
else:
joints_to_compute = list_of_joints
# assign probability function
probability_function = p_function
if (use_triangle_function):
probability_function = trinangle_function
#translation
translation_vector = np.array([
-reference_join.get_WorldJoint()[0],
-reference_join.get_WorldJoint()[1],
-reference_join.get_WorldJoint()[2]
])
# print(translation_vector)
for joint in joints_to_compute:
point = np.array(joint.get_WorldJoint())
transformed_point = point + translation_vector
joint.set_WorldJoint(transformed_point.item(0),
transformed_point.item(1),
transformed_point.item(2))
#
# X = right_hip to left_hip
#
# Y = bottom_center to spine
# (allmost perpendicular to alpha)
#
x_vector = np.array(reference_join_left.get_WorldJoint()) - np.array(
reference_join_right.get_WorldJoint())
y_vector = np.array(reference_join_up.get_WorldJoint()) - np.array(
reference_join.get_WorldJoint())
# calculate z
z_vector = np.cross(x_vector, y_vector)
# recalculate y so it is perpendicular to XZ plane
y_vector = np.cross(z_vector, x_vector)
for joint in joints_to_compute:
r, alpha, theta = transform_coordinate(x_vector, y_vector, z_vector,
joint.get_WorldJoint())
j = 0
for t in theta_bin:
if (t >= 180):
break
# find theta-bins to calculate
if ((theta - t > 60) or (theta - t < -30)):
j += 1
continue
i = 0
for a in alpha_bin:
if (a == 360):
break
# find alpha-bins to calculate
if ((alpha - a <= 60) and (alpha - a >= -30)):
probability = abs((probability_function(
alpha, (alpha_bin[i + 1] + alpha_bin[i]) / 2))) * abs(
(probability_function(
theta, (theta_bin[j + 1] + theta_bin[j]) / 2)))
# print(probability)
hoj3d[j][i] += probability
# wrap around the sphere
if ((alpha + 360 - a <= 60) or (alpha - 360 - a >= -30)):
if (alpha < 30):
probability = abs((probability_function(
alpha + 360,
(alpha_bin[i + 1] + alpha_bin[i]) / 2))) * abs(
(probability_function(
theta,
(theta_bin[j + 1] + theta_bin[j]) / 2)))
# print(probability)
hoj3d[j][i] += probability
elif (alpha > 330):
probability = abs((probability_function(
alpha - 360,
(alpha_bin[i + 1] + alpha_bin[i]) / 2))) * abs(
(probability_function(
theta,
(theta_bin[j + 1] + theta_bin[j]) / 2)))
# print(probability)
hoj3d[j][i] += probability
i += 1
j += 1
# debug print
#np.set_printoptions(precision = 3,suppress = True)
#print(np.array(hoj3d))
#hoj = np.array(hoj3d)
#print(hoj.sum())
t1 = time.time()
n_time += t1 - t0
return hoj3d, n_time
def get_distance_descriptor(list_of_joints, joint_indexes = [], n_time = 0.0, local=False): t0 = time.time() # get joints to compute if(joint_indexes): joints_to_compute = [] for index in joint_indexes: joints_to_compute.append(cp.deepcopy(list_of_joints[index])) else: joints_to_compute = list_of_joints # Compute a local skeleton ( reference joint is the mid between hipl and hipr ) if local is True: #translation translation_vector = np.array([-reference_join.get_WorldJoint()[0], -reference_join.get_WorldJoint()[1], -reference_join.get_WorldJoint()[2]]) # print(translation_vector) for joint in joints_to_compute: point = np.array(joint.get_WorldJoint()) transformed_point = point + translation_vector joint.set_WorldJoint(transformed_point.item(0),transformed_point.item(1),transformed_point.item(2)) # Compute output vector. coords = np.zeros(len(joints_to_compute) * ( len(joints_to_compute) ) ) # Compute joint to joint distance descriptor. # Run through all joints for idx_host,joint_host in enumerate(joints_to_compute): host_vec = np.array(joint_host.get_WorldJoint()) # Compute for each joint ( except with itself ) a descriptor entry # containing euclidean distances to each other joint. ( TODO Later followed by an oriantation ??? ) # n joints x n-1 distances = ((n^2)-n) view independend entrys guest_index = 0 for idx_guest,joint_guest in enumerate(joints_to_compute): # # As long as host and guest are different entities. # if idx_host != idx_guest: guest_vec = np.array(joint_guest.get_WorldJoint()) # Compute euclidean between host and guest. dist = np.linalg.norm( host_vec - guest_vec ) # Store joint to joint distance. coords[idx_host * ( len(joints_to_compute) -1 ) + guest_index] = dist guest_index = guest_index + 1 # TODO Include local transformation later if necessary. # x,y,z = transform_coordinate_linear(x_vector,y_vector,z_vector,joint.get_WorldJoint()) t1 = time.time() n_time += t1 - t0 return coords,n_time
def compute_hoj3d( list_of_joints, reference_join, reference_join_up, reference_join_left, reference_join_right, joint_indexes = [], use_triangle_function = False, n_time = 0.0, _depth_measurement = None, _body_parts = False):
t0 = time.time()
# bins for alpha, the horizontal angle, starting from
alpha_bin = [0,30,60,90,120,150,180,210,240,270,300,330,360]
# bins tor theta, the vertical angle, starting from north pole
theta_bin = [-15,15,45,75,105,135,165,195]
# the historamm of joints 3D
hoj3d = np.zeros((7,12))
if _depth_measurement is not None:
hoj3d = np.zeros((7,24))
# get joints to compute
if(joint_indexes):
joints_to_compute = []
for index in joint_indexes:
joints_to_compute.append(cp.deepcopy(list_of_joints[index]))
else:
joints_to_compute = list_of_joints
# assign probability function
probability_function = p_function
if(use_triangle_function):
probability_function = trinangle_function
# assign hoj add function
hoj_add_function = add_to_hoj_without_depth
if _depth_measurement is not None:
hoj_add_function = add_to_hoj_with_depth
# calculate bodyparts
arm_length, left_hand_sholder, right_hand_sholder = calculate_arm_length(list_of_joints)
leg_length, left_foot_hip, right_foot_hip = calculate_leg_length(list_of_joints)
# calculate radius for the inner cylinder/sphere
cut_radius = arm_length
# print('cut_radius='+str(cut_radius))
#translation
translation_vector = np.array([-reference_join.get_WorldJoint()[0], -reference_join.get_WorldJoint()[1], -reference_join.get_WorldJoint()[2]])
# print(translation_vector)
for joint in joints_to_compute:
point = np.array(joint.get_WorldJoint())
transformed_point = point + translation_vector
joint.set_WorldJoint(transformed_point.item(0),transformed_point.item(1),transformed_point.item(2))
#
# X = right_hip to left_hip
#
# Y = bottom_center to spine
# (allmost perpendicular to alpha)
#
x_vector = np.array(reference_join_left.get_WorldJoint()) - np.array(reference_join_right.get_WorldJoint())
y_vector = np.array(reference_join_up.get_WorldJoint()) - np.array(reference_join.get_WorldJoint())
# calculate z
z_vector = np.cross(x_vector,y_vector)
# recalculate y so it is perpendicular to XZ plane
y_vector = np.cross(z_vector,x_vector)
for joint in joints_to_compute:
# Catch the zero division raised by NaNs in the skeleton files
with warnings.catch_warnings():
warnings.filterwarnings('error')
try:
r,flat_r,alpha,theta = transform_coordinate_shpere(x_vector,y_vector,z_vector,joint.get_WorldJoint())
except Warning:
return [],-1
considered_radius = flat_r
if _depth_measurement is not "cylinder":
considered_radius = r
inner, outer = r_function(considered_radius, cut_radius)
j = 0
for t in theta_bin:
if(t >= 180):
break
# find theta-bins to calculate
if((theta - t > 60) or (theta - t < -30)):
j+=1
continue
i = 0
for a in alpha_bin:
if(a == 360):
break
# find alpha-bins to calculate
if((alpha - a <= 60) and (alpha - a >= -30)):
probability = abs((probability_function(alpha,(alpha_bin[i+1]+alpha_bin[i])/2))) * abs((probability_function(theta,(theta_bin[j+1]+theta_bin[j])/2)))
# print(probability)
hoj3d = hoj_add_function(hoj3d, probability, inner, outer, i, j)
# wrap around the sphere
if((alpha+360 - a <= 60) or (alpha-360 - a >= -30)):
if(alpha < 30):
probability = abs((probability_function(alpha+360,(alpha_bin[i+1]+alpha_bin[i])/2))) * abs((probability_function(theta,(theta_bin[j+1]+theta_bin[j])/2)))
# print(probability)
hoj3d = hoj_add_function(hoj3d, probability, inner, outer, i, j)
elif(alpha > 330):
probability = abs((probability_function(alpha-360,(alpha_bin[i+1]+alpha_bin[i])/2))) * abs((probability_function(theta,(theta_bin[j+1]+theta_bin[j])/2)))
# print(probability)
hoj3d = hoj_add_function(hoj3d, probability, inner, outer, i, j)
i += 1
j += 1
# debug print
#np.set_printoptions(precision = 3,suppress = True)
#print(np.array(hoj3d))
#hoj = np.array(hoj3d)
#print(hoj.sum())
# add bodyparts
if _body_parts:
hoj3d = np.append(hoj3d.flatten(),[left_hand_sholder, right_hand_sholder, left_foot_hip, right_foot_hip])
# Flat the data. Always.
hoj3d = hoj3d.flatten()
t1 = time.time()
n_time += t1 - t0
return hoj3d,n_time