def test_pose_utils():
"""
Tests the pose utils
:return:
"""
TEST_COMPOSE = True
TEST_INV = True
def ra(_): return np.random.uniform(0, 2 * math.pi)
if TEST_COMPOSE:
print('Testing pose composing...')
R1 = txe.euler2mat(ra(1), ra(1), ra(1))
t1 = np.random.rand(3)
R2 = txe.euler2mat(ra(1), ra(1), ra(1))
t2 = np.random.rand(3)
# homogeneous matrix method
R = np.dot(R1, R2)
t = t1 + np.dot(R1, t2)
print('From homogeneous matrices, t = ')
print(t)
print('R = ')
print(R)
# quaternion method
q1 = txq.mat2quat(R1)
q2 = txq.mat2quat(R2)
p1 = torch.cat((torch.from_numpy(t1), torch.from_numpy(q1)))
p2 = torch.cat((torch.from_numpy(t2), torch.from_numpy(q2)))
p = compose_pose_quaternion(
torch.unsqueeze(
p1, 0), torch.unsqueeze(
p2, 0))
t = p[:, :3].numpy().squeeze()
q = p[:, 3:].numpy().squeeze()
print('From quaternions, t = ')
print(t)
print('R = ')
print(txe.quat2mat(q))
if TEST_INV:
print('Testing pose inversion...')
R = txe.euler2mat(ra(1), ra(1), ra(1))
t = np.random.rand(3)
T = np.eye(4)
T[:3, :3] = R
T[:3, -1] = t
q = txq.mat2quat(R)
p = torch.cat((torch.from_numpy(t), torch.from_numpy(q)))
pinv = invert_pose_quaternion(torch.unsqueeze(p, 0))
tinv, qinv = pinv[:, :3], pinv[:, 3:]
Rinv = txq.quat2mat(qinv.numpy().squeeze())
Tinv = np.eye(4)
Tinv[:3, :3] = Rinv
Tinv[:3, -1] = tinv.numpy().squeeze()
print('T * T^(-1) = ')
print(np.dot(T, Tinv))
def allocentric_to_egocentric(allo_pose, src_type="mat", dst_type="mat"):
"""Given an allocentric (object-centric) pose, compute new camera-centric
pose Since we do detection on the image plane and our kernels are
2D-translationally invariant, we need to ensure that rendered objects
always look identical, independent of where we render them.
Since objects further away from the optical center undergo skewing,
we try to visually correct by rotating back the amount between
optical center ray and object centroid ray. Another way to solve
that might be translational variance
(https://arxiv.org/abs/1807.03247)
"""
# Compute rotation between ray to object centroid and optical center ray
cam_ray = np.asarray([0, 0, 1.0])
if src_type == "mat":
trans = allo_pose[:3, 3]
elif src_type == "quat":
trans = allo_pose[4:7]
else:
raise ValueError(
"src_type should be mat or quat, got: {}".format(src_type))
obj_ray = trans.copy() / np.linalg.norm(trans)
angle = math.acos(cam_ray.dot(obj_ray))
# Rotate back by that amount
if angle > 0:
if dst_type == "mat":
ego_pose = np.zeros((3, 4), dtype=allo_pose.dtype)
ego_pose[:3, 3] = trans
rot_mat = axangle2mat(axis=np.cross(cam_ray, obj_ray), angle=angle)
if src_type == "mat":
ego_pose[:3, :3] = np.dot(rot_mat, allo_pose[:3, :3])
elif src_type == "quat":
ego_pose[:3, :3] = np.dot(rot_mat, quat2mat(allo_pose[:4]))
elif dst_type == "quat":
ego_pose = np.zeros((7, ), dtype=allo_pose.dtype)
ego_pose[4:7] = trans
rot_q = axangle2quat(np.cross(cam_ray, obj_ray), angle)
if src_type == "quat":
ego_pose[:4] = qmult(rot_q, allo_pose[:4])
elif src_type == "mat":
ego_pose[:4] = qmult(rot_q, mat2quat(allo_pose[:3, :3]))
else:
raise ValueError(
"dst_type should be mat or quat, got: {}".format(dst_type))
else: # allo to ego
if src_type == "mat" and dst_type == "quat":
ego_pose = np.zeros((7, ), dtype=allo_pose.dtype)
ego_pose[:4] = mat2quat(allo_pose[:3, :3])
ego_pose[4:7] = allo_pose[:3, 3]
elif src_type == "quat" and dst_type == "mat":
ego_pose = np.zeros((3, 4), dtype=allo_pose.dtype)
ego_pose[:3, :3] = quat2mat(allo_pose[:4])
ego_pose[:3, 3] = allo_pose[4:7]
else:
ego_pose = allo_pose.copy()
return ego_pose
def test_q_error():
ra = lambda _: np.random.uniform(0, 2 * math.pi)
# rotation along x axis
a1 = ra(1)
a2 = ra(1)
q1 = txq.mat2quat(txe.euler2mat(a1, 0, 0))
q2 = txq.mat2quat(txe.euler2mat(a2, 0, 0))
a1 = np.rad2deg(a1)
a2 = np.rad2deg(a2)
print('Angles: {:f}, {:f}, difference = {:f}'.format(a1, a2, a1 - a2))
print('Error: {:f}'.format(quaternion_angular_error(q1, q2)))
def test_log_q_error():
ra = lambda _: np.random.uniform(0, 2 * math.pi)
# rotation along x axis
a1 = ra(1)
a2 = ra(1)
q1 = txq.mat2quat(txe.euler2mat(0, a1, 0))
q2 = txq.mat2quat(txe.euler2mat(0, a2, 0))
# apply log map
q1 = np.arccos(q1[0]) * q1[1:] / np.linalg.norm(q1[1:])
q2 = np.arccos(q2[0]) * q2[1:] / np.linalg.norm(q2[1:])
a1 = np.rad2deg(a1)
a2 = np.rad2deg(a2)
print('Angles: {:f}, {:f}, difference = {:f}'.format(a1, a2, a1 - a2))
print('Error: {:f}'.format(log_quaternion_angular_error(q1, q2)))
def egocentric_to_allocentric(ego_pose,
src_type="mat",
dst_type="mat",
cam_ray=(0, 0, 1.0)):
# Compute rotation between ray to object centroid and optical center ray
cam_ray = np.asarray(cam_ray)
if src_type == "mat":
trans = ego_pose[:3, 3]
elif src_type == "quat":
trans = ego_pose[4:7]
else:
raise ValueError(
"src_type should be mat or quat, got: {}".format(src_type))
obj_ray = trans.copy() / np.linalg.norm(trans)
angle = math.acos(cam_ray.dot(obj_ray))
# Rotate back by that amount
if angle > 0:
if dst_type == "mat":
allo_pose = np.zeros((3, 4), dtype=ego_pose.dtype)
allo_pose[:3, 3] = trans
rot_mat = axangle2mat(axis=np.cross(cam_ray, obj_ray),
angle=-angle)
if src_type == "mat":
allo_pose[:3, :3] = np.dot(rot_mat, ego_pose[:3, :3])
elif src_type == "quat":
allo_pose[:3, :3] = np.dot(rot_mat, quat2mat(ego_pose[:4]))
elif dst_type == "quat":
allo_pose = np.zeros((7, ), dtype=ego_pose.dtype)
allo_pose[4:7] = trans
rot_q = axangle2quat(np.cross(cam_ray, obj_ray), -angle)
if src_type == "quat":
allo_pose[:4] = qmult(rot_q, ego_pose[:4])
elif src_type == "mat":
allo_pose[:4] = qmult(rot_q, mat2quat(ego_pose[:3, :3]))
else:
raise ValueError(
"dst_type should be mat or quat, got: {}".format(dst_type))
else:
if src_type == "mat" and dst_type == "quat":
allo_pose = np.zeros((7, ), dtype=ego_pose.dtype)
allo_pose[:4] = mat2quat(ego_pose[:3, :3])
allo_pose[4:7] = ego_pose[:3, 3]
elif src_type == "quat" and dst_type == "mat":
allo_pose = np.zeros((3, 4), dtype=ego_pose.dtype)
allo_pose[:3, :3] = quat2mat(ego_pose[:4])
allo_pose[:3, 3] = ego_pose[4:7]
else:
allo_pose = ego_pose.copy()
return allo_pose
def parse_rots_by_len(DATA_ROOT, length):
all_quats = np.zeros(shape=(length, 4), dtype=np.float32)
all_rots = np.zeros(shape=(length, 3, 3), dtype=np.float32)
all_axang = np.zeros(shape=(length, 3))
print(all_rots.shape)
for i in range(length):
#file_name ='./driller/data/rot' + str(i) + '.rot'
#file_name = '\\driller\\data\\rot' + str(i) + '.rot'
p0 = os.path.abspath(DATA_ROOT)
file_name = os.path.join(p0, 'driller', 'driller', 'data',
'rot' + str(i) + '.rot')
# print(file_name)
p = Path(file_name)
with p.open() as f:
j = 0
for line in f:
line = line.split()
if len(line) == 2:
continue
else:
all_rots[i, j, :] = line
j = j + 1
all_quats[i, :] = mat2quat(all_rots[i, :, :])
direc, angle = mat2axangle(all_rots[i, :, :])
all_axang[i, :] = direc * angle
return all_rots, all_quats, all_axang
def write_asol(self, photons, asolfile, timestep=0.256):
time = np.arange(0, photons.meta['EXPOSURE'][0], timestep)
pointing = np.rad2deg(self.pointing(time))
asol = Table([time, pointing[:, 0], pointing[:, 1], pointing[:, 2]],
names=['time', 'ra', 'dec', 'roll'],
)
asol['time'].unit = 's'
# The following columns represent measured offsets in Chandra
# They are not part of this simulation. Simply set them to 0
for col in [ 'ra_err', 'dec_err', 'roll_err',
'dy', 'dz', 'dtheta', 'dy_err', 'dz_err', 'dtheta_err',
'roll_bias', 'pitch_bias', 'yaw_bias', 'roll_bias_err', 'pitch_bias_err', 'yaw_bias_err']:
asol[col] = np.zeros_like(time)
if 'bias' in col:
asol[col].unit = 'deg / s'
elif ('dy' in col) or ('dz' in col):
asol[col].unit = 'mm'
else:
asol[col].unit = 'deg'
asol['q_att'] = [mat2quat(euler2mat(np.deg2rad(p[0]),
np.deg2rad(-p[1]),
np.deg2rad(-p[2]), 'rzyx'))
for p in pointing]
# Copy info like the exposure time from the photons list meta to asol,
# but not column specific keywords like TTYPEn, TCTYPn, MTYPEn, MFORMn, etc:
for k in photons.meta:
if (('TYP' not in k) and ('FORM' not in k) and
('TCR' not in k) and ('TCDEL' not in k) and (k not in asol.meta)):
asol.meta[k] = photons.meta[k]
asol.meta['EXTNAME'] = 'ASPECT'
complete_header(asol.meta, None, 'ACASOL', ['OGIP', 'TEMPORALDATA', 'ASPECT'])
# In MARXS t=0 is the start of the observation, but for Chandra we need to make that
# consistent with the value of the TSTART keyword.
asol['time'] += asol.meta['TSTART'][0]
asol.write(asolfile, format='fits') # , checksum=True) - works only with fits interface
def run_grasp(self, grasp_list, num, use_gdn=True):
if len(grasp_list) == 0:
print('No any grasp detected!')
grasp_iter = min(num, len(grasp_list))
for i in range(grasp_iter):
if use_gdn:
grasp_matrix = np.dot(grasp_list[i], self.original_matrix_gdn)
else:
grasp_matrix = np.dot(grasp_list[i], self.original_matrix)
#visualize the grasping pose
rot_matrix = grasp_matrix[:3, :3]
quat = mat2quat(rot_matrix)
quat = np.array([quat[1], quat[2], quat[3], quat[0]])
self.set_object_quat('staples_pose', quat)
self.set_object_position('staples_pose', grasp_matrix[:3, 3])
send_matrix = list(grasp_matrix.flatten())[0:12]
res, retInts, path, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI', vrep.sim_scripttype_childscript,
'GraspMovement', [], send_matrix, [], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
running = True
while running:
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
self.clientID, 'RemoteAPI',
vrep.sim_scripttype_childscript, 'isRunning', [], [],
['UR5_GraspMatrix'], self.emptyBuff,
vrep.simx_opmode_oneshot_wait)
if retInts[0] == 0:
running = False
def test_quaternion_reconstruction():
# Test reconstruction of arbitrary unit quaternions
for q in unit_quats:
M = tq.quat2mat(q)
qt = tq.mat2quat(M)
# Accept positive or negative match
posm = np.allclose(q, qt)
negm = np.allclose(q, -qt)
assert posm or negm
# Test vectorized operations
Q = np.array(list(unit_quats))
M = tq.quat2mat(Q)
QT = tq.mat2quat(M)
posm = np.isclose(Q, QT)
negm = np.isclose(Q, -QT)
assert np.logical_or(posm, negm).all()
def writexml(self, xmlfilename='scene.xml'):
if self.topfromreader is not None:
self.top = self.topfromreader
else:
self.top = Element('scene')
for i in range(len(self.poselist)):
obj_entry = SubElement(self.top, 'obj')
obj_name = SubElement(obj_entry, 'obj_id')
obj_name.text = str(self.objidlist[i])
obj_name = SubElement(obj_entry, 'obj_name')
obj_name.text = self.objnamelist[i]
obj_path = SubElement(obj_entry, 'obj_path')
obj_path.text = self.objpathlist[i]
pose = self.poselist[i]
pose_in_world = SubElement(obj_entry, 'pos_in_world')
pose_in_world.text = '{:.4f} {:.4f} {:.4f}'.format(
pose[0, 3], pose[1, 3], pose[2, 3])
rotationMatrix = pose[0:3, 0:3]
quat = mat2quat(rotationMatrix)
ori_in_world = SubElement(obj_entry, 'ori_in_world')
ori_in_world.text = '{:.4f} {:.4f} {:.4f} {:.4f}'.format(
quat[0], quat[1], quat[2], quat[3])
xmlstr = xml.dom.minidom.parseString(tostring(
self.top)).toprettyxml(indent=' ')
# remove blank line
xmlstr = "".join([s for s in xmlstr.splitlines(True) if s.strip()])
with open(xmlfilename, 'w') as f:
f.write(xmlstr)
def to_quatpose_mtx(se3_mtx):
trans = se3_mtx[:3, 3]
rot = mat2quat(se3_mtx[:3, :3])
pose = np.zeros(7)
pose[0:3] = trans
pose[3:7] = rot
return pose
def get_closest_pose_batch(poses_est, poses_gt, sym_infos):
"""
get closest poses_gt according to current predicted poses_est and sym_infos
--------------------
poses_est: [B, 8]
poses_gt: [B, 8]
sym_infos: dict {label_idx: Kx3x3 or None}, stores K rotations regarding symmetries, if not symmetric, None
-----
closest_poses_gt: [B, 8]
"""
batch_size = poses_est.shape[0]
device = poses_est.device
rots_est = quat2mat_torch(poses_est[:, :4])
rots_gt = quat2mat_torch(poses_gt[:, :4])
labels = poses_est[:, 7].long()
closest_poses_gt = poses_gt.clone().cpu().numpy()
for i in range(batch_size):
closest_rot = get_closest_rot(rots_est[i].detach().cpu().numpy(), rots_gt[i].cpu().numpy(),
sym_infos[int(labels[i])])
# TODO: automatically detect rot_gt's format in PM_Loss to avoid converting multiple times
closest_poses_gt[i][:4] = mat2quat(closest_rot)
closest_poses_gt = torch.tensor(closest_poses_gt, device=device, dtype=poses_gt.dtype)
return closest_poses_gt
def convert_xyzquat(dataset_dir, split_file, write_file):
with open(split_file, 'r') as f:
seqs = [
int(l.split('sequence')[-1]) for l in f if not l.startswith('#')
]
with open(write_file, 'w') as f:
f.write(
'7 Scenes Datasets (convert rotation matrix to translation + quaternion)\n'
)
f.write('Image File, Camera Position [X Y Z W P Q R]\n')
f.write('\n')
for seq in seqs:
seq_dir = 'seq-{:02d}'.format(seq)
p_filenames = get_pose_filenames(dataset_dir, seq_dir)
assert p_filenames, 'no poses in directory {}'.format(seq_dir)
ss = p_filenames[0].find(seq_dir)
se = p_filenames[0].find('.pose')
pose_out = np.zeros(7)
for i in range(len(p_filenames)):
pose_in = np.loadtxt(p_filenames[i])
pose_in = np.asarray(pose_in)
pose_out[3:] = txq.mat2quat(pose_in[:3, :3])
pose_out[0:3] = pose_in[:, 3].flatten()[:3]
pose_str = p_filenames[i][ss:se] + '.color.png'
for i in range(7):
pose_str += ' {:0.8f}'.format(pose_out[i])
f.write(pose_str + '\n')
def ori_transform(frame_ori, relative_ori):
frame_matrix = quat2mat(frame_ori[[3, 0, 1, 2]])
relative_matrix = quat2mat(relative_ori[[3, 0, 1, 2]])
new_ori_matrix = frame_matrix.dot(relative_matrix)
return mat2quat(new_ori_matrix)[[1, 2, 3, 0]]
def get_quat_translation(self, object_to_camera_pose):
object_to_camera_pose = np.append(object_to_camera_pose, [[0, 0, 0, 1]], axis=0)
world_to_camera_pose = np.append(self.world_to_camera_pose, [[0, 0, 0, 1]], axis=0)
object_to_world_pose = np.dot(np.linalg.inv(world_to_camera_pose), object_to_camera_pose)
quat = mat2quat(object_to_world_pose[:3, :3])
translation = object_to_world_pose[:3, 3]
return quat, translation
def icp_refine_(self, pose, anno, output):
depth = read_depth(anno['depth_path']).astype(np.uint16)
mask = torch.argmax(output['seg'], dim=1)[0].detach().cpu().numpy()
mask = mask.astype(np.int32)
pose = pose.astype(np.float32)
poses = np.zeros([1, 7], dtype=np.float32)
poses[0, :4] = mat2quat(pose[:, :3])
poses[0, 4:] = pose[:, 3]
poses_new = np.zeros([1, 7], dtype=np.float32)
poses_icp = np.zeros([1, 7], dtype=np.float32)
fx = 572.41140
fy = 573.57043
px = 325.26110
py = 242.04899
zfar = 6.0
znear = 0.25
factor = 1000.0
error_threshold = 0.01
rois = np.zeros([1, 6], dtype=np.float32)
rois[:, :] = 1
self.icp_refiner.solveICP(mask, depth, self.height, self.width, fx, fy,
px, py, znear, zfar, factor, rois.shape[0],
rois, poses, poses_new, poses_icp,
error_threshold)
pose_icp = np.zeros([3, 4], dtype=np.float32)
pose_icp[:, :3] = quat2mat(poses_icp[0, :4])
pose_icp[:, 3] = poses_icp[0, 4:]
return pose_icp
def write_asol(self, photons, asolfile, timestep=0.256):
'''Write an aspect solution (asol) file
Chandra analysis scripts often require an aspect solution,
which is essientially a list of pointing directions in the
dither pattern vs. time. This method write such a list to a
file.
Parameters
----------
photons : `astropy.table.Table` or `astropy.table.Row`
Table with photon properties. Some meta data from the header of
this table is required (e.g. the length of the observation).
asolfile : string
Path and file name where the asol file is saved.
timestamp : float
Time step between entries in the asol file in seconds.
'''
time = np.arange(0, photons.meta['EXPOSURE'][0], timestep)
pointing = self.pointing(time).to(u.deg).value
asol = Table(
[time, pointing[:, 0], pointing[:, 1], pointing[:, 2]],
names=['time', 'ra', 'dec', 'roll'],
)
asol['time'].unit = 's'
# The following columns represent measured offsets in Chandra
# They are not part of this simulation. Simply set them to 0
for col in [
'ra_err', 'dec_err', 'roll_err', 'dy', 'dz', 'dtheta',
'dy_err', 'dz_err', 'dtheta_err', 'roll_bias', 'pitch_bias',
'yaw_bias', 'roll_bias_err', 'pitch_bias_err', 'yaw_bias_err'
]:
asol[col] = np.zeros_like(time)
if 'bias' in col:
asol[col].unit = 'deg / s'
elif ('dy' in col) or ('dz' in col):
asol[col].unit = 'mm'
else:
asol[col].unit = 'deg'
asol['q_att'] = [
mat2quat(
euler2mat(np.deg2rad(p[0]), np.deg2rad(-p[1]),
np.deg2rad(-p[2]), 'rzyx')) for p in pointing
]
# Copy info like the exposure time from the photons list meta to asol,
# but not column specific keywords like TTYPEn, TCTYPn, MTYPEn, MFORMn, etc:
for k in photons.meta:
if (('TYP' not in k) and ('FORM' not in k) and ('TCR' not in k)
and ('TCDEL' not in k) and (k not in asol.meta)):
asol.meta[k] = photons.meta[k]
asol.meta['EXTNAME'] = 'ASPECT'
complete_header(asol.meta, None, 'ACASOL',
['OGIP', 'TEMPORALDATA', 'ASPECT'])
# In MARXS t=0 is the start of the observation, but for Chandra we need to make that
# consistent with the value of the TSTART keyword.
asol['time'] += asol.meta['TSTART'][0]
asol.write(
asolfile,
format='fits') # , checksum=True) - works only with fits interface
def extract_x_y_theta(self,
object_info,
t_worldaug_world=None,
preserve_theta=False):
"""Extract in-plane theta."""
object_position = object_info[0]
object_quat_xyzw = object_info[1]
if t_worldaug_world is not None:
object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0],
object_quat_xyzw[1], object_quat_xyzw[2])
t_world_object = quaternions.quat2mat(object_quat_wxyz)
t_world_object[0:3, 3] = np.array(object_position)
t_worldaug_object = t_worldaug_world @ t_world_object
object_quat_wxyz = quaternions.mat2quat(t_worldaug_object)
if not preserve_theta:
object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2],
object_quat_wxyz[3], object_quat_wxyz[0])
object_position = t_worldaug_object[0:3, 3]
object_xy = object_position[0:2]
object_theta = -np.float32(
utils.quatXYZW_to_eulerXYZ(object_quat_xyzw)
[2]) / self.theta_scale
return np.hstack(
(object_xy,
object_theta)).astype(np.float32), object_position, object_quat_xyzw
def __init__(self, rootAABB, aligned, A_IK, leafs, aabbTree):
self.aabb = rootAABB;
self.aligned = aligned;
self.leafs = leafs;
self.aabbTree = aabbTree;
self.A_IK = A_IK;
self.q_KI = mat2quat(A_IK); # A_IK = R_KI
def process_poses_quaternion(xyz_in, q_in, mean_t, std_t, align_R, align_t, align_s):
"""
processes the 1x12 raw pose from dataset by aligning and then normalizing
:param poses_in: N x 12
:param mean_t: 3
:param std_t: 3
:param align_R: 3 x 3
:param align_t: 3
:param align_s: 1
:return: processed poses (translation + quaternion) N x 7
"""
poses_out = np.zeros((len(xyz_in), 6))
poses_out[:, 0:3] = xyz_in
align_q = txq.mat2quat(align_R)
# align
for i in range(len(poses_out)):
q = txq.qmult(align_q, q_in[i])
q *= np.sign(q[0]) # constrain to hemisphere
q = qlog(q)
poses_out[i, 3:] = q
t = poses_out[i, :3] - align_t
poses_out[i, :3] = align_s * \
np.dot(align_R, t[:, np.newaxis]).squeeze()
# normalize translation
poses_out[:, :3] -= mean_t
poses_out[:, :3] /= std_t
return poses_out
def extract_x_y_theta(self,
object_info,
t_worldaug_world=None,
preserve_theta=False):
"""Extract in-plane theta."""
object_position = object_info[0]
object_quat_xyzw = object_info[1]
if t_worldaug_world is not None:
object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0],
object_quat_xyzw[1], object_quat_xyzw[2])
t_world_object = quaternions.quat2mat(object_quat_wxyz)
#t_world_object[0:3, 3] = np.array(object_position)
p0_position_2d = np.reshape(object_position, (3, 1))
t_world_object = np.append(t_world_object, p0_position_2d, axis=1)
arr = np.transpose(np.array([[0], [0], [0], [1]]))
t_world_object = np.append(t_world_object, arr, axis=0)
t_worldaug_object = t_worldaug_world @ t_world_object
t_worldaug_object = t_worldaug_object[0:3, 0:3]
object_quat_wxyz = quaternions.mat2quat(t_worldaug_object)
if not preserve_theta:
object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2],
object_quat_wxyz[3], object_quat_wxyz[0])
t_worldaug_object_two = t_worldaug_world @ t_world_object
object_position = t_worldaug_object_two[0:3, 3]
object_xy = object_position[0:2]
object_theta = -np.float32(
utils.quatXYZW_to_eulerXYZ(object_quat_xyzw)[2]) / self.theta_scale
return np.hstack((object_xy, object_theta)).astype(
np.float32), object_position, object_quat_xyzw
def prosess_poses(poses_in, mean_t, std_t, align_R, align_t, align_s):
"""
processes the 1x12 raw pose from dataset by aligning and then normalizing
:param poses_in: N x 12
:param mean_t: 3
:param std_t: 3
:param align_R: 3 x 3
:param align_t: 3
:param align_s: 1
:return: processed poses (translation + quaternion) N x 7
"""
poses_out = np.zeros((len(poses_in), 6))
poses_out[:, 0:3] = poses_in[:, [3, 7, 11]]
# align
for i in range(len(poses_out)):
R = poses_in[i].reshape((3, 4))[:3, :3]
q = txq.mat2quat(np.dot(align_R, R))
q *= np.sign(q[0]) # 限制在半球上
q = qlog(q)
poses_out[i, 3:] = q
t = poses_out[i, :3] - align_t
poses_out[i, :3] = align_s * np.dot(align_R, t[:, np.newaxis]).squeeze(
) # squeeze()从数组的形状中删除单维度条目，即把shape中为1的维度去掉
# normalize translation
poses_out[:, :3] -= mean_t
poses_out[:, :3] /= std_t
return poses_out
def rotate_axangle(Axangles, new_center):
"""
Rotates a series of orientation described by axangle to a new center
Parameters
----------
Axangles : diffsims.Axangles
Pre-rotation
new_center : (alpha,beta,gamma)
The location of the (0,0,0) rotation as an rzxz euler angle
Returns
-------
AxAngles : diffsims.Axangles
Rotated
See Also
--------
generators.get_local_grid
"""
quats = Axangles.to_Quat()
q = mat2quat(rotation_matrix_from_euler_angles((new_center)))
stored_quats = vectorised_qmult(q, quats)
return AxAngle.from_Quat(stored_quats)
def RT2QT(poses_in, mean_t, std_t):
"""
processes the 1x12 raw pose from dataset by aligning and then normalizing
:param poses_in: N x 12
:param mean_t: 3
:param std_t: 3
:return: processed poses (translation + quaternion) N x 7
"""
poses_out = np.zeros((len(poses_in), 7))
poses_out[:, 0:3] = poses_in[:, [3, 7, 11]]
# align
for i in range(len(poses_out)):
R = poses_in[i].reshape((3, 4))[:3, :3]
# q = txq.mat2quat(np.dot(align_R, R))
q = txq.mat2quat(R)
q = q / (np.linalg.norm(q) + 1e-12) # normalize
q *= np.sign(q[0]) # constrain to hemisphere
# q = qlog(q)
poses_out[i, 3:] = q
# t = poses_out[i, :3] - align_t
# poses_out[i, :3] = align_s * np.dot(align_R, t[:, np.newaxis]).squeeze()
# normalize translation
poses_out[:, :3] -= mean_t
poses_out[:, :3] /= std_t
return poses_out
def rotate_pose(point, quat, theta, axis="x"):
# set quaternion to transforms3d format, i.e. w,x,y,z
quat_t3d = [quat[3], quat[0], quat[1], quat[2]]
rot_mat = quaternions.quat2mat(quat_t3d)
H1 = np.eye(4)
H1[:3,:3] = rot_mat
H1[:3,3] = point
H2 = np.eye(4)
if axis == "x":
rot_axis = np.array(eulerangles.x_rotation(theta))
elif axis == "y":
rot_axis = np.array(eulerangles.y_rotation(theta))
elif axis == "z":
rot_axis = np.array(eulerangles.z_rotation(theta))
print rot_axis H2[:3,:3] = rot_axis
H3 = np.dot(H1, H2)
point_out = H3[:3,3]
quat_t3dout = quaternions.mat2quat(H3[:3,:3])
# Turn to ROS format, i.e. x, y, z, w
quat_out = [quat_t3dout[1], quat_t3dout[2], quat_t3dout[3], quat_t3do
ut[0]]
rospy.loginfo("Rotated pose: ")
print point_out, quat_out
return point_out, quat_out
def get_six_dof_object(self, object_info, t_worldaug_world=None):
"""Calculate the pose of 6DOF object."""
object_position = object_info[0]
object_quat_xyzw = object_info[1]
if t_worldaug_world is not None:
object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0],
object_quat_xyzw[1], object_quat_xyzw[2])
t_world_object = quaternions.quat2mat(object_quat_wxyz)
t_world_object[0:3, 3] = np.array(object_position)
t_worldaug_object = t_worldaug_world @ t_world_object
object_quat_wxyz = quaternions.mat2quat(
t_worldaug_object)
object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2],
object_quat_wxyz[3], object_quat_wxyz[0])
object_position = t_worldaug_object[0:3, 3]
euler = utils.quatXYZW_to_eulerXYZ(object_quat_xyzw)
roll = euler[0]
pitch = euler[1]
theta = -euler[2]
return np.asarray([
object_position[0], object_position[1], object_position[2], roll, pitch,
theta
])
def pose_transform(frame_pose, relative_pose):
"""
pose is pos + quat: (x, y, z, w)
Args:
frame_pose:
relative_pose:
Returns:
"""
frame_pose = np.array(frame_pose)
relative_pose = np.array(relative_pose)
frame_matrix = np.eye(4)
frame_matrix[:3, :3] = quat2mat(frame_pose[[6, 3, 4, 5]])
frame_matrix[:3, -1] = frame_pose[:3]
relative_matrix = np.eye(4)
relative_matrix[:3, :3] = quat2mat(relative_pose[[6, 3, 4, 5]])
relative_matrix[:3, -1] = relative_pose[:3]
new_pos_matrix = frame_matrix.dot(relative_matrix)
pose = np.zeros(7)
pose[:3] = new_pos_matrix[:3, -1]
pose[3:] = mat2quat(new_pos_matrix[:3, :3])[[1, 2, 3, 0]]
return pose
def get_instance_id(gt_poses, pose_target):
gt_poses_quats = []
for i in range(gt_poses.shape[2]):
gt_orientation = Quaternion(mat2quat(gt_poses[:, :3, i]))
gt_poses_quats.append(gt_orientation)
if np.allclose(gt_orientation.elements, pose_target.elements, 1e-5):
return i, gt_orientation
print(gt_poses_quats, pose_target)
def safemat2quat(mat):
quat = np.array([1, 0, 0, 0])
try:
quat = mat2quat(mat)
except:
pass
quat[np.isnan(quat)] = 0
return quat
def get_pos_quat(
self,
device: VrDevice,
raw: bool = False,
) -> Tuple[np.ndarray, np.ndarray]:
"""Returns the translation quaternion rotation of the given device."""
pos, rot = self.get_pos_rot(device, raw)
return pos, mat2quat(rot).astype(np.float32)
def transform_msg(R, t, stamp, frame, child_frame):
"""Create TF message from rotation matrxi and translation vector."""
# Convert rotation matrix to 'xyzw' quaternion.
q = mat2quat(R)[[1, 2, 3, 0]] # to
# Compose and return ROS message.
msg = TransformStamped(Header(None, stamp, frame), child_frame,
Transform(Vector3(*t.ravel()), Quaternion(*q)))
return msg
def test_quaternion_reconstruction():
# Test reconstruction of arbitrary unit quaternions
for q in unit_quats:
M = tq.quat2mat(q)
qt = tq.mat2quat(M)
# Accept positive or negative match
posm = np.allclose(q, qt)
negm = np.allclose(q, -qt)
assert posm or negm
def write_asol(self, photons, asolfile, timestep=0.256):
'''Write an aspect solution (asol) file
Chandra analysis scripts often require an aspect solution, which is essientiall
a list of pointing directions in the dither pattern vs. time.
This method write such a list to a file.
Parameters
----------
photons : `astropy.table.Table` or `astropy.table.Row`
Table with photon properties. Some meta data from the header of this table
is required (e.g. the length of the observation).
asolfile : string
Path and file name where the asol file is saved.
timestamp : float
Time step between entries in the asol file in seconds.
'''
time = np.arange(0, photons.meta['EXPOSURE'][0], timestep)
pointing = self.pointing(time).to(u.deg).value
asol = Table([time, pointing[:, 0], pointing[:, 1], pointing[:, 2]],
names=['time', 'ra', 'dec', 'roll'],
)
asol['time'].unit = 's'
# The following columns represent measured offsets in Chandra
# They are not part of this simulation. Simply set them to 0
for col in [ 'ra_err', 'dec_err', 'roll_err',
'dy', 'dz', 'dtheta', 'dy_err', 'dz_err', 'dtheta_err',
'roll_bias', 'pitch_bias', 'yaw_bias', 'roll_bias_err', 'pitch_bias_err', 'yaw_bias_err']:
asol[col] = np.zeros_like(time)
if 'bias' in col:
asol[col].unit = 'deg / s'
elif ('dy' in col) or ('dz' in col):
asol[col].unit = 'mm'
else:
asol[col].unit = 'deg'
asol['q_att'] = [mat2quat(euler2mat(np.deg2rad(p[0]),
np.deg2rad(-p[1]),
np.deg2rad(-p[2]), 'rzyx'))
for p in pointing]
# Copy info like the exposure time from the photons list meta to asol,
# but not column specific keywords like TTYPEn, TCTYPn, MTYPEn, MFORMn, etc:
for k in photons.meta:
if (('TYP' not in k) and ('FORM' not in k) and
('TCR' not in k) and ('TCDEL' not in k) and (k not in asol.meta)):
asol.meta[k] = photons.meta[k]
asol.meta['EXTNAME'] = 'ASPECT'
complete_header(asol.meta, None, 'ACASOL', ['OGIP', 'TEMPORALDATA', 'ASPECT'])
# In MARXS t=0 is the start of the observation, but for Chandra we need to make that
# consistent with the value of the TSTART keyword.
asol['time'] += asol.meta['TSTART'][0]
asol.write(asolfile, format='fits') # , checksum=True) - works only with fits interface
def __init__(self,processDim, aligned, aabb, A_IK):
self.processDim = processDim;
self.aabb = aabb;
self.aligned = aligned;
self.A_IK = A_IK;
self.q_KI = mat2quat(A_IK); # A_IK = R_KI
import numpy as np
from numpy.testing import (assert_array_almost_equal, assert_array_equal,
assert_almost_equal)
from transforms3d import quaternions as tq
from transforms3d import axangles as taa
from transforms3d.testing import assert_raises
from transforms3d.tests.samples import euler_mats
# Example quaternions (from rotations)
euler_quats = []
for M in euler_mats:
euler_quats.append(tq.mat2quat(M))
# M, quaternion pairs
eg_pairs = list(zip(euler_mats, euler_quats))
# Sets of arbitrary unit and not-unit quaternions
quats = set()
unit_quats = set()
params = np.arange(-2, 3, 0.5)
for w, x, y, z in product(params, params, params, params):
q = (w, x, y, z)
Nq = np.sqrt(np.dot(q, q))
if Nq == 0:
continue
quats.add(q)
q_n = tuple([e / Nq for e in q])
unit_quats.add(q_n)
