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
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]
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)
def get_chessboard_pose(im,shape_cb,size,cam_matrix):
width_cb,height_cb = shape_cb
xs,ys = np.meshgrid(range(0,width_cb),range(0,height_cb))
xys = size*np.c_[xs.flat,ys.flat,np.zeros(xs.size)]
print xys
height_im,width_im = im.shape[:2]
cx = width_im/2-.5
cy = height_im/2-.5
dist_coeffs = np.zeros(5)
corners = cv2.findChessboardCorners(im,(width_cb,height_cb))[1]
if corners is None:
raise ValueError
else:
rod,trans = cv2.solvePnP(np.array(xys),np.array(corners),cam_matrix,dist_coeffs)
quat = conversions.mat2quat(conversions.rod2mat(rod))
return trans,quat,corners
