import csv import os import numpy as np sum_shoulder_left_error = 0 sum_elbow_left_error = 0 sum_shoulder_right_error = 0 sum_elbow_right_error = 0 sum_knee_left_error = 0 sum_ankle_left_error = 0 sum_knee_right_error = 0 sum_ankle_right_error = 0 def get_vector(row, joint_name): """ Get vector from csv row and joint_name (convert string to float) Parameters: row: row from csv joint_name: name of joint Returns: vector: vector with type float """ vector = row[joint_name].split(';') vector = list(map(float, vector)) return vector def get_angle(a, b, c): """ Get angle b (imagine there are line ba and bc) Parameters: a: Vector a b: Vector b c: Vector c Returns: angle: Angle b in degrees """ p = np.subtract(a, b) q = np.subtract(c, b) angle = np.arccos(np.dot(p, q) / (np.linalg.norm(p) * np.linalg.norm(q))) return np.rad2deg(angle) path = os.path.join(os.getcwd(), 'Assets\\demo_and_body_positions.csv') with open(path, newline='') as csvfile: reader = csv.DictReader(csvfile) for row in reader: demo_spine_shoulder = get_vector(row, 'Demo_SpineShoulder') demo_shoulder_left = get_vector(row, 'Demo_ShoulderLeft') demo_elbow_left = get_vector(row, 'Demo_ElbowLeft') demo_wrist_left = get_vector(row, 'Demo_WristLeft') demo_shoulder_right = get_vector(row, 'Demo_ShoulderRight') demo_elbow_right = get_vector(row, 'Demo_ElbowRight') demo_wrist_right = get_vector(row, 'Demo_WristRight') demo_hip_left = get_vector(row, 'Demo_HipLeft') demo_knee_left = get_vector(row, 'Demo_KneeLeft') demo_ankle_left = get_vector(row, 'Demo_AnkleLeft') demo_foot_left = get_vector(row, 'Demo_FootLeft') demo_hip_right = get_vector(row, 'Demo_HipRight') demo_knee_right = get_vector(row, 'Demo_KneeRight') demo_ankle_right = get_vector(row, 'Demo_AnkleRight') demo_foot_right = get_vector(row, 'Demo_FootRight') body_spine_shoulder = get_vector(row, 'Body_SpineShoulder') body_shoulder_left = get_vector(row, 'Body_ShoulderLeft') body_elbow_left = get_vector(row, 'Body_ElbowLeft') body_wrist_left = get_vector(row, 'Body_WristLeft') body_shoulder_right = get_vector(row, 'Body_ShoulderRight') body_elbow_right = get_vector(row, 'Body_ElbowRight') body_wrist_right = get_vector(row, 'Body_WristRight') body_hip_left = get_vector(row, 'Body_HipLeft') body_knee_left = get_vector(row, 'Body_KneeLeft') body_ankle_left = get_vector(row, 'Body_AnkleLeft') body_foot_left = get_vector(row, 'Body_FootLeft') body_hip_right = get_vector(row, 'Body_HipRight') body_knee_right = get_vector(row, 'Body_KneeRight') body_ankle_right = get_vector(row, 'Body_AnkleRight') body_foot_right = get_vector(row, 'Body_FootRight') # Shoulder left demo_angle_shoulder_left = get_angle(demo_spine_shoulder, demo_shoulder_left, demo_elbow_left) body_angle_shoulder_left = get_angle(body_spine_shoulder, body_shoulder_left, body_elbow_left) shoulder_left_error = abs(demo_angle_shoulder_left - body_angle_shoulder_left) sum_shoulder_left_error += shoulder_left_error # Elbow left demo_angle_elbow_left = get_angle(demo_shoulder_left, demo_elbow_left, demo_wrist_left) body_angle_elbow_left = get_angle(body_shoulder_left, body_elbow_left, body_wrist_left) elbow_left_error = abs(demo_angle_elbow_left - body_angle_elbow_left) sum_elbow_left_error += elbow_left_error # Shoulder right demo_angle_shoulder_right = get_angle(demo_spine_shoulder, demo_shoulder_right, demo_elbow_right) body_angle_shoulder_right = get_angle(body_spine_shoulder, body_shoulder_right, body_elbow_right) shoulder_right_error = abs(demo_angle_shoulder_right - body_angle_shoulder_right) sum_shoulder_right_error += shoulder_right_error # Elbow right demo_angle_elbow_right = get_angle(demo_shoulder_right, demo_elbow_right, demo_wrist_right) body_angle_elbow_right = get_angle(body_shoulder_right, body_elbow_right, body_wrist_right) elbow_right_error = abs(demo_angle_elbow_right - body_angle_elbow_right) sum_elbow_right_error += elbow_right_error # Knee left demo_angle_knee_left = get_angle(demo_hip_left, demo_knee_left, demo_ankle_left) body_angle_knee_left = get_angle(body_hip_left, body_knee_left, body_ankle_left) knee_left_error = abs(demo_angle_knee_left - body_angle_knee_left) sum_knee_left_error += knee_left_error # Ankle left demo_angle_ankle_left = get_angle(demo_knee_left, demo_ankle_left, demo_foot_left) body_angle_ankle_left = get_angle(body_knee_left, body_ankle_left, body_foot_left) ankle_left_error = abs(demo_angle_ankle_left - body_angle_ankle_left) sum_ankle_left_error += ankle_left_error # Knee right demo_angle_knee_right = get_angle(demo_hip_right, demo_knee_right, demo_ankle_right) body_angle_knee_right = get_angle(body_hip_right, body_knee_right, body_ankle_right) knee_right_error = abs(demo_angle_knee_right - body_angle_knee_right) sum_knee_right_error += knee_right_error # Ankle right demo_angle_ankle_right = get_angle(demo_knee_right, demo_ankle_right, demo_foot_right) body_angle_ankle_right = get_angle(body_knee_right, body_ankle_right, body_foot_right) ankle_right_error = abs(demo_angle_ankle_right - body_angle_ankle_right) sum_ankle_right_error += ankle_right_error size = reader.line_num - 1 print("shoulder_left_error: ", sum_shoulder_left_error / size) print("elbow_left_error: ", sum_elbow_left_error / size) print("shoulder_right_error: ", sum_shoulder_right_error / size) print("elbow_right_error: ", sum_elbow_right_error / size) print("knee_left_error: ", sum_knee_left_error / size) print("ankle_left_error: ", sum_ankle_left_error / size) print("knee_right_error: ", sum_knee_right_error / size) print("ankle_right_error: ", sum_ankle_right_error / size)