def _global_rigid_transformation(self): results = {} pose = self.pose.reshape((-1, 3)) parent = { i: self.kintree_table[0, i] for i in range(1, self.kintree_table.shape[1]) } with_zeros = lambda x: ch.vstack((x, ch.array([[0.0, 0.0, 0.0, 1.0]]))) pack = lambda x: ch.hstack([ch.zeros((4, 3)), x.reshape((4, 1))]) results[0] = with_zeros( ch.hstack((Rodrigues(pose[0, :]), self.J[0, :].reshape((3, 1))))) for i in range(1, self.kintree_table.shape[1]): results[i] = results[parent[i]].dot( with_zeros( ch.hstack(( Rodrigues(pose[i, :]), # rotation around bone endpoint (self.J[i, :] - self.J[parent[i], :]).reshape( (3, 1)) # bone )))) results = [results[i] for i in sorted(results.keys())] results_global = results # subtract rotated J position results2 = [ results[i] - (pack(results[i].dot(ch.concatenate((self.J[i, :], [0]))))) for i in range(len(results)) ] result = ch.dstack(results2) return result, results_global
def model_params_in_camera_coords(trans, pose, J0, camera_t, camera_rt): root = Rodrigues(np.matmul(Rodrigues(camera_rt).r, Rodrigues(pose[:3]).r)).r.reshape(-1) pose[:3] = root trans = (Rodrigues(camera_rt).dot(J0 + trans) - J0 + camera_t).r return trans, pose