Almost right rotation for the top body

This commit is contained in:
2021-12-13 13:07:31 +07:00
parent 7cc5954609
commit 47d9d7514a
2 changed files with 428 additions and 67 deletions

View File

@@ -43,10 +43,309 @@ def getAngle(avec0, avec1, bvec0, bvec1):
chis = float(avec0*bvec0 + avec1*bvec1)
znam1 = float(np.sqrt(pow(avec0, 2) + pow(avec1, 2)))
znam2 = float(np.sqrt(pow(bvec0, 2) + pow(bvec1, 2)))
if (znam1*znam2) == 0:
if (znam1*znam2) == 0 or np.fabs(chis) > np.fabs(znam1*znam2):
return None
return np.arccos(chis / (znam1 * znam2)) * 180. / np.pi
def HeadRot(poselms, coef):
head0x = poselms[0].x * coef
head0y = -poselms[0].z * coef
head0z = -poselms[0].y * coef
head7x = poselms[7].x * coef
head7y = -poselms[7].z * coef
head7z = -poselms[7].y * coef
head8x = poselms[8].x * coef
head8y = -poselms[8].z * coef
head8z = -poselms[8].y * coef
rotx = 90.0
p78x = 0.5*(head7x + head8x)
p78y = 0.5*(head7y + head8y)
p78z = 0.5*(head7z + head8z)
vec_head_x = [head0y - p78y, head0z - p78z]
alpha_x = np.arctan(vec_head_x[1]/vec_head_x[0])
rotx += (alpha_x) * 180. / np.pi
roty = 90.0
vec_head_y = [head8x - head7x, head8z - head7z]
alpha_y = np.arctan(vec_head_y[1] / vec_head_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 90.0
vec_head_z = [head8x - head7x, head8y - head7y]
alpha_z = np.arctan(vec_head_z[1] / vec_head_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [rotx, rotz, roty]
return rvec
def ShoulderLRot(poselms, coef):
sh11x = poselms[11].x * coef
sh11y = -poselms[11].z * coef
sh11z = -poselms[11].y * coef
sh13x = poselms[13].x * coef
sh13y = -poselms[13].z * coef
sh13z = -poselms[13].y * coef
rotx = 0.
roty = 0.
vec_sh_y = [sh13x - sh11x, sh13z - sh11z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [sh13x - sh11x, sh13y - sh11y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def ShoulderRRot(poselms, coef):
sh12x = poselms[12].x * coef
sh12y = -poselms[12].z * coef
sh12z = -poselms[12].y * coef
sh14x = poselms[14].x * coef
sh14y = -poselms[14].z * coef
sh14z = -poselms[14].y * coef
rotx = 0.
roty = 0.
vec_sh_y = [sh14x - sh12x, sh14z - sh12z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [sh14x - sh12x, sh14y - sh12y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def ElbowLRot(poselms, coef):
sh13x = poselms[13].x * coef
sh13y = -poselms[13].z * coef
sh13z = -poselms[13].y * coef
sh15x = poselms[15].x * coef
sh15y = -poselms[15].z * coef
sh15z = -poselms[15].y * coef
rotx = 0.
roty = 0.
vec_sh_y = [sh15x - sh13x, sh15z - sh13z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [sh15x - sh13x, sh15y - sh13y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def ElbowRRot(poselms, coef):
sh14x = poselms[14].x * coef
sh14y = -poselms[14].z * coef
sh14z = -poselms[14].y * coef
sh16x = poselms[16].x * coef
sh16y = -poselms[16].z * coef
sh16z = -poselms[16].y * coef
rotx = 0.
roty = 0.
vec_sh_y = [sh16x - sh14x, sh16z - sh14z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [sh16x - sh14x, sh16y - sh14y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def HandLRot(poselms, coef):
sh16x = poselms[16].x * coef
sh16y = -poselms[16].z * coef
sh16z = -poselms[16].y * coef
sh18x = poselms[18].x * coef
sh18y = -poselms[18].z * coef
sh18z = -poselms[18].y * coef
sh20x = poselms[20].x * coef
sh20y = -poselms[20].z * coef
sh20z = -poselms[20].y * coef
hand_vec_x = 0.5 * (sh18x + sh20x)
hand_vec_y = 0.5 * (sh18y + sh20y)
hand_vec_z = 0.5 * (sh18z + sh20z)
rotx = 0.
roty = 0.
vec_sh_y = [hand_vec_x - sh16x, hand_vec_z - sh16z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [hand_vec_x - sh16x, hand_vec_y - sh16y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def HandRRot(poselms, coef):
sh15x = poselms[15].x * coef
sh15y = -poselms[15].z * coef
sh15z = -poselms[15].y * coef
sh17x = poselms[17].x * coef
sh17y = -poselms[17].z * coef
sh17z = -poselms[17].y * coef
sh19x = poselms[19].x * coef
sh19y = -poselms[19].z * coef
sh19z = -poselms[19].y * coef
hand_vec_x = 0.5 * (sh17x + sh19x)
hand_vec_y = 0.5 * (sh17y + sh19y)
hand_vec_z = 0.5 * (sh17z + sh19z)
rotx = 0.
roty = 0.
vec_sh_y = [hand_vec_x - sh15x, hand_vec_z - sh15z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [hand_vec_x - sh15x, hand_vec_y - sh15y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def Spine5Rot(poselms, coef):
sh11x = poselms[11].x * coef
sh11y = -poselms[11].z * coef
sh11z = -poselms[11].y * coef
sh12x = poselms[12].x * coef
sh12y = -poselms[12].z * coef
sh12z = -poselms[12].y * coef
rotx = 90.0
# p78x = 0.5*(head7x + head8x)
# p78y = 0.5*(head7y + head8y)
# p78z = 0.5*(head7z + head8z)
# vec_head_x = [head0y - p78y, head0z - p78z]
# alpha_x = np.arctan(vec_head_x[1]/vec_head_x[0])
# rotx += (alpha_x) * 180. / np.pi
roty = -90.0
# vec_head_y = [head8x - head7x, head8z - head7z]
# alpha_y = np.arctan(vec_head_y[1] / vec_head_y[0])
# roty += (alpha_y) * 180. / np.pi
rotz = -90.0
# vec_head_z = [head8x - head7x, head8y - head7y]
# alpha_z = np.arctan(vec_head_z[1] / vec_head_z[0])
# rotz += (alpha_z) * 180. / np.pi
vec_spine_z = [sh12x - sh11x, sh12y - sh11y]
alpha_z = np.arctan(vec_spine_z[1]/vec_spine_z[0])
roty += (alpha_z) * 180. / np.pi
rvec = [rotx, roty, rotz]
return rvec
def PelvisRot(poselms, coef):
sh23x = poselms[23].x * coef
sh23y = -poselms[23].z * coef
sh23z = -poselms[23].y * coef
sh24x = poselms[24].x * coef
sh24y = -poselms[24].z * coef
sh24z = -poselms[24].y * coef
rotx = 90.0
# p78x = 0.5*(head7x + head8x)
# p78y = 0.5*(head7y + head8y)
# p78z = 0.5*(head7z + head8z)
# vec_head_x = [head0y - p78y, head0z - p78z]
# alpha_x = np.arctan(vec_head_x[1]/vec_head_x[0])
# rotx += (alpha_x) * 180. / np.pi
roty = -90.0
# vec_head_y = [head8x - head7x, head8z - head7z]
# alpha_y = np.arctan(vec_head_y[1] / vec_head_y[0])
# roty += (alpha_y) * 180. / np.pi
rotz = -90.0
# vec_head_z = [head8x - head7x, head8y - head7y]
# alpha_z = np.arctan(vec_head_z[1] / vec_head_z[0])
# rotz += (alpha_z) * 180. / np.pi
vec_spine_z = [sh24x - sh23x, sh24y - sh23y]
alpha_z = np.arctan(vec_spine_z[1]/vec_spine_z[0])
roty += (alpha_z) * 180. / np.pi
rvec = [rotx, roty, rotz]
return rvec
def HipLRot(poselms, coef):
sh23x = poselms[23].x * coef
sh23y = -poselms[23].z * coef
sh23z = -poselms[23].y * coef
sh25x = poselms[25].x * coef
sh25y = -poselms[25].z * coef
sh25z = -poselms[25].y * coef
rotx = 90.
roty = 0.
vec_sh_y = [sh25x - sh23x, sh25z - sh23z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [sh25x - sh23x, sh25y - sh23y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def HipRRot(poselms, coef):
sh24x = poselms[24].x * coef
sh24y = -poselms[24].z * coef
sh24z = -poselms[24].y * coef
sh26x = poselms[26].x * coef
sh26y = -poselms[26].z * coef
sh26z = -poselms[26].y * coef
rotx = 90.
roty = 0.
vec_sh_y = [sh26x - sh24x, sh26z - sh24z]
alpha_y = np.arctan(vec_sh_y[1] / vec_sh_y[0])
roty += (alpha_y) * 180. / np.pi
rotz = 0.
vec_sh_z = [sh26x - sh24x, sh26y - sh24y]
alpha_z = np.arctan(vec_sh_z[1] / vec_sh_z[0])
rotz += (alpha_z) * 180. / np.pi
rvec = [roty, rotz, rotx]
return rvec
def checkRot(tvec0, tvec1, tvec0_apose, tvec1_apose, rvec_apose):
# tvec0 - parent bone by root
# tvec1 - current bone by root
@@ -487,9 +786,9 @@ def updateValueAPoseWorld(data, dataxyz, parent_name, current_name):
data[current_name]["translation"]["x"] = dataxyz[current_name][0]
data[current_name]["translation"]["y"] = dataxyz[current_name][1]
data[current_name]["translation"]["z"] = dataxyz[current_name][2]
data[current_name]["rotation"]["rotx"] = rvec[0]
data[current_name]["rotation"]["roty"] = rvec[1]
data[current_name]["rotation"]["rotz"] = rvec[2]
# data[current_name]["rotation"]["rotx"] = rvec[0]
# data[current_name]["rotation"]["roty"] = rvec[1]
# data[current_name]["rotation"]["rotz"] = rvec[2]
data[current_name]["visible"] = dataxyz[current_name][3]
def bodyconvertwithrot(poseslms, data, coef, maxy):
@@ -660,14 +959,42 @@ def bodyconvertwithrot(poseslms, data, coef, maxy):
dataxyz["heel_l"] = [rootx, rooty, rootz, rootv]
updateValueAPoseWorld(data, dataxyz, "elbow_r", "hand_r")
rvecHandR = HandRRot(poseslms, coef)
data["hand_r"]["rotation"]["rotx"] = rvecHandR[0]
data["hand_r"]["rotation"]["roty"] = rvecHandR[1]
# data["hand_r"]["rotation"]["rotz"] = rvecHandR[2]
updateValueAPoseWorld(data, dataxyz, "elbow_l", "hand_l")
rvecHandL = HandLRot(poseslms, coef)
data["hand_l"]["rotation"]["rotx"] = rvecHandL[0]
data["hand_l"]["rotation"]["roty"] = rvecHandL[1]
# data["hand_l"]["rotation"]["rotz"] = rvecHandL[2]
updateValueAPoseWorld(data, dataxyz, "shoulder_r", "elbow_r")
rvecElbowR = ShoulderRRot(poseslms, coef)
data["elbow_r"]["rotation"]["rotx"] = rvecElbowR[0]
data["elbow_r"]["rotation"]["roty"] = rvecElbowR[1]
# data["elbow_r"]["rotation"]["rotz"] = rvecElbowR[2]
updateValueAPoseWorld(data, dataxyz, "shoulder_l", "elbow_l")
rvecElbowL = ShoulderLRot(poseslms, coef)
data["elbow_l"]["rotation"]["rotx"] = rvecElbowL[0]
data["elbow_l"]["rotation"]["roty"] = rvecElbowL[1]
# data["elbow_l"]["rotation"]["rotz"] = rvecElbowL[2]
updateValueAPoseWorld(data, dataxyz, "clavicle_r", "shoulder_r")
rvecShoulderR = ShoulderRRot(poseslms, coef)
data["shoulder_r"]["rotation"]["rotx"] = rvecShoulderR[0]
data["shoulder_r"]["rotation"]["roty"] = rvecShoulderR[1]
# data["shoulder_r"]["rotation"]["rotz"] = rvecShoulderR[2]
updateValueAPoseWorld(data, dataxyz, "clavicle_l", "shoulder_l")
rvecShoulderL = ShoulderLRot(poseslms, coef)
data["shoulder_l"]["rotation"]["rotx"] = rvecShoulderL[0]
data["shoulder_l"]["rotation"]["roty"] = rvecShoulderL[1]
# data["shoulder_l"]["rotation"]["rotz"] = rvecShoulderL[2]
updateValueAPoseWorld(data, dataxyz, "spine_05", "clavicle_r")
updateValueAPoseWorld(data, dataxyz, "spine_05", "clavicle_l")
updateValueAPoseWorld(data, dataxyz, "spine_05", "head")
rvecHead = HeadRot(poseslms, coef)
data["head"]["rotation"]["rotx"] = rvecHead[0]
data["head"]["rotation"]["roty"] = rvecHead[1]
# data["head"]["rotation"]["rotz"] = rvecHead[2]
updateValueAPoseWorld(data, dataxyz, "spine_04", "spine_05")
updateValueAPoseWorld(data, dataxyz, "spine_02", "spine_04")
updateValueAPoseWorld(data, dataxyz, "pelvis", "spine_02")
@@ -680,7 +1007,15 @@ def bodyconvertwithrot(poseslms, data, coef, maxy):
updateValueAPoseWorld(data, dataxyz, "hip_l", "knee_l")
updateValueAPoseWorld(data, dataxyz, "hip_r", "knee_r")
updateValueAPoseWorld(data, dataxyz, "pelvis", "hip_l")
rvecHipL = HipLRot(poseslms, coef)
# data["hip_l"]["rotation"]["rotx"] = rvecHipL[0]
# data["hip_l"]["rotation"]["roty"] = rvecHipL[1]
# data["hip_l"]["rotation"]["rotz"] = rvecHipL[2]
updateValueAPoseWorld(data, dataxyz, "pelvis", "hip_r")
rvecHipR = HipRRot(poseslms, coef)
# data["hip_r"]["rotation"]["rotx"] = rvecHipR[0]
# data["hip_r"]["rotation"]["roty"] = rvecHipR[1]
# data["hip_r"]["rotation"]["rotz"] = rvecHipR[2]
updateValueAPoseWorld(data, dataxyz, "root", "pelvis")
data["root"]["translation"]["x"] = dataxyz["root"][0]
@@ -699,6 +1034,32 @@ def bodyconvertwithrot(poseslms, data, coef, maxy):
data["spine_03"]["rotation"]["roty"] = data["spine_02"]["rotation"]["roty"]
data["spine_03"]["rotation"]["rotz"] = data["spine_02"]["rotation"]["rotz"]
rvecSpine5 = Spine5Rot(poseslms, coef)
data["spine_05"]["rotation"]["rotx"] = rvecSpine5[0]
data["spine_05"]["rotation"]["roty"] = rvecSpine5[1]
data["spine_05"]["rotation"]["rotz"] = rvecSpine5[2]
rvecPelvis = PelvisRot(poseslms, coef)
data["pelvis"]["rotation"]["rotx"] = rvecPelvis[0]
data["pelvis"]["rotation"]["roty"] = rvecPelvis[1]
data["pelvis"]["rotation"]["rotz"] = rvecPelvis[2]
data["spine_04"]["rotation"]["rotx"] = 0.4*rvecSpine5[0] + 0.6*rvecPelvis[0]
data["spine_04"]["rotation"]["roty"] = 0.4*rvecSpine5[1] + 0.6*rvecPelvis[1]
data["spine_04"]["rotation"]["rotz"] = 0.4*rvecSpine5[2] + 0.6*rvecPelvis[2]
data["spine_03"]["rotation"]["rotx"] = 0.25*rvecSpine5[0] + 0.75*rvecPelvis[0]
data["spine_03"]["rotation"]["roty"] = 0.25*rvecSpine5[1] + 0.75*rvecPelvis[1]
data["spine_03"]["rotation"]["rotz"] = 0.25*rvecSpine5[2] + 0.75*rvecPelvis[2]
data["spine_02"]["rotation"]["rotx"] = 0.12*rvecSpine5[0] + 0.88*rvecPelvis[0]
data["spine_02"]["rotation"]["roty"] = 0.12*rvecSpine5[1] + 0.88*rvecPelvis[1]
data["spine_02"]["rotation"]["rotz"] = 0.12*rvecSpine5[2] + 0.88*rvecPelvis[2]
data["spine_01"]["rotation"]["rotx"] = 0.05*rvecSpine5[0] + 0.95*rvecPelvis[0]
data["spine_01"]["rotation"]["roty"] = 0.05*rvecSpine5[1] + 0.95*rvecPelvis[1]
data["spine_01"]["rotation"]["rotz"] = 0.05*rvecSpine5[2] + 0.95*rvecPelvis[2]
def bodyconvert(poseslms, data, coef, maxy):
dataxyz = {}
@@ -899,7 +1260,7 @@ def rhandconverttranslation(data):
bonetranslation(data, "ring_4_r", tvec)
bonetranslation(data, "ring_3_r", tvec)
bonetranslation(data, "ring_2_r", tvec)
bonetranslation(data, "hand_r", tvec)
# bonetranslation(data, "hand_r", tvec)
def lhandconverttranslation(data):
tvec = [data["hand_l"]["translation"]["x"], data["hand_l"]["translation"]["y"], data["hand_l"]["translation"]["z"]]
@@ -925,7 +1286,7 @@ def lhandconverttranslation(data):
bonetranslation(data, "ring_4_l", tvec)
bonetranslation(data, "ring_3_l", tvec)
bonetranslation(data, "ring_2_l", tvec)
bonetranslation(data, "hand_l", tvec)
# bonetranslation(data, "hand_l", tvec)
def updateValueAPoseWorldForHands(data, dataxyz, data_apose, parent_name, current_name):
tvec0 = [dataxyz[parent_name][0], dataxyz[parent_name][1], dataxyz[parent_name][2]]

View File

@@ -70,9 +70,9 @@ class hpe_mp_class():
def show(self, image):
try:
if self.holistic_use:
mpDraw.draw_landmarks(
image,
self.results_hol.face_landmarks)
# mpDraw.draw_landmarks(
# image,
# self.results_hol.face_landmarks)
mpDraw.draw_landmarks(
image,
self.results_hol.pose_landmarks,
@@ -162,67 +162,67 @@ class hpe_mp_class():
def getJSON(self, apose=False, world=True, old_world=False):
data = {}
try:
if apose:
if world:
bodyaposeworld(data)
else:
bodyaposelocal(data)
# try:
if apose:
if world:
bodyaposeworld(data)
else:
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)
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)
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)
else:
bodyaposelocal(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
bodyaposelocal(data)
else:
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)
bodyconvertlocal(poseslms, data, self.coef, maxy)
except Exception as err:
logger.exception("Error json converting hpe class: " + str(err))
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)
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)
else:
bodyaposelocal(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
bodyconvert(poseslms, data, self.coef, maxy)
bodyconvertlocal(poseslms, data, self.coef, maxy)
# except Exception as err:
# logger.exception("Error json converting hpe class: " + str(err))
return data