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"] ==