Exemplo n.º 1
0
def transform_pose(xyz,quat,F):
    x,y,z = xyz
    xp, yp, zp = F.eval(np.r_[x],np.r_[y],np.r_[z]).T
    jac = F.grad(np.r_[x],np.r_[y],np.r_[z])[0]
    old_rot_mat = conv.quat2mat(quat)
    new_rot_mat = np.dot(jac, old_rot_mat)
    new_rot_mat_orth = np.linalg.qr(new_rot_mat)[0]
    new_quat = conv.mat2quat(new_rot_mat_orth)
    return np.r_[xp,yp,zp], new_quat
Exemplo n.º 2
0
def transform_poses(pvec_n7, tps33):
  x_n, y_n, z_n = pvec_n7[:,:3].T
  quat_n4 = pvec_n7[:,3:7]
  xp_n, yp_n, zp_n = tps33.eval(x_n, y_n, z_n).T
  grad_n33 = tps33.grad(x_n, y_n, z_n)
  mats_n33 = [conv.quat2mat(quat) for quat in quat_n4]
  tmats_n33 = [np.dot(g,p) for (g,p) in zip(grad_n33, mats_n33)]
  tmats_n33 = [np.linalg.qr(mat)[0] for mat in tmats_n33]
  tquats_n4 = [conv.mat2quat(mat) for mat in tmats_n33]
  
  return np.c_[xp_n, yp_n, zp_n, tquats_n4]
Exemplo n.º 3
0
def transform_poses(xyzs,quats,F):
    x,y,z = xyzs.T
    xyzp = F.eval(x,y,z)
    jacs = F.grad(x,y,z)    
    
    new_quats = []
    for (quat,jac) in zip(quats,jacs):
        old_rot_mat = conv.quat2mat(quat)
        new_rot_mat = np.dot(jac, old_rot_mat)
        q,r = np.linalg.qr(new_rot_mat.T)
        new_rot_mat_orth = np.sign(np.diag(r))[:,None]* q.T
        new_quat = conv.mat2quat(new_rot_mat_orth)
        new_quats.append(new_quat)
    return xyzp, np.array(new_quats)