def get_bodyparts(project_dir):
"""
Returns an array of all the bodyparts labelled for a specific project
"""
print(f"\n\n\nLoading data")
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
arr = points_2d_df[points_2d_df["frame"] == 0][[
"marker"
]][points_2d_df["camera"] == 0].values
final_arr = arr.flatten().tolist()
return (final_arr)
def plot_skeleton(project_dir, part):
"""
Returns arrays of the x, y, and z coordinates for a given body part in a given project
"""
scene_fpath = os.path.join(project_dir, 'scene_sba.json')
print(scene_fpath)
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_fpath)
D_arr = D_arr.reshape((-1, 4))
print(f"\n\n\nLoading data")
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
#print(df_paths)
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
triangulate_func = calib.triangulate_points_fisheye
points_2d_filtered_df = points_2d_df[points_2d_df['likelihood'] > 0.5]
points_3d_df = calib.get_pairwise_3d_points_from_df(
points_2d_filtered_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
# estimate initial points
nose_pts = points_3d_df[points_3d_df["marker"] == part][[
"x", "y", "z", "frame"
]].values
x_slope, x_intercept, *_ = stats.linregress(nose_pts[:, 3], nose_pts[:, 0])
y_slope, y_intercept, *_ = stats.linregress(nose_pts[:, 3], nose_pts[:, 1])
z_slope, z_intercept, *_ = stats.linregress(nose_pts[:, 3], nose_pts[:, 2])
frame_est = np.arange(100)
x_est = frame_est * x_slope + x_intercept
y_est = frame_est * y_slope + y_intercept
z_est = frame_est * z_slope + z_intercept
psi_est = np.arctan2(y_slope, x_slope)
#print(points_2d_df)
#print(points_2d_df[points_2d_df['frame']==160])
#return([nose_pts[:,0], nose_pts[:,1], nose_pts[:,2]])
return (x_est, y_est, z_est)
def build_model(skel_dict, project_dir) -> ConcreteModel:
"""
Builds a pyomo model from a given saved skeleton dictionary
"""
links = skel_dict["links"]
positions = skel_dict["positions"]
dofs = skel_dict["dofs"]
markers = skel_dict["markers"]
rot_dict = {}
pose_dict = {}
L = len(positions)
phi = [sp.symbols(f"\\phi_{{{l}}}") for l in range(L)]
theta = [sp.symbols(f"\\theta_{{{l}}}") for l in range(L)]
psi = [sp.symbols(f"\\psi_{{{l}}}") for l in range(L)]
i = 0
for part in dofs:
rot_dict[part] = sp.eye(3)
if dofs[part][1]:
rot_dict[part] = rot_y(theta[i]) @ rot_dict[part]
if dofs[part][0]:
rot_dict[part] = rot_x(phi[i]) @ rot_dict[part]
if dofs[part][2]:
rot_dict[part] = rot_z(psi[i]) @ rot_dict[part]
rot_dict[part + "_i"] = rot_dict[part].T
i += 1
x, y, z = sp.symbols("x y z")
dx, dy, dz = sp.symbols("\\dot{x} \\dot{y} \\dot{z}")
ddx, ddy, ddz = sp.symbols("\\ddot{x} \\ddot{y} \\ddot{z}")
for link in links:
if len(link) == 1:
pose_dict[link[0]] = sp.Matrix([x, y, z])
else:
if link[0] not in pose_dict:
pose_dict[link[0]] = sp.Matrix([x, y, z])
translation_vec = sp.Matrix([
positions[link[1]][0] - positions[link[0]][0],
positions[link[1]][1] - positions[link[0]][1],
positions[link[1]][2] - positions[link[0]][2]
])
rot_dict[link[1]] = rot_dict[link[1]] @ rot_dict[link[0]]
rot_dict[link[1] + "_i"] = rot_dict[link[1] + "_i"].T
pose_dict[link[1]] = pose_dict[
link[0]] + rot_dict[link[0] + "_i"] @ translation_vec
t_poses = []
for pose in pose_dict:
t_poses.append(pose_dict[pose].T)
t_poses_mat = sp.Matrix(t_poses)
func_map = {"sin": sin, "cos": cos, "ImmutableDenseMatrix": np.array}
sym_list = [x, y, z, *phi, *theta, *psi]
pose_to_3d = sp.lambdify(sym_list, t_poses_mat, modules=[func_map])
pos_funcs = []
for i in range(t_poses_mat.shape[0]):
lamb = sp.lambdify(sym_list, t_poses_mat[i, :], modules=[func_map])
pos_funcs.append(lamb)
scene_path = os.path.join(project_dir, "4_cam_scene_static_sba.json")
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1, 4))
markers_dict = dict(enumerate(markers))
print(f"\n\n\nLoading data")
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
print("2d df points:")
print(points_2d_df)
# break here
def get_meas_from_df(n, c, l, d):
n_mask = points_2d_df["frame"] == n - 1
l_mask = points_2d_df["marker"] == markers[l - 1]
c_mask = points_2d_df["camera"] == c - 1
d_idx = {1: "x", 2: "y"}
val = points_2d_df[n_mask & l_mask & c_mask]
return val[d_idx[d]].values[0]
def get_likelihood_from_df(n, c, l):
n_mask = points_2d_df["frame"] == n - 1
if (markers[l - 1] == "neck"):
return 0
else:
l_mask = points_2d_df["marker"] == markers[l - 1]
c_mask = points_2d_df["camera"] == c - 1
val = points_2d_df[n_mask & l_mask & c_mask]
return val["likelihood"].values[0]
h = 1 / 120 #timestep
start_frame = 1600 # 50
N = 50
P = 3 + len(phi) + len(theta) + len(psi)
L = len(pos_funcs)
C = len(K_arr)
D2 = 2
D3 = 3
proj_funcs = [pt3d_to_x2d, pt3d_to_y2d]
R = 5 # measurement standard deviation
Q = np.array([ # model parameters variance
4.0, 7.0, 5.0, 13.0, 32.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, 9.0, 18.0, 43.0, 53.0, 90.0, 118.0, 247.0, 186.0, 194.0,
164.0, 295.0, 243.0, 334.0, 149.0, 26.0, 12.0, 0.0, 34.0, 43.0, 51.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
])**2
triangulate_func = calib.triangulate_points_fisheye
points_2d_filtered_df = points_2d_df[points_2d_df['likelihood'] > 0.5]
points_3d_df = calib.get_pairwise_3d_points_from_df(
points_2d_filtered_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
print("3d points")
print(points_3d_df)
# estimate initial points
nose_pts = points_3d_df[points_3d_df["marker"] == "forehead"][[
"x", "y", "z", "frame"
]].values
print(nose_pts[:, 3])
print(nose_pts[:, 0])
xs = stats.linregress(nose_pts[:, 3], nose_pts[:, 0])
ys = stats.linregress(nose_pts[:, 3], nose_pts[:, 1])
zs = stats.linregress(nose_pts[:, 3], nose_pts[:, 2])
frame_est = np.arange(N)
x_est = np.array([
frame_est[i] * (xs.slope) + (xs.intercept)
for i in range(len(frame_est))
])
y_est = np.array([
frame_est[i] * (ys.slope) + (ys.intercept)
for i in range(len(frame_est))
])
z_est = np.array([
frame_est[i] * (zs.slope) + (zs.intercept)
for i in range(len(frame_est))
])
print(x_est)
#print("x est shape:")
#print(x_est.shape)
psi_est = np.arctan2(ys.slope, xs.slope)
print("Started Optimisation")
m = ConcreteModel(name="Skeleton")
# ===== SETS =====
m.N = RangeSet(N) #number of timesteps in trajectory
m.P = RangeSet(
P
) #number of pose parameters (x, y, z, phi_1..n, theta_1..n, psi_1..n)
m.L = RangeSet(L) #number of labels
m.C = RangeSet(C) #number of cameras
m.D2 = RangeSet(D2) #dimensionality of measurements
m.D3 = RangeSet(D3) #dimensionality of measurements
def init_meas_weights(model, n, c, l):
likelihood = get_likelihood_from_df(n + start_frame, c, l)
if likelihood > 0.5:
return 1 / R
else:
return 0
m.meas_err_weight = Param(
m.N, m.C, m.L, initialize=init_meas_weights, mutable=True, within=Any
) # IndexError: index 0 is out of bounds for axis 0 with size 0
def init_model_weights(m, p):
#if Q[p-1] != 0.0:
#return 1/Q[p-1]
#else:
return 0.01
m.model_err_weight = Param(m.P, initialize=init_model_weights, within=Any)
m.h = h
def init_measurements_df(m, n, c, l, d2):
if (markers[l - 1] == "neck"):
return Constraint.Skip
else:
return get_meas_from_df(n + start_frame, c, l, d2)
m.meas = Param(m.N,
m.C,
m.L,
m.D2,
initialize=init_measurements_df,
within=Any)
# ===== VARIABLES =====
m.x = Var(m.N, m.P) #position
m.dx = Var(m.N, m.P) #velocity
m.ddx = Var(m.N, m.P) #acceleration
m.poses = Var(m.N, m.L, m.D3)
m.slack_model = Var(m.N, m.P)
m.slack_meas = Var(m.N, m.C, m.L, m.D2, initialize=0.0)
# ===== VARIABLES INITIALIZATION =====
init_x = np.zeros((N, P))
init_x[:, 0] = x_est #x
init_x[:, 1] = y_est #y
init_x[:, 2] = z_est #z
#init_x[:,(3+len(pos_funcs)*2)] = psi_est #yaw - psi
init_dx = np.zeros((N, P))
init_ddx = np.zeros((N, P))
for n in range(1, N + 1):
for p in range(1, P + 1):
if n < len(init_x): #init using known values
m.x[n, p].value = init_x[n - 1, p - 1]
m.dx[n, p].value = init_dx[n - 1, p - 1]
m.ddx[n, p].value = init_ddx[n - 1, p - 1]
else: #init using last known value
m.x[n, p].value = init_x[-1, p - 1]
m.dx[n, p].value = init_dx[-1, p - 1]
m.ddx[n, p].value = init_ddx[-1, p - 1]
#init pose
var_list = [m.x[n, p].value for p in range(1, P + 1)]
for l in range(1, L + 1):
[pos] = pos_funcs[l - 1](*var_list)
for d3 in range(1, D3 + 1):
m.poses[n, l, d3].value = pos[d3 - 1]
# ===== CONSTRAINTS =====
# 3D POSE
def pose_constraint(m, n, l, d3):
#get 3d points
var_list = [m.x[n, p] for p in range(1, P + 1)]
[pos] = pos_funcs[l - 1](*var_list)
return pos[d3 - 1] == m.poses[n, l, d3]
m.pose_constraint = Constraint(m.N, m.L, m.D3, rule=pose_constraint)
def backwards_euler_pos(m, n, p): # position
if n > 1:
# return m.x[n,p] == m.x[n-1,p] + m.h*m.dx[n-1,p] + m.h**2 * m.ddx[n-1,p]/2
return m.x[n, p] == m.x[n - 1, p] + m.h * m.dx[n, p]
else:
return Constraint.Skip
m.integrate_p = Constraint(m.N, m.P, rule=backwards_euler_pos)
def backwards_euler_vel(m, n, p): # velocity
if n > 1:
return m.dx[n, p] == m.dx[n - 1, p] + m.h * m.ddx[n, p]
else:
return Constraint.Skip
m.integrate_v = Constraint(m.N, m.P, rule=backwards_euler_vel)
m.angs = ConstraintList()
for n in range(1, N):
for i in range(3, 3 * len(positions)):
m.angs.add(expr=(abs(m.x[n, i]) <= np.pi / 2))
# MODEL
def constant_acc(m, n, p):
if n > 1:
return m.ddx[n, p] == m.ddx[n - 1, p] + m.slack_model[n, p]
else:
return Constraint.Skip
m.constant_acc = Constraint(m.N, m.P, rule=constant_acc)
# MEASUREMENT
def measurement_constraints(m, n, c, l, d2):
#project
K, D, R, t = K_arr[c - 1], D_arr[c - 1], R_arr[c - 1], t_arr[c - 1]
x, y, z = m.poses[n, l, 1], m.poses[n, l, 2], m.poses[n, l, 3]
if (markers[l - 1] == "neck"):
return Constraint.Skip
else:
return proj_funcs[d2 - 1](
x, y, z, K, D, R,
t) - m.meas[n, c, l, d2] - m.slack_meas[n, c, l, d2] == 0
m.measurement = Constraint(m.N,
m.C,
m.L,
m.D2,
rule=measurement_constraints)
def obj(m):
slack_model_err = 0.0
slack_meas_err = 0.0
for n in range(1, N + 1):
#Model Error
for p in range(1, P + 1):
slack_model_err += m.model_err_weight[p] * m.slack_model[n,
p]**2
#Measurement Error
for l in range(1, L + 1):
for c in range(1, C + 1):
for d2 in range(1, D2 + 1):
slack_meas_err += redescending_loss(
m.meas_err_weight[n, c, l] *
m.slack_meas[n, c, l, d2], 3, 10, 20)
return slack_meas_err + slack_model_err
m.obj = Objective(rule=obj)
return (m, pose_to_3d)
def build_model(project_dir, dlc_thresh=0.5) -> Tuple[ConcreteModel, Dict]:
#SYMBOLIC ROTATION MATRIX FUNCTIONS
print("Generate and load data...")
L = 14 #number of joints in the cheetah model
# defines arrays ofa angles, velocities and accelerations
phi = [sp.symbols(f"\\phi_{{{l}}}") for l in range(L)]
theta = [sp.symbols(f"\\theta_{{{l}}}") for l in range(L)]
psi = [sp.symbols(f"\\psi_{{{l}}}") for l in range(L)]
#ROTATIONS
# head
RI_0 = rot_z(psi[0]) @ rot_x(phi[0]) @ rot_y(theta[0])
R0_I = RI_0.T
# neck
RI_1 = rot_z(psi[1]) @ rot_x(phi[1]) @ rot_y(theta[1]) @ RI_0
R1_I = RI_1.T
# front torso
RI_2 = rot_y(theta[2]) @ RI_1
R2_I = RI_2.T
# back torso
RI_3 = rot_z(psi[3])@ rot_x(phi[3]) @ rot_y(theta[3]) @ RI_2
R3_I = RI_3.T
# tail base
RI_4 = rot_z(psi[4]) @ rot_y(theta[4]) @ RI_3
R4_I = RI_4.T
# tail mid
RI_5 = rot_z(psi[5]) @ rot_y(theta[5]) @ RI_4
R5_I = RI_5.T
#l_shoulder
RI_6 = rot_y(theta[6]) @ RI_2
R6_I = RI_6.T
#l_front_knee
RI_7 = rot_y(theta[7]) @ RI_6
R7_I = RI_7.T
#r_shoulder
RI_8 = rot_y(theta[8]) @ RI_2
R8_I = RI_8.T
#r_front_knee
RI_9 = rot_y(theta[9]) @ RI_8
R9_I = RI_9.T
#l_hip
RI_10 = rot_y(theta[10]) @ RI_3
R10_I = RI_10.T
#l_back_knee
RI_11 = rot_y(theta[11]) @ RI_10
R11_I = RI_11.T
#r_hip
RI_12 = rot_y(theta[12]) @ RI_3
R12_I = RI_12.T
#r_back_knee
RI_13 = rot_y(theta[13]) @ RI_12
R13_I = RI_13.T
# defines the position, velocities and accelerations in the inertial frame
x, y, z = sp.symbols("x y z")
dx, dy, dz = sp.symbols("\\dot{x} \\dot{y} \\dot{z}")
ddx, ddy, ddz = sp.symbols("\\ddot{x} \\ddot{y} \\ddot{z}")
# SYMBOLIC CHEETAH POSE POSITIONS
p_head = sp.Matrix([x, y, z])
p_l_eye = p_head + R0_I @ sp.Matrix([0, 0.03, 0])
p_r_eye = p_head + R0_I @ sp.Matrix([0, -0.03, 0])
p_nose = p_head + R0_I @ sp.Matrix([0.055, 0, -0.055])
p_neck_base = p_head + R1_I @ sp.Matrix([-0.28, 0, 0])
p_spine = p_neck_base + R2_I @ sp.Matrix([-0.37, 0, 0])
p_tail_base = p_spine + R3_I @ sp.Matrix([-0.37, 0, 0])
p_tail_mid = p_tail_base + R4_I @ sp.Matrix([-0.28, 0, 0])
p_tail_tip = p_tail_mid + R5_I @ sp.Matrix([-0.36, 0, 0])
p_l_shoulder = p_neck_base + R2_I @ sp.Matrix([-0.04, 0.08, -0.10])
p_l_front_knee = p_l_shoulder + R6_I @ sp.Matrix([0, 0, -0.24])
p_l_front_ankle = p_l_front_knee + R7_I @ sp.Matrix([0, 0, -0.28])
p_r_shoulder = p_neck_base + R2_I @ sp.Matrix([-0.04, -0.08, -0.10])
p_r_front_knee = p_r_shoulder + R8_I @ sp.Matrix([0, 0, -0.24])
p_r_front_ankle = p_r_front_knee + R9_I @ sp.Matrix([0, 0, -0.28])
p_l_hip = p_tail_base + R3_I @ sp.Matrix([0.12, 0.08, -0.06])
p_l_back_knee = p_l_hip + R10_I @ sp.Matrix([0, 0, -0.32])
p_l_back_ankle = p_l_back_knee + R11_I @ sp.Matrix([0, 0, -0.25])
p_r_hip = p_tail_base + R3_I @ sp.Matrix([0.12, -0.08, -0.06])
p_r_back_knee = p_r_hip + R12_I @ sp.Matrix([0, 0, -0.32])
p_r_back_ankle = p_r_back_knee + R13_I @ sp.Matrix([0, 0, -0.25])
positions = sp.Matrix([
p_l_eye.T, p_r_eye.T, p_nose.T,
p_neck_base.T, p_spine.T,
p_tail_base.T, p_tail_mid.T, p_tail_tip.T,
p_l_shoulder.T, p_l_front_knee.T, p_l_front_ankle.T,
p_r_shoulder.T, p_r_front_knee.T, p_r_front_ankle.T,
p_l_hip.T, p_l_back_knee.T, p_l_back_ankle.T,
p_r_hip.T, p_r_back_knee.T, p_r_back_ankle.T
])
# ========= LAMBDIFY SYMBOLIC FUNCTIONS ========
func_map = {"sin":sin, "cos":cos, "ImmutableDenseMatrix":np.array}
sym_list = [x, y, z, *phi, *theta, *psi]
pose_to_3d = sp.lambdify(sym_list, positions, modules=[func_map])
pos_funcs = []
for i in range(positions.shape[0]):
lamb = sp.lambdify(sym_list, positions[i,:], modules=[func_map])
pos_funcs.append(lamb)
scene_path = os.path.join(project_dir, "scene_sba.json")
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1,4))
# markers = misc.get_markers()
markers = dict(enumerate([
"l_eye", "r_eye", "nose",
"neck_base", "spine",
"tail_base", "tail1", "tail2",
"l_shoulder", "l_front_knee", "l_front_ankle",
"r_shoulder", "r_front_knee", "r_front_ankle",
"l_hip", "l_back_knee", "l_back_ankle",
"r_hip", "r_back_knee", "r_back_ankle"
]))
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
def get_meas_from_df(n, c, l, d):
n_mask = points_2d_df["frame"]== n-1
l_mask = points_2d_df["marker"]== markers[l-1]
c_mask = points_2d_df["camera"]== c-1
d_idx = {1:"x", 2:"y"}
val = points_2d_df[n_mask & l_mask & c_mask]
return val[d_idx[d]].values[0]
def get_likelihood_from_df(n, c, l):
n_mask = points_2d_df["frame"]== n-1
l_mask = points_2d_df["marker"]== markers[l-1]
c_mask = points_2d_df["camera"]== c-1
val = points_2d_df[n_mask & l_mask & c_mask]
return val["likelihood"].values[0]
# Parameters
h = 1/120 #timestep
end_frame = 165
start_frame = 50
N = end_frame-start_frame # N > start_frame !!
P = 3 + len(phi)+len(theta)+len(psi)
L = len(pos_funcs)
C = len(K_arr)
D2 = 2
D3 = 3
W = 2
proj_funcs = [pt3d_to_x2d, pt3d_to_y2d]
# measurement standard deviation
R = np.array([
1.24,
1.18,
1.2,
2.08,
2.04,
2.52,
2.73,
1.83,
3.4,
2.91,
2.85,
# 2.27, # l_front_paw
3.47,
2.75,
2.69,
# 2.24, # r_front_paw
3.53,
2.69,
2.49,
# 2.34, # l_back_paw
3.26,
2.76,
2.33,
# 2.4, # r_back_paw
])
R_pw = np.repeat(7, len(R))
Q = np.array([ # model parameters variance
4.0,
7.0,
5.0,
13.0,
32.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,
9.0,
18.0,
43.0,
53.0,
90.0,
118.0,
247.0,
186.0,
194.0,
164.0,
295.0,
243.0,
334.0,
149.0,
26.0,
12.0,
0.0,
34.0,
43.0,
51.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
])**2
triangulate_func = calib.triangulate_points_fisheye
points_2d_filtered_df = points_2d_df[points_2d_df['likelihood']>dlc_thresh]
points_3d_df = calib.get_pairwise_3d_points_from_df(points_2d_filtered_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
# estimate initial points
nose_pts = points_3d_df[points_3d_df["marker"]=="nose"][["x", "y", "z", "frame"]].values
x_slope, x_intercept, *_ = stats.linregress(nose_pts[:,3], nose_pts[:,0])
y_slope, y_intercept, *_ = stats.linregress(nose_pts[:,3], nose_pts[:,1])
z_slope, z_intercept, *_ = stats.linregress(nose_pts[:,3], nose_pts[:,2])
frame_est = np.arange(N)
x_est = frame_est*x_slope + x_intercept
y_est = frame_est*y_slope + y_intercept
z_est = frame_est*z_slope + z_intercept
print(x_est.shape)
psi_est = np.arctan2(y_slope, x_slope)
print("Build optimisation problem - Start")
m = ConcreteModel(name = "Skeleton")
# ===== SETS =====
m.N = RangeSet(N) #number of timesteps in trajectory
m.P = RangeSet(P) #number of pose parameters (x, y, z, phi_1..n, theta_1..n, psi_1..n)
m.L = RangeSet(L) #number of labels
m.C = RangeSet(C) #number of cameras
m.D2 = RangeSet(D2) #dimensionality of measurements
m.D3 = RangeSet(D3) #dimensionality of measurements
m.W = RangeSet(W) # Number of pairwise terms to include.
def init_meas_weights(model, n, c, l):
likelihood = get_likelihood_from_df(n+start_frame, c, l)
if likelihood > dlc_thresh:
return 1/R[l-1]
else:
return 0
m.meas_err_weight = Param(m.N, m.C, m.L, initialize=init_meas_weights, mutable=True) # IndexError: index 0 is out of bounds for axis 0 with size 0
def init_pw_meas_weights(model, n, c, l):
likelihood = get_likelihood_from_df(n+start_frame, c, l)
if likelihood <= dlc_thresh:
return 1/R_pw[l-1]
else:
return 0
m.meas_pw_err_weight = Param(m.N, m.C, m.L, initialize=init_pw_meas_weights, mutable=True) # IndexError: index 0 is out of bounds for axis 0 with size 0
def init_model_weights(m, p):
if Q[p-1] != 0.0:
return 1/Q[p-1]
else:
return 0
m.model_err_weight = Param(m.P, initialize=init_model_weights)
m.h = h
def init_measurements_df(m, n, c, l, d2):
return get_meas_from_df(n+start_frame, c, l, d2)
m.meas = Param(m.N, m.C, m.L, m.D2, initialize=init_measurements_df)
# resultsfilename='C://Users//user-pc//Desktop//pwpoints.pickle'
# with open(resultsfilename, 'rb') as f:
# data=pickle.load(f)
pw_data = {}
for cam in range(C):
pw_data[cam] = load_data(f"/Users/zico/msc/dev/FYP/data/09_03_2019/lily/run/cam{cam+1}-predictions.pickle")
index_dict = {"nose":23, "r_eye":0, "l_eye":1, "neck_base":24, "spine":6, "tail_base":22, "tail1":11,
"tail2":12, "l_shoulder":13,"l_front_knee":14,"l_front_ankle":15,"r_shoulder":2,
"r_front_knee":3, "r_front_ankle":4,"l_hip":17,"l_back_knee":18, "l_back_ankle":19,
"r_hip":7,"r_back_knee":8,"r_back_ankle":9}
pair_dict = {"r_eye":[23, 24], "l_eye":[23, 24], "nose":[6, 24], "neck_base":[6, 23], "spine":[22, 24], "tail_base":[6, 11], "tail1":[6, 22],
"tail2":[11, 22], "l_shoulder":[6, 24],"l_front_knee":[6, 24],"l_front_ankle":[6, 24],"r_shoulder":[6, 24],
"r_front_knee":[6, 24], "r_front_ankle":[6, 24],"l_hip":[6, 22],"l_back_knee":[6, 22], "l_back_ankle":[6, 22],
"r_hip":[6, 22],"r_back_knee":[6, 22],"r_back_ankle":[6, 22]}
def init_pw_measurements(m, n, c, l, d2, w):
pw_values = pw_data[c-1][n+start_frame]
marker = markers[l-1]
base = pair_dict[marker][w-1]
val = pw_values['pose'][d2-1::3]
val_pw = pw_values['pws'][:,:,:,d2-1]
return val[base]+val_pw[0,base,index_dict[marker]]
m.pw_meas = Param(m.N, m.C, m.L, m.D2, m.W, initialize=init_pw_measurements, within=Any)
"""
def init_pw_measurements2(m, n, c, l, d2):
val=0
if n-1 >= 20 and n-1 < 30:
fn = 10*(c-1)+(n-20)-1
x=data[fn]['pose'][0::3]
y=data[fn]['pose'][1::3]
xpw=data[fn]['pws'][:,:,:,0]
ypw=data[fn]['pws'][:,:,:,1]
marker = markers[l-1]
if "ankle" in marker:
base = pair_dict[marker][1]
if d2==1:
val=x[base]+xpw[0,base,index_dict[marker]]
elif d2==2:
val=y[base]+ypw[0,base,index_dict[marker]]
#sum/=len(pair_dict[marker])
return val
else:
return(0.0)
m.pw_meas2 = Param(m.N, m.C, m.L, m.D2, initialize=init_pw_measurements2, within=Any)
"""
# ===== VARIABLES =====
m.x = Var(m.N, m.P) #position
m.dx = Var(m.N, m.P) #velocity
m.ddx = Var(m.N, m.P) #acceleration
m.poses = Var(m.N, m.L, m.D3)
m.slack_model = Var(m.N, m.P)
m.slack_meas = Var(m.N, m.C, m.L, m.D2, initialize=0.0)
m.slack_pw_meas = Var(m.N, m.C, m.L, m.D2, m.W, initialize=0.0)
# ===== VARIABLES INITIALIZATION =====
init_x = np.zeros((N-start_frame, P))
init_x[:,0] = x_est[start_frame: start_frame+N] #x
init_x[:,1] = y_est[start_frame: start_frame+N] #y
init_x[:,2] = z_est[start_frame: start_frame+N] #z
init_x[:,31] = psi_est #yaw - psi
init_dx = np.zeros((N, P))
init_ddx = np.zeros((N, P))
for n in m.N:
for p in m.P:
if n<len(init_x): #init using known values
m.x[n,p].value = init_x[n-1,p-1]
m.dx[n,p].value = init_dx[n-1,p-1]
m.ddx[n,p].value = init_ddx[n-1,p-1]
else: #init using last known value
m.x[n,p].value = init_x[-1,p-1]
m.dx[n,p].value = init_dx[-1,p-1]
m.ddx[n,p].value = init_ddx[-1,p-1]
#init pose
var_list = [m.x[n,p].value for p in range(1, P+1)]
for l in m.L:
[pos] = pos_funcs[l-1](*var_list)
for d3 in m.D3:
m.poses[n,l,d3].value = pos[d3-1]
# ===== CONSTRAINTS =====
# 3D POSE
def pose_constraint(m,n,l,d3):
#get 3d points
var_list = [m.x[n,p] for p in range(1, P+1)]
[pos] = pos_funcs[l-1](*var_list)
return pos[d3-1] == m.poses[n,l,d3]
m.pose_constraint = Constraint(m.N, m.L, m.D3, rule=pose_constraint)
def backwards_euler_pos(m,n,p): # position
if n > 1:
# return m.x[n,p] == m.x[n-1,p] + m.h*m.dx[n-1,p] + m.h**2 * m.ddx[n-1,p]/2
return m.x[n,p] == m.x[n-1,p] + m.h*m.dx[n,p]
else:
return Constraint.Skip
m.integrate_p = Constraint(m.N, m.P, rule = backwards_euler_pos)
def backwards_euler_vel(m,n,p): # velocity
if n > 1:
return m.dx[n,p] == m.dx[n-1,p] + m.h*m.ddx[n,p]
else:
return Constraint.Skip
m.integrate_v = Constraint(m.N, m.P, rule = backwards_euler_vel)
# MODEL
def constant_acc(m, n, p):
if n > 1:
return m.ddx[n,p] == m.ddx[n-1,p] + m.slack_model[n,p]
else:
return Constraint.Skip
m.constant_acc = Constraint(m.N, m.P, rule = constant_acc)
# MEASUREMENT
def measurement_constraints(m, n, c, l, d2):
#project
K, D, R, t = K_arr[c-1], D_arr[c-1], R_arr[c-1], t_arr[c-1]
x, y, z = m.poses[n,l,1], m.poses[n,l,2], m.poses[n,l,3]
return proj_funcs[d2-1](x, y, z, K, D, R, t) - m.meas[n, c, l, d2] - m.slack_meas[n, c, l, d2] ==0
m.measurement = Constraint(m.N, m.C, m.L, m.D2, rule = measurement_constraints)
def pw_measurement_constraints(m, n, c, l, d2, w):
#project
K, D, R, t = K_arr[c-1], D_arr[c-1], R_arr[c-1], t_arr[c-1]
x, y, z = m.poses[n,l,1], m.poses[n,l,2], m.poses[n,l,3]
return proj_funcs[d2-1](x, y, z, K, D, R, t) - m.pw_meas[n, c, l, d2, w] - m.slack_pw_meas[n, c, l, d2, w] ==0.0
m.pw_measurement = Constraint(m.N, m.C, m.L, m.D2, m.W, rule = pw_measurement_constraints)
"""
def pw_measurement_constraints2(m, n, c, l, d2):
#project
if n-1 >= 20 and n-1 < 30 and "ankle" in markers[l-1]:
K, D, R, t = K_arr[c-1], D_arr[c-1], R_arr[c-1], t_arr[c-1]
x, y, z = m.poses[n,l,1], m.poses[n,l,2], m.poses[n,l,3]
return proj_funcs[d2-1](x, y, z, K, D, R, t) - m.pw_meas2[n, c, l, d2] - m.slack_meas[n, c, l, d2] ==0.0
else:
return(Constraint.Skip)
m.pw_measurement2 = Constraint(m.N, m.C, m.L, m.D2, rule = pw_measurement_constraints2)
"""
#===== POSE CONSTRAINTS (Note 1 based indexing for pyomo!!!!...@#^!@#&) =====
#Head
def head_psi_0(m,n):
return abs(m.x[n,4]) <= np.pi/6
m.head_psi_0 = Constraint(m.N, rule=head_psi_0)
def head_theta_0(m,n):
return abs(m.x[n,18]) <= np.pi/6
m.head_theta_0 = Constraint(m.N, rule=head_theta_0)
#Neck
def neck_phi_1(m,n):
return abs(m.x[n,5]) <= np.pi/6
m.neck_phi_1 = Constraint(m.N, rule=neck_phi_1)
def neck_theta_1(m,n):
return abs(m.x[n,19]) <= np.pi/6
m.neck_theta_1 = Constraint(m.N, rule=neck_theta_1)
def neck_psi_1(m,n):
return abs(m.x[n,33]) <= np.pi/6
m.neck_psi_1 = Constraint(m.N, rule=neck_psi_1)
#Front torso
def front_torso_theta_2(m,n):
return abs(m.x[n,20]) <= np.pi/6
m.front_torso_theta_2 = Constraint(m.N, rule=front_torso_theta_2)
#Back torso
def back_torso_theta_3(m,n):
return abs(m.x[n,21]) <= np.pi/6
m.back_torso_theta_3 = Constraint(m.N, rule=back_torso_theta_3)
# --- Back torso phi constraint - uncomment if needed! ---
def back_torso_phi_3(m,n):
return abs(m.x[n,7]) <= np.pi/6
m.back_torso_phi_3 = Constraint(m.N, rule=back_torso_phi_3)
def back_torso_psi_3(m,n):
return abs(m.x[n,35]) <= np.pi/6
m.back_torso_psi_3 = Constraint(m.N, rule=back_torso_psi_3)
#Tail base
def tail_base_theta_4(m,n):
return abs(m.x[n,22]) <= np.pi/1.5
m.tail_base_theta_4 = Constraint(m.N, rule=tail_base_theta_4)
def tail_base_psi_4(m,n):
return abs(m.x[n,36]) <= np.pi/1.5
m.tail_base_psi_4 = Constraint(m.N, rule=tail_base_psi_4)
#Tail base
def tail_mid_theta_5(m,n):
return abs(m.x[n,23]) <= np.pi/1.5
m.tail_mid_theta_5 = Constraint(m.N, rule=tail_mid_theta_5)
def tail_mid_psi_5(m,n):
return abs(m.x[n,37]) <= np.pi/1.5
m.tail_mid_psi_5 = Constraint(m.N, rule=tail_mid_psi_5)
#Front left leg
def l_shoulder_theta_6(m,n):
return abs(m.x[n,24]) <= np.pi/2
m.l_shoulder_theta_6 = Constraint(m.N, rule=l_shoulder_theta_6)
def l_front_knee_theta_7(m,n):
return abs(m.x[n,25] + np.pi/2) <= np.pi/2
m.l_front_knee_theta_7 = Constraint(m.N, rule=l_front_knee_theta_7)
#Front right leg
def r_shoulder_theta_8(m,n):
return abs(m.x[n,26]) <= np.pi/2
m.r_shoulder_theta_8 = Constraint(m.N, rule=r_shoulder_theta_8)
def r_front_knee_theta_9(m,n):
return abs(m.x[n,27] + np.pi/2) <= np.pi/2
m.r_front_knee_theta_9 = Constraint(m.N, rule=r_front_knee_theta_9)
#Back left leg
def l_hip_theta_10(m,n):
return abs(m.x[n,28]) <= np.pi/2
m.l_hip_theta_10 = Constraint(m.N, rule=l_hip_theta_10)
def l_back_knee_theta_11(m,n):
return abs(m.x[n,29] - np.pi/2) <= np.pi/2
m.l_back_knee_theta_11 = Constraint(m.N, rule=l_back_knee_theta_11)
#Back right leg
def r_hip_theta_12(m,n):
return abs(m.x[n,30]) <= np.pi/2
m.r_hip_theta_12 = Constraint(m.N, rule=r_hip_theta_12)
def r_back_knee_theta_13(m,n):
return abs(m.x[n,31] - np.pi/2) <= np.pi/2
m.r_back_knee_theta_13 = Constraint(m.N, rule=r_back_knee_theta_13)
# ======= OBJECTIVE FUNCTION =======
def obj(m):
slack_model_err = 0.0
slack_meas_err = 0.0
slack_pw_meas_err = 0.0
for n in m.N:
#Model Error
for p in m.P:
slack_model_err += m.model_err_weight[p] * m.slack_model[n, p] ** 2
#Measurement Error
for l in m.L:
for c in m.C:
for d2 in m.D2:
slack_meas_err += misc.redescending_loss(m.meas_err_weight[n, c, l] * m.slack_meas[n, c, l, d2], 3, 7, 20)
for w in m.W:
slack_meas_err += misc.redescending_loss(m.meas_pw_err_weight[n, c, l] * m.slack_pw_meas[n, c, l, d2, w], 3, 7, 20)
return slack_meas_err + slack_model_err
m.obj = Objective(rule = obj)
print("Build optimisation problem - End")
return m, pose_to_3d
def compare_plots() -> None:
fig = plt.figure()
#ax =fig.add_subplot(111, projection='3d')
pose_dict = {}
currdir = "C://Users/user-pc/Documents/Scripts/FYP"
skel_name = "cheetah_serious"
skelly_dir = os.path.join(currdir, "skeletons", (skel_name + ".pickle"))
results_dir = os.path.join(currdir, "data", "results", (skel_name + ".pickle"))
skel_dict = bd.load_skeleton(skelly_dir)
results = an.load_pickle(results_dir)
links = skel_dict["links"]
markers = skel_dict["markers"]
traj_errs=[]
# --- Ground Truth Data ---
#ax.view_init(elev=20,azim=110)
scene_path = "C://Users//user-pc//Documents//Scripts//FYP_tests//GT//scene_sba.json"
project_dir = "C://Users//user-pc//Documents//Scripts//FYP_tests//GT"
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1,4))
gt_dict = {}
triangulate_func = calib.triangulate_points_fisheye
#points_2d_filtered_df = points_2d_df[points_2d_df['likelihood']>0.5]
#print(points_2d_df)
points_3d_df = calib.get_pairwise_3d_points_from_df(points_2d_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
for fn in range(105,110):
pts = points_3d_df[points_3d_df["frame"]==str(fn)][["x", "y", "z", "marker"]].values
for pt in pts:
gt_dict[pt[3]] = [pt[0], pt[1], pt[2]]
#for pt in pts:
#if pt[3] in markers:
#ax.scatter(pt[0], pt[1], pt[2], c="b")
#ax.xaxis.pane.fill = False
#ax.yaxis.pane.fill = False
#ax.zaxis.pane.fill = False
#for link in links:
#if len(link)>1:
#ax.plot3D([gt_dict[link[0]][0], gt_dict[link[1]][0]],
#[gt_dict[link[0]][1], gt_dict[link[1]][1]],
#[gt_dict[link[0]][2], gt_dict[link[1]][2]], c="b")
# --- Triangulation ---
#ax = fig.add_subplot(323, projection='3d')
#ax.title.set_text('Sparse Bundle Adjustment - Side')
#ax.view_init(elev=20,azim=10)
scene_path = "C://Users//user-pc//Documents//Scripts//FYP_tests//DLC//scene_sba.json"
project_dir = "C://Users//user-pc//Documents//Scripts//FYP_tests//DLC"
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1,4))
triangulate_func = calib.triangulate_points_fisheye
points_3d_df = calib.get_pairwise_3d_points_from_df(points_2d_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
sba_errs = []
sba_dict = {}
#for fn in range(100,110):
pts = points_3d_df[points_3d_df["frame"]==fn][["x", "y", "z", "marker"]].values
#print(pts)
#for pt in pts:
#if pt[3] in markers:
#ax.scatter(pt[0], pt[1], pt[2], c="r")
for pt in pts:
sba_dict[pt[3]] = [pt[0], pt[1], pt[2]]
#for mark in markers:
# sba_errs.append((sba_dict[mark][0] - gt_dict[mark][0] + sba_dict[mark][1] - gt_dict[mark][1] + sba_dict[mark][2] - gt_dict[mark][2])/3)
#for link in links:
#if len(link)>1:
#ax.plot3D([sba_dict[link[0]][0], sba_dict[link[1]][0]],
#[sba_dict[link[0]][1], sba_dict[link[1]][1]],
# [sba_dict[link[0]][2], sba_dict[link[1]][2]], c="r")
#plt.xlim(-2,5)
#plt.ylim(6,9)
#ax.xaxis.pane.fill = False
#ax.yaxis.pane.fill = False
#ax.zaxis.pane.fill = False
# --- Full Traj Opt ---
#ax.view_init(elev=45,azim=130)
#for frame in range(20,30):
frame=fn-80
for i in range(len(markers)):
pose_dict[markers[i]] = [results["positions"][frame][i][0], results["positions"][frame][i][1], results["positions"][frame][i][2]]
#ax.scatter(results["positions"][frame][i][0], results["positions"][frame][i][1], results["positions"][frame][i][2], c="g")
#for link in links:
#if len(link)>1:
#ax.plot3D([pose_dict[link[0]][0], pose_dict[link[1]][0]],
#[pose_dict[link[0]][1], pose_dict[link[1]][1]],
#[pose_dict[link[0]][2], pose_dict[link[1]][2]], c="g")
#print(pose_dict)
#print(gt_dict)
for mark in markers:
if not np.isnan(gt_dict[mark][0]):
traj_errs.append((pose_dict[mark][0] - gt_dict[mark][0] + pose_dict[mark][1] - gt_dict[mark][1] + pose_dict[mark][2] - gt_dict[mark][2])/3)
#ax.xaxis.pane.fill = False
#ax.yaxis.pane.fill = False
#ax.zaxis.pane.fill = False
#plt.show()
print(traj_errs)
std = np.std(traj_errs)
rmse = np.sqrt(np.mean(np.square(traj_errs)))
print(std)
print(rmse)
#ax.view_init(elev=80,azim=60)
fig.add_subplot(111)
plt.hist(traj_errs)
plt.title("Histogram of 3D errors from traj. opt. results with pairwise predictions")
plt.xlabel("Error in metres")
plt.ylabel("Number of errors")
plt.xlim(-2.5,2.5)
plt.grid()
plt.show()
def compare_plots_cloud() -> None:
fig = plt.figure()
pose_dict = {}
currdir = "C://Users/user-pc/Documents/Scripts/FYP"
skel_name = "cheetah_serious"
skelly_dir = os.path.join(currdir, "skeletons", (skel_name + ".pickle"))
results_dir = os.path.join(currdir, "data", "results", (skel_name + ".pickle"))
skel_dict = bd.load_skeleton(skelly_dir)
results = an.load_pickle(results_dir)
links = skel_dict["links"]
markers = skel_dict["markers"]
# --- Ground Truth Data ---
ax = fig.add_subplot(321, projection='3d')
ax.title.set_text('Ground Truth - Side')
#ax.view_init(elev=20,azim=110)
scene_path = "C://Users//user-pc//Documents//Scripts//FYP_tests//GT//scene_sba.json"
project_dir = "C://Users//user-pc//Documents//Scripts//FYP_tests//GT"
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1,4))
triangulate_func = calib.triangulate_points_fisheye
#points_2d_filtered_df = points_2d_df[points_2d_df['likelihood']>0.5]
#print(points_2d_df)
points_3d_df = calib.get_pairwise_3d_points_from_df(points_2d_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
for fn in range(100,110):
pts = points_3d_df[points_3d_df["frame"]==str(fn)][["x", "y", "z", "marker"]].values
#print(pts)
gt_dict = {}
for pt in pts:
gt_dict[pt[3]] = [pt[0], pt[1], pt[2]]
for pt in pts:
if pt[3] in markers:
ax.scatter(pt[0], pt[1], pt[2], c="b")
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax = fig.add_subplot(322, projection='3d')
ax.title.set_text('Ground Truth - Top')
ax.view_init(elev = 80,azim=60)
for fn in range(100,110):
pts = points_3d_df[points_3d_df["frame"]==str(fn)][["x", "y", "z", "marker"]].values
#print(pts)
gt_dict = {}
for pt in pts:
gt_dict[pt[3]] = [pt[0], pt[1], pt[2]]
for pt in pts:
if pt[3] in markers:
ax.scatter(pt[0], pt[1], pt[2], c="b")
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
#ax.set_zlabel('Z Axis')
#for link in links:
#if len(link)>1:
#ax.plot3D([gt_dict[link[0]][0], gt_dict[link[1]][0]],
#[gt_dict[link[0]][1], gt_dict[link[1]][1]],
#[gt_dict[link[0]][2], gt_dict[link[1]][2]], c="b")
# --- Triangulation ---
ax = fig.add_subplot(323, projection='3d')
ax.title.set_text('Sparse Bundle Adjustment - Side')
#ax.view_init(elev=20,azim=10)
scene_path = "C://Users//user-pc//Documents//Scripts//FYP_tests//DLC//scene_sba.json"
project_dir = "C://Users//user-pc//Documents//Scripts//FYP_tests//DLC"
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1,4))
triangulate_func = calib.triangulate_points_fisheye
points_3d_df = calib.get_pairwise_3d_points_from_df(points_2d_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
for fn in range(100,110):
pts = points_3d_df[points_3d_df["frame"]==fn][["x", "y", "z", "marker"]].values
#print(pts)
for pt in pts:
if pt[3] in markers:
ax.scatter(pt[0], pt[1], pt[2], c="r")
gt_dict = {}
for pt in pts:
gt_dict[pt[3]] = [pt[0], pt[1], pt[2]]
plt.xlim(-2,5)
plt.ylim(6,9)
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax = fig.add_subplot(324, projection='3d')
ax.title.set_text('Sparse Bundle Adjustment - Top')
ax.view_init(elev=80,azim=60)
for fn in range(100,110):
pts = points_3d_df[points_3d_df["frame"]==fn][["x", "y", "z", "marker"]].values
#print(pts)
for pt in pts:
if pt[3] in markers:
ax.scatter(pt[0], pt[1], pt[2], c="r")
gt_dict = {}
for pt in pts:
gt_dict[pt[3]] = [pt[0], pt[1], pt[2]]
plt.xlim(-2,5)
plt.ylim(6,9)
"""
for link in links:
if len(link)>1:
ax.plot3D([gt_dict[link[0]][0], gt_dict[link[1]][0]],
[gt_dict[link[0]][1], gt_dict[link[1]][1]],
[gt_dict[link[0]][2], gt_dict[link[1]][2]], c="r")
"""
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
#ax.set_zlabel('Z Axis')
# --- Full Traj Opt ---
ax = fig.add_subplot(325, projection='3d')
ax.title.set_text('Trajectory Optimisation - Side')
#ax.view_init(elev=45,azim=130)
for frame in range(20, 30):
for i in range(len(markers)):
pose_dict[markers[i]] = [results["positions"][frame][i][0], results["positions"][frame][i][1], results["positions"][frame][i][2]]
ax.scatter(results["positions"][frame][i][0], results["positions"][frame][i][1], results["positions"][frame][i][2], c="g")
#for link in links:
#if len(link)>1:
#ax.plot3D([pose_dict[link[0]][0], pose_dict[link[1]][0]],
#[pose_dict[link[0]][1], pose_dict[link[1]][1]],
#[pose_dict[link[0]][2], pose_dict[link[1]][2]], c="g")
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax = fig.add_subplot(326, projection='3d')
ax.title.set_text('Trajectory Optimisation - Top')
ax.view_init(elev=80,azim=60)
for frame in range(20, 30):
for i in range(len(markers)):
pose_dict[markers[i]] = [results["positions"][frame][i][0], results["positions"][frame][i][1], results["positions"][frame][i][2]]
ax.scatter(results["positions"][frame][i][0], results["positions"][frame][i][1], results["positions"][frame][i][2], c="g")
#for link in links:
#if len(link)>1:
#ax.plot3D([pose_dict[link[0]][0], pose_dict[link[1]][0]],
#[pose_dict[link[0]][1], pose_dict[link[1]][1]],
#[pose_dict[link[0]][2], pose_dict[link[1]][2]], c="g")
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
#ax.set_zlabel('Z Axis')
plt.show()
import analyse as an
import numpy as np
import math
import glob
import os
scene_path = "C://Users//user-pc//Documents//Scripts//FYP_tests//GT//scene_sba.json"
#scene_fpath = 'C://Users//user-pc//Documents//Scripts//09//scene_sba.json'
currdir = "C://Users/user-pc/Documents/Scripts/FYP"
skel_name = "cheetah_serious"
skelly_dir = os.path.join(currdir, "skeletons", (skel_name + ".pickle"))
results_dir = os.path.join(currdir, "data", "results", (skel_name + ".pickle"))
project_dir = "C://Users//user-pc//Documents//Scripts//FYP_tests//GT"
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
#print(points_2d_df)
pts1 = []
pts2 = []
pts3 = []
pts4 = []
pts5 = []
pts6 = []
for i in range(100, 110):
pts1.append(points_2d_df[points_2d_df["frame"] ==
str("labeled-data\\09_03_2019LilyRun1CAM1\\img" +
str(i) + ".png")][["x", "y"]].values)
pts2.append(points_2d_df[points_2d_df["frame"] ==
str("labeled-data\\09_03_2019LilyRun1CAM2\\img" +
str(i) + ".png")][["x", "y"]].values)
pts3.append(points_2d_df[points_2d_df["frame"] ==