def plot_states():
fig = plt.figure()
ax =fig.add_subplot(111)
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"]
print(len(results["dx"]))
xs=[]
ys=[]
zs=[]
for state in results["x"]:
xs.append(state[3])
ys.append(state[21])
zs.append(state[39])
ax.plot(xs, label="phi")
ax.plot(ys, label="theta")
ax.plot(zs, label="psi")
plt.xlabel("Frame")
plt.ylabel("Magnitude of state (rad)")
plt.title("Tail phi, theta, and psi states for a 100-frame trajectory")
plt.legend()
plt.grid()
plt.show()
def plot_results(frame=0) -> None:
"""
Plots results for the given skeleton (frame 0)
"""
pose_dict = {}
currdir = os.getcwd()
skel_name = (field_name1.get())
skelly_dir = os.path.join(
"C://Users//user-pc//Documents//Scripts//amaan", "skeletons",
("new_human.pickle"))
results_dir = os.path.join(
"C://Users//user-pc//Documents//Scripts//amaan", "data",
"results", ("traj_results.pickle"))
skel_dict = bd.load_skeleton(skelly_dir)
results = an.load_pickle(results_dir)
links = skel_dict["links"]
markers = skel_dict["markers"]
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]
]
a.scatter(results["positions"][frame][i][0],
results["positions"][frame][i][1],
results["positions"][frame][i][2])
#print(pose_dict)
for link in links:
if len(link) > 1:
a.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]])
update_canvas()
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()
for i in range(100, 110):
pts3d.append(
np.array(
[points_3d_df[points_3d_df["frame"] == i][["x", "y",
"z"]].values]))
pts3d = np.array(pts3d)
print(len(pts1[0]))
im = cv2.imread("C://Users//user-pc//Documents//Scripts//FYP//src//img100.png")
RGB_img = cv2.cvtColor(im, cv2.COLOR_BGR2RGB)
fig = plt.figure()
ax = fig.add_subplot(312)
ax.imshow(RGB_img)
ax.title.set_text("Camera 2")
skel_dict = bd.load_skeleton(skelly_dir)
results = an.load_pickle(results_dir)
k, d, r, t, _ = utils.load_scene(scene_path)
d = d.reshape((-1, 4))
def load_data(file_path):
with open(file_path, 'rb') as f:
data = pickle.load(f)
return data
pts_3d_trajopt = results