npy3d世界坐标转bvh

from . import math3d_SmartBody from . import bvh_helper_SmartBody as bvh_helper import numpy as np class SmartBodySkeleton(object): def __init__(self): # 定义bvh 节点名,和npy 对应idx # self.root = 'Hips' self.root = 'Pelvis' self.keypoint2index = { 'Pelvis': 0, 'Left_hip': 1, 'Right_hip':2, 'Spine1': 3, 'Left_knee': 4, 'Right_knee': 5, 'Spine2': 6, 'Left_ankle': 7, 'Right_ankle': 8, 'Spine3': 9, 'Left_foot': 10, 'Right_foot': 11, 'Neck': 12, 'Left_collar':13, 'Right_collar': 14, 'Head': 15, 'Left_shoulder': 16, 'Right_shoulder': 17, 'Left_elbow': 18, 'Right_elbow': 19, 'Left_wrist': 20, 'Right_wrist': 21, 'Left_palm': 22, 'Right_palm': 23, 'EndSite': -1, } # self.keypoint2index = { # 'Hips': 0, # 'LeftHipJoint': -1, # 'RightHipJoint':-1, # 'LowerBack': 3, # 'LeftUpLeg': 1, # 'RightUpLeg': 2, # 'Spine': 6, # 'LeftLeg': 4, # 'RightLeg': 5, # 'Spine1': 9, # 'LeftFoot': 7, # 'RightFoot': 8, # 'Neck': 12, # 'LeftShoulder':-1, # 'RightShoulder': -1, # 'Neck1': 15, # 'LeftArm': 16, # 'RightArm': 17, # 'LeftForeArm': 18, # 'RightForeArm': 19, # 'LeftHand': 20, # 'RightHand': 21, # 'LeftHandEndSite': -1, # 'RightHandEndSite': -1, # 'LeftFootEndSite': -1, # 'RightFootEndSite': -1, # 'HeadEndSite': -1, # } self.index2keypoint = {v: k for k, v in self.keypoint2index.items()} self.keypoint_num = len(self.keypoint2index) # 定义 bvh 节点关系 self.children = { 'Pelvis': ['Left_hip','Right_hip','Spine1'], 'Left_hip': ['Left_knee'], 'Left_knee': ['Left_ankle'], 'Left_ankle': ['Left_foot'], 'Left_foot': ['EndSite'], 'EndSite': [], 'Right_hip': ['Right_knee'], 'Right_knee': ['Right_ankle'], 'Right_ankle': ['Right_foot'], 'Right_foot': ['EndSite'], 'Spine1': ['Spine2'], 'Spine2': ['Spine3'], 'Spine3': ['Neck','Left_collar','Right_collar'], 'Neck': ['Head'], 'Head': ['EndSite'], 'Left_collar': ['Left_shoulder'], 'Left_shoulder': ['Left_elbow'], 'Left_elbow': ['Left_wrist'], 'Left_wrist': ['Left_palm'], 'Left_palm': ['EndSite'], 'Right_collar': ['Right_shoulder'], 'Right_shoulder': ['Right_elbow'], 'Right_elbow': ['Right_wrist'], 'Right_wrist': ['Right_palm'], 'Right_palm': ['EndSite'], } # self.children = { # 'Hips': ['LeftHipJoint', 'LowerBack', 'RightHipJoint'], # 'LeftHipJoint': ['LeftUpLeg'], # 'LeftUpLeg': ['LeftLeg'], # 'LeftLeg': ['LeftFoot'], # 'LeftFoot': ['LeftFootEndSite'], # 'LeftFootEndSite': [], # 'LowerBack': ['Spine'], # 'Spine': ['Spine1'], # 'Spine1': ['LeftShoulder', 'Neck', 'RightShoulder'], # 'LeftShoulder': ['LeftArm'], # 'LeftArm': ['LeftForeArm'], # 'LeftForeArm': ['LeftHand'], # 'LeftHand': ['LeftHandEndSite'], # 'LeftHandEndSite': [], # 'Neck': ['Neck1'], # 'Neck1': ['HeadEndSite'], # 'HeadEndSite': [], # 'RightShoulder': ['RightArm'], # 'RightArm': ['RightForeArm'], # 'RightForeArm': ['RightHand'], # 'RightHand': ['RightHandEndSite'], # 'RightHandEndSite': [], # 'RightHipJoint': ['RightUpLeg'], # 'RightUpLeg': ['RightLeg'], # 'RightLeg': ['RightFoot'], # 'RightFoot': ['RightFootEndSite'], # 'RightFootEndSite': [], # } self.parent = {self.root: None} for parent, children in self.children.items(): for child in children: self.parent[child] = parent self.left_joints = [ joint for joint in self.keypoint2index if 'Left' in joint ] self.right_joints = [ joint for joint in self.keypoint2index if 'Right' in joint ] # # self.initial_directions = { # 'Hips': [0, 0, 0], # 'LeftUpLeg': [1, -0.5, 0], # 'LeftLeg': [0, -1, 0], # 'LeftFoot': [0, -1, 0], # 'LeftFootEndSite': [0, 0, 1], # 'LowerBack': [0, 1, 0], # 'Spine': [0, 1, 0], # 'Spine1': [0, 1, 0], # 'RightShoulder': [-1, 0, 0], # 'LeftShoulder': [1, 0, 0], # 'LeftArm': [1, 0.5, 0], # 'LeftHand': [1, 0, 0], # 'LeftHandEndSite': [1, 0, 0], # 'Neck': [0, 1, 0], # 'Neck1': [0, 1, 0], # 'HeadEndSite': [0, 1, 0], # 'RightArm': [-1, 0.5, 0], # 'RightHand': [-1, 0, 0], # 'RightHandEndSite': [-1, 0, 0], # 'RightUpLeg': [-1, -0.5, 0], # 'RightLeg': [0, -1, 0], # 'RightFoot': [0, -1, 0], # 'RightFootEndSite': [0, 0, 1], # 'LeftForeArm': [1, 0, 0], # 'RightForeArm': [-1, 0, 0], # 'RightHipJoint': [-1, 0, 0], # 'LeftHipJoint': [1, 0, 0], # } # SmartBody坐标系(Y向上，Z向前，X向右)下的T-pose,定义节点方向 self.initial_directions = { 'Pelvis': [0, 0, 0], 'Left_hip': [1, -0.9, 0], 'Right_hip': [-1, -0.9, 0], 'Spine1': [0, 1, 0], 'Left_knee': [0, -1, 0], 'Right_knee': [0, -1, 0], 'Spine2': [0, 1, 0], 'Left_ankle': [0, -1, 0], 'Right_ankle': [0, -1, 0], 'Spine3': [0, 1, 0], 'Left_foot': [0, -0.5, 1], 'Right_foot': [0, -0.5, 1], 'Neck': [0, 1, 0], 'Left_collar': [1, 0.9, 0], 'Right_collar': [-1, 0.9, 0], 'Head': [0, 1, 0], 'Left_shoulder': [1, 0, 0], 'Right_shoulder': [-1, 0, 0], 'Left_elbow': [1, 0, 0], 'Right_elbow': [-1, 0, 0], 'Left_wrist': [1, 0, 0], 'Right_wrist': [-1, 0, 0], 'Left_palm': [1, 0, 0], 'Right_palm': [-1, 0, 0], 'EndSite': [0, 0, 1], } def get_initial_offset(self, poses_3d): # TODO: RANSAC # 计算父节点-子节点 欧氏距离 bone_lens = {self.root: [0]} stack = [self.root] while stack: parent = stack.pop() p_idx = self.keypoint2index[parent] p_name = parent while p_idx == -1: # find real parent p_name = self.parent[p_name] p_idx = self.keypoint2index[p_name] for child in self.children[parent]: stack.append(child) if self.keypoint2index[child] == -1: bone_lens[child] = [0.1] else: c_idx = self.keypoint2index[child] bone_lens[child] = np.linalg.norm( poses_3d[:, p_idx] - poses_3d[:, c_idx], axis=1 ) # 吧所有动作 取平均距离、左右距离相同 bone_len = {} for joint in self.keypoint2index