From 7d0a41dedd2d41afc0879ee051e0efb37fa2d7e8 Mon Sep 17 00:00:00 2001 From: ark Date: Thu, 23 Dec 2021 15:15:45 +0700 Subject: [PATCH] working rotation hands --- ModelUE4.py | 436 +++++++++++++++++++++++++++++++++++++++++++++++- config.yml | 2 +- hpe_mp_class.py | 56 ++++--- 3 files changed, 466 insertions(+), 28 deletions(-) diff --git a/ModelUE4.py b/ModelUE4.py index 8418782..c5a7e8f 100644 --- a/ModelUE4.py +++ b/ModelUE4.py @@ -1242,7 +1242,7 @@ def rhandconverttranslation(data): bodyaposeworld(data_apose) tvec_pose = [data_apose["hand_r"]["translation"]["x"], data_apose["hand_r"]["translation"]["y"], data_apose["hand_r"]["translation"]["z"]] tvec[0] = tvec[0] - tvec_pose[0] - tvec[1] = tvec[1] - tvec_pose[1] + tvec[1] = -(tvec[1] - tvec_pose[1]) tvec[2] = tvec[2] - tvec_pose[2] bonetranslation(data, "pinky_4_r", tvec) @@ -1268,7 +1268,7 @@ def lhandconverttranslation(data): bodyaposeworld(data_apose) tvec_pose = [data_apose["hand_l"]["translation"]["x"], data_apose["hand_l"]["translation"]["y"], data_apose["hand_l"]["translation"]["z"]] tvec[0] = tvec[0] - tvec_pose[0] - tvec[1] = tvec[1] - tvec_pose[1] + tvec[1] = -(tvec[1] - tvec_pose[1]) tvec[2] = tvec[2] - tvec_pose[2] bonetranslation(data, "pinky_4_l", tvec) @@ -1920,3 +1920,435 @@ def lhandconvert(lhandlms, data, coef): data["index_1_l"]["rotation"]["rotx"] = data["hand_l"]["rotation"]["rotx"] data["index_1_l"]["rotation"]["roty"] = data["hand_l"]["rotation"]["roty"] data["index_1_l"]["rotation"]["rotz"] = data["hand_l"]["rotation"]["rotz"] + +def getVecNorm(vec): + length = np.sqrt(pow(vec[0],2)+pow(vec[1],2)+pow(vec[2],2)) + vec_norm = [vec[0]/length, + vec[1]/length, + vec[2]/length] + return vec_norm + +def getRotaionVect(vec1, vec2): + vec1_norm = getVecNorm(vec1) + vec2_norm = getVecNorm(vec2) + cr = np.cross(vec1_norm, vec2_norm) + length = np.sqrt(pow(cr[0],2)+pow(cr[1],2)+pow(cr[2],2)) + cr[0] /= length + cr[1] /= length + cr[2] /= length + angle = np.arccos(np.dot(vec1_norm, vec2_norm)) + return (cr[0],cr[1],cr[2],angle) + +def toEuler(x, y, z, angle): + s = np.sin(angle) + c = np.cos(angle) + t = 1-c + if ((x*y*t + z*s) > 0.998): + heading = 2*np.arctan2(x*np.sin(angle/2),np.cos(angle/2)) + attitude = np.pi/2 + bank = 0 + heading *= 180 / np.pi + attitude *= 180 / np.pi + bank *= 180 / np.pi + return (heading, attitude, bank) + if ((x*y*t + z*s) < -0.998): + heading = -2*np.arctan2(x*np.sin(angle/2),np.cos(angle/2)) + attitude = -np.pi/2 + bank = 0 + heading *= 180 / np.pi + attitude *= 180 / np.pi + bank *= 180 / np.pi + return (heading, attitude, bank) + heading = np.arctan2(y * s- x * z * t , 1 - (y*y+ z*z ) * t) + attitude = np.arcsin(x * y * t + z * s) + bank = np.arctan2(x * s - y * z * t , 1 - (x*x + z*z) * t) + heading *= 180 / np.pi + attitude *= 180 / np.pi + bank *= 180 / np.pi + return (heading, attitude, bank) + +def lfingerRot(current, child, data, dataxyz): + vec2 = [dataxyz[child][0] - dataxyz[current][0], + dataxyz[child][1] - dataxyz[current][1], + dataxyz[child][2] - dataxyz[current][2]] + vec1 = [1, 0, 0] + + rotvec = getRotaionVect(vec1, vec2) + euler = toEuler(rotvec[0], rotvec[1], rotvec[2], rotvec[3]) + + data[current]["rotation"]["rotx"] = -euler[0] + data[current]["rotation"]["roty"] = euler[1] + data[current]["rotation"]["rotz"] = -90 + +def lhandRot(current, child, data, dataxyz, support1, support2): + vec2 = [dataxyz[child][0] - dataxyz[current][0], + dataxyz[child][1] - dataxyz[current][1], + dataxyz[child][2] - dataxyz[current][2]] + vec1 = [1, 0, 0] + + rotvec = getRotaionVect(vec1, vec2) + euler = toEuler(rotvec[0],rotvec[1],rotvec[2],rotvec[3]) + # print(euler) + + vec3 = [dataxyz[support2][0] - dataxyz[support1][0], + dataxyz[support2][1] - dataxyz[support1][1], + dataxyz[support2][2] - dataxyz[support1][2]] + + vec2_norm = getVecNorm(vec2) + vec3_norm = getVecNorm(vec3) + sup2 = np.cross(vec2_norm, vec3_norm) + sup2_norm = getVecNorm(sup2) + sup1 = [0, 1, 0] + sup1_norm = getVecNorm(sup1) + + rotsup = getRotaionVect(sup1_norm, sup2_norm) + euler_sup = toEuler(rotsup[0], rotsup[1], rotsup[2], rotsup[3]) + print(euler_sup) + + data[current]["rotation"]["rotx"] = -euler[0] + data[current]["rotation"]["roty"] = euler[1] + data[current]["rotation"]["rotz"] = -euler_sup[2] + +def lhandrot(lhandlms, data, coef): + dataxyz = {} + + rootx = lhandlms[17].x * coef + rooty = -lhandlms[17].z * coef + rootz = -lhandlms[17].y * coef + rootv = 1.0 + dataxyz["pinky_2_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[18].x * coef + rooty = -lhandlms[18].z * coef + rootz = -lhandlms[18].y * coef + rootv = 1.0 + dataxyz["pinky_3_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[19].x * coef + rooty = -lhandlms[19].z * coef + rootz = -lhandlms[19].y * coef + rootv = 1.0 + dataxyz["pinky_4_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[20].x * coef + rooty = -lhandlms[20].z * coef + rootz = -lhandlms[20].y * coef + rootv = 1.0 + dataxyz["pinky_5_l"] = [rootx, rooty, rootz, rootv] + + lfingerRot("pinky_2_l", "pinky_3_l", data, dataxyz) + lfingerRot("pinky_3_l", "pinky_4_l", data, dataxyz) + lfingerRot("pinky_4_l", "pinky_5_l", data, dataxyz) + + rootx = lhandlms[13].x * coef + rooty = -lhandlms[13].z * coef + rootz = -lhandlms[13].y * coef + rootv = 1.0 + dataxyz["ring_2_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[14].x * coef + rooty = -lhandlms[14].z * coef + rootz = -lhandlms[14].y * coef + rootv = 1.0 + dataxyz["ring_3_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[15].x * coef + rooty = -lhandlms[15].z * coef + rootz = -lhandlms[15].y * coef + rootv = 1.0 + dataxyz["ring_4_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[16].x * coef + rooty = -lhandlms[16].z * coef + rootz = -lhandlms[16].y * coef + rootv = 1.0 + dataxyz["ring_5_l"] = [rootx, rooty, rootz, rootv] + + lfingerRot("ring_2_l", "ring_3_l", data, dataxyz) + lfingerRot("ring_3_l", "ring_4_l", data, dataxyz) + lfingerRot("ring_4_l", "ring_5_l", data, dataxyz) + + rootx = lhandlms[9].x * coef + rooty = -lhandlms[9].z * coef + rootz = -lhandlms[9].y * coef + rootv = 1.0 + dataxyz["middle_2_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[10].x * coef + rooty = -lhandlms[10].z * coef + rootz = -lhandlms[10].y * coef + rootv = 1.0 + dataxyz["middle_3_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[11].x * coef + rooty = -lhandlms[11].z * coef + rootz = -lhandlms[11].y * coef + rootv = 1.0 + dataxyz["middle_4_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[12].x * coef + rooty = -lhandlms[12].z * coef + rootz = -lhandlms[12].y * coef + rootv = 1.0 + dataxyz["middle_5_l"] = [rootx, rooty, rootz, rootv] + + lfingerRot("middle_2_l", "middle_3_l", data, dataxyz) + lfingerRot("middle_3_l", "middle_4_l", data, dataxyz) + lfingerRot("middle_4_l", "middle_5_l", data, dataxyz) + + rootx = lhandlms[5].x * coef + rooty = -lhandlms[5].z * coef + rootz = -lhandlms[5].y * coef + rootv = 1.0 + dataxyz["index_2_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[6].x * coef + rooty = -lhandlms[6].z * coef + rootz = -lhandlms[6].y * coef + rootv = 1.0 + dataxyz["index_3_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[7].x * coef + rooty = -lhandlms[7].z * coef + rootz = -lhandlms[7].y * coef + rootv = 1.0 + dataxyz["index_4_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[8].x * coef + rooty = -lhandlms[8].z * coef + rootz = -lhandlms[8].y * coef + rootv = 1.0 + dataxyz["index_5_l"] = [rootx, rooty, rootz, rootv] + + lfingerRot("index_2_l", "index_3_l", data, dataxyz) + lfingerRot("index_3_l", "index_4_l", data, dataxyz) + lfingerRot("index_4_l", "index_5_l", data, dataxyz) + + rootx = lhandlms[1].x * coef + rooty = -lhandlms[1].z * coef + rootz = -lhandlms[1].y * coef + rootv = 1.0 + dataxyz["thumb_2_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[2].x * coef + rooty = -lhandlms[2].z * coef + rootz = -lhandlms[2].y * coef + rootv = 1.0 + dataxyz["thumb_3_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[3].x * coef + rooty = -lhandlms[3].z * coef + rootz = -lhandlms[3].y * coef + rootv = 1.0 + dataxyz["thumb_4_l"] = [rootx, rooty, rootz, rootv] + + rootx = lhandlms[4].x * coef + rooty = -lhandlms[4].z * coef + rootz = -lhandlms[4].y * coef + rootv = 1.0 + dataxyz["thumb_5_l"] = [rootx, rooty, rootz, rootv] + + lfingerRot("thumb_2_l", "thumb_3_l", data, dataxyz) + lfingerRot("thumb_3_l", "thumb_4_l", data, dataxyz) + lfingerRot("thumb_4_l", "thumb_5_l", data, dataxyz) + + rootx = lhandlms[0].x * coef + rooty = -lhandlms[0].z * coef + rootz = -lhandlms[0].y * coef + rootv = 1.0 + dataxyz["hand_l"] = [rootx, rooty, rootz, rootv] + + lhandRot("hand_l", "middle_2_l", data, dataxyz, "pinky_2_l", "index_2_l") + +def rhandRot(current, child, data, dataxyz, support1, support2): + vec2 = [dataxyz[child][0] - dataxyz[current][0], + dataxyz[child][1] - dataxyz[current][1], + dataxyz[child][2] - dataxyz[current][2]] + vec1 = [-1, 0, 0] + + rotvec = getRotaionVect(vec1, vec2) + euler = toEuler(rotvec[0],rotvec[1],rotvec[2],rotvec[3]) + # print(euler) + + vec3 = [dataxyz[support2][0] - dataxyz[support1][0], + dataxyz[support2][1] - dataxyz[support1][1], + dataxyz[support2][2] - dataxyz[support1][2]] + + vec2_norm = getVecNorm(vec2) + vec3_norm = getVecNorm(vec3) + sup2 = np.cross(vec2_norm, vec3_norm) + sup2_norm = getVecNorm(sup2) + sup1 = [0, 1, 0] + sup1_norm = getVecNorm(sup1) + + rotsup = getRotaionVect(sup1_norm, sup2_norm) + euler_sup = toEuler(rotsup[0], rotsup[1], rotsup[2], rotsup[3]) + print(euler_sup) + + data[current]["rotation"]["rotx"] = -euler[0] + data[current]["rotation"]["roty"] = euler[1] + data[current]["rotation"]["rotz"] = -euler_sup[2] + +def rfingerRot(current, child, data, dataxyz): + vec2 = [dataxyz[child][0] - dataxyz[current][0], + dataxyz[child][1] - dataxyz[current][1], + dataxyz[child][2] - dataxyz[current][2]] + vec1 = [-1, 0, 0] + + rotvec = getRotaionVect(vec1, vec2) + euler = toEuler(rotvec[0], rotvec[1], rotvec[2], rotvec[3]) + + data[current]["rotation"]["rotx"] = -euler[0] + data[current]["rotation"]["roty"] = euler[1] + data[current]["rotation"]["rotz"] = 90 + +def rhandrot(rhandlms, data, coef): + dataxyz = {} + + rootx = rhandlms[17].x * coef + rooty = -rhandlms[17].z * coef + rootz = -rhandlms[17].y * coef + rootv = 1.0 + dataxyz["pinky_2_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[18].x * coef + rooty = -rhandlms[18].z * coef + rootz = -rhandlms[18].y * coef + rootv = 1.0 + dataxyz["pinky_3_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[19].x * coef + rooty = -rhandlms[19].z * coef + rootz = -rhandlms[19].y * coef + rootv = 1.0 + dataxyz["pinky_4_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[20].x * coef + rooty = -rhandlms[20].z * coef + rootz = -rhandlms[20].y * coef + rootv = 1.0 + dataxyz["pinky_5_r"] = [rootx, rooty, rootz, rootv] + + rfingerRot("pinky_2_r", "pinky_3_r", data, dataxyz) + rfingerRot("pinky_3_r", "pinky_4_r", data, dataxyz) + rfingerRot("pinky_4_r", "pinky_5_r", data, dataxyz) + + rootx = rhandlms[13].x * coef + rooty = -rhandlms[13].z * coef + rootz = -rhandlms[13].y * coef + rootv = 1.0 + dataxyz["ring_2_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[14].x * coef + rooty = -rhandlms[14].z * coef + rootz = -rhandlms[14].y * coef + rootv = 1.0 + dataxyz["ring_3_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[15].x * coef + rooty = -rhandlms[15].z * coef + rootz = -rhandlms[15].y * coef + rootv = 1.0 + dataxyz["ring_4_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[16].x * coef + rooty = -rhandlms[16].z * coef + rootz = -rhandlms[16].y * coef + rootv = 1.0 + dataxyz["ring_5_r"] = [rootx, rooty, rootz, rootv] + + rfingerRot("ring_2_r", "ring_3_r", data, dataxyz) + rfingerRot("ring_3_r", "ring_4_r", data, dataxyz) + rfingerRot("ring_4_r", "ring_5_r", data, dataxyz) + + rootx = rhandlms[9].x * coef + rooty = -rhandlms[9].z * coef + rootz = -rhandlms[9].y * coef + rootv = 1.0 + dataxyz["middle_2_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[10].x * coef + rooty = -rhandlms[10].z * coef + rootz = -rhandlms[10].y * coef + rootv = 1.0 + dataxyz["middle_3_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[11].x * coef + rooty = -rhandlms[11].z * coef + rootz = -rhandlms[11].y * coef + rootv = 1.0 + dataxyz["middle_4_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[12].x * coef + rooty = -rhandlms[12].z * coef + rootz = -rhandlms[12].y * coef + rootv = 1.0 + dataxyz["middle_5_r"] = [rootx, rooty, rootz, rootv] + + rfingerRot("middle_2_r", "middle_3_r", data, dataxyz) + rfingerRot("middle_3_r", "middle_4_r", data, dataxyz) + rfingerRot("middle_4_r", "middle_5_r", data, dataxyz) + + rootx = rhandlms[5].x * coef + rooty = -rhandlms[5].z * coef + rootz = -rhandlms[5].y * coef + rootv = 1.0 + dataxyz["index_2_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[6].x * coef + rooty = -rhandlms[6].z * coef + rootz = -rhandlms[6].y * coef + rootv = 1.0 + dataxyz["index_3_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[7].x * coef + rooty = -rhandlms[7].z * coef + rootz = -rhandlms[7].y * coef + rootv = 1.0 + dataxyz["index_4_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[8].x * coef + rooty = -rhandlms[8].z * coef + rootz = -rhandlms[8].y * coef + rootv = 1.0 + dataxyz["index_5_r"] = [rootx, rooty, rootz, rootv] + + rfingerRot("index_2_r", "index_3_r", data, dataxyz) + rfingerRot("index_3_r", "index_4_r", data, dataxyz) + rfingerRot("index_4_r", "index_5_r", data, dataxyz) + + rootx = rhandlms[1].x * coef + rooty = -rhandlms[1].z * coef + rootz = -rhandlms[1].y * coef + rootv = 1.0 + dataxyz["thumb_2_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[2].x * coef + rooty = -rhandlms[2].z * coef + rootz = -rhandlms[2].y * coef + rootv = 1.0 + dataxyz["thumb_3_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[3].x * coef + rooty = -rhandlms[3].z * coef + rootz = -rhandlms[3].y * coef + rootv = 1.0 + dataxyz["thumb_4_r"] = [rootx, rooty, rootz, rootv] + + rootx = rhandlms[4].x * coef + rooty = -rhandlms[4].z * coef + rootz = -rhandlms[4].y * coef + rootv = 1.0 + dataxyz["thumb_5_r"] = [rootx, rooty, rootz, rootv] + + rfingerRot("thumb_2_r", "thumb_3_r", data, dataxyz) + rfingerRot("thumb_3_r", "thumb_4_r", data, dataxyz) + rfingerRot("thumb_4_r", "thumb_5_r", data, dataxyz) + + rootx = rhandlms[0].x * coef + rooty = -rhandlms[0].z * coef + rootz = -rhandlms[0].y * coef + rootv = 1.0 + dataxyz["hand_r"] = [rootx, rooty, rootz, rootv] + + rhandRot("hand_r", "middle_2_r", data, dataxyz, "pinky_2_r", "index_2_r") diff --git a/config.yml b/config.yml index 8864826..881e954 100644 --- a/config.yml +++ b/config.yml @@ -1,5 +1,5 @@ # Input -address_input: "0" # input video path or webcam index +address_input: "1" # input video path or webcam index # Image processing scale_pose: 35.4 # shoulder width diff --git a/hpe_mp_class.py b/hpe_mp_class.py index a0b26f5..7804253 100644 --- a/hpe_mp_class.py +++ b/hpe_mp_class.py @@ -172,42 +172,48 @@ class hpe_mp_class(): if world: bodyaposeworld(data) if self.holistic_use: - poseslms = {} - maxy = 0 - if self.results_hol.pose_landmarks: - for id, lm in enumerate(self.results_hol.pose_landmarks.landmark): - poseslms[id] = lm - if lm.y > maxy: - maxy = lm.y - - if old_world: - bodyconvert(poseslms, data, self.coef, maxy) - else: - bodyconvertwithrot(poseslms, data, self.coef, maxy) - + # poseslms = {} + # maxy = 0 + # if self.results_hol.pose_landmarks: + # for id, lm in enumerate(self.results_hol.pose_landmarks.landmark): + # poseslms[id] = lm + # if lm.y > maxy: + # maxy = lm.y + # + # if old_world: + # bodyconvert(poseslms, data, self.coef, maxy) + # else: + # bodyconvertwithrot(poseslms, data, self.coef, maxy) + # rhandlms = {} if self.results_hol.right_hand_landmarks: for id, lm in enumerate(self.results_hol.right_hand_landmarks.landmark): rhandlms[id] = lm - if old_world: - rhandconvert(rhandlms, data, self.coef) - else: - rhandconvertwithrot(rhandlms, data, self.coef) - else: - rhandconverttranslation(data) + rhandrot(rhandlms, data, self.coef) + + # if old_world: + # rhandconvert(rhandlms, data, self.coef) + # else: + # rhandconvert(rhandlms, data, self.coef) + # # rhandconverttranslation(data) + # # else: + # # rhandconverttranslation(data) lhandlms = {} if self.results_hol.left_hand_landmarks: for id, lm in enumerate(self.results_hol.left_hand_landmarks.landmark): lhandlms[id] = lm - if old_world: - lhandconvert(lhandlms, data, self.coef) - else: - lhandconvertwithrot(lhandlms, data, self.coef) - else: - lhandconverttranslation(data) + lhandrot(lhandlms,data,self.coef) + + # if old_world: + # lhandconvert(lhandlms, data, self.coef) + # else: + # lhandconvert(lhandlms, data, self.coef) + # lhandconverttranslation(data) + # else: + # lhandconverttranslation(data) else: bodyaposelocal(data) if self.holistic_use: