(positions, feet_thre, n_raw_offsets, kinematic_chain, face_joint_indx, fid_r, fid_l)
| 37 | |
| 38 | |
| 39 | def extract_features(positions, feet_thre, n_raw_offsets, kinematic_chain, face_joint_indx, fid_r, fid_l): |
| 40 | global_positions = positions.copy() |
| 41 | """ Get Foot Contacts """ |
| 42 | |
| 43 | def foot_detect(positions, thres): |
| 44 | velfactor, heightfactor = np.array([thres, thres]), np.array([3.0, 2.0]) |
| 45 | |
| 46 | feet_l_x = (positions[1:, fid_l, 0] - positions[:-1, fid_l, 0]) ** 2 |
| 47 | feet_l_y = (positions[1:, fid_l, 1] - positions[:-1, fid_l, 1]) ** 2 |
| 48 | feet_l_z = (positions[1:, fid_l, 2] - positions[:-1, fid_l, 2]) ** 2 |
| 49 | # feet_l_h = positions[:-1,fid_l,1] |
| 50 | # feet_l = (((feet_l_x + feet_l_y + feet_l_z) < velfactor) & (feet_l_h < heightfactor)).astype(np.float64) |
| 51 | feet_l = ((feet_l_x + feet_l_y + feet_l_z) < velfactor).astype(np.float64) |
| 52 | |
| 53 | feet_r_x = (positions[1:, fid_r, 0] - positions[:-1, fid_r, 0]) ** 2 |
| 54 | feet_r_y = (positions[1:, fid_r, 1] - positions[:-1, fid_r, 1]) ** 2 |
| 55 | feet_r_z = (positions[1:, fid_r, 2] - positions[:-1, fid_r, 2]) ** 2 |
| 56 | # feet_r_h = positions[:-1,fid_r,1] |
| 57 | # feet_r = (((feet_r_x + feet_r_y + feet_r_z) < velfactor) & (feet_r_h < heightfactor)).astype(np.float64) |
| 58 | feet_r = (((feet_r_x + feet_r_y + feet_r_z) < velfactor)).astype(np.float64) |
| 59 | return feet_l, feet_r |
| 60 | |
| 61 | # |
| 62 | feet_l, feet_r = foot_detect(positions, feet_thre) |
| 63 | # feet_l, feet_r = foot_detect(positions, 0.002) |
| 64 | |
| 65 | '''Quaternion and Cartesian representation''' |
| 66 | r_rot = None |
| 67 | |
| 68 | def get_rifke(positions): |
| 69 | '''Local pose''' |
| 70 | positions[..., 0] -= positions[:, 0:1, 0] |
| 71 | positions[..., 2] -= positions[:, 0:1, 2] |
| 72 | '''All pose face Z+''' |
| 73 | positions = qrot_np(np.repeat(r_rot[:, None], positions.shape[1], axis=1), positions) |
| 74 | return positions |
| 75 | |
| 76 | def get_quaternion(positions): |
| 77 | skel = Skeleton(n_raw_offsets, kinematic_chain, "cpu") |
| 78 | # (seq_len, joints_num, 4) |
| 79 | quat_params = skel.inverse_kinematics_np(positions, face_joint_indx, smooth_forward=False) |
| 80 | |
| 81 | '''Fix Quaternion Discontinuity''' |
| 82 | quat_params = qfix(quat_params) |
| 83 | # (seq_len, 4) |
| 84 | r_rot = quat_params[:, 0].copy() |
| 85 | # print(r_rot[0]) |
| 86 | '''Root Linear Velocity''' |
| 87 | # (seq_len - 1, 3) |
| 88 | velocity = (positions[1:, 0] - positions[:-1, 0]).copy() |
| 89 | # print(r_rot.shape, velocity.shape) |
| 90 | velocity = qrot_np(r_rot[1:], velocity) |
| 91 | '''Root Angular Velocity''' |
| 92 | # (seq_len - 1, 4) |
| 93 | r_velocity = qmul_np(r_rot[1:], qinv_np(r_rot[:-1])) |
| 94 | quat_params[1:, 0] = r_velocity |
| 95 | # (seq_len, joints_num, 4) |
| 96 | return quat_params, r_velocity, velocity, r_rot |
nothing calls this directly
no test coverage detected