本文整理汇总了Python中quaternion.as_float_array方法的典型用法代码示例。如果您正苦于以下问题:Python quaternion.as_float_array方法的具体用法?Python quaternion.as_float_array怎么用?Python quaternion.as_float_array使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类quaternion
的用法示例。
在下文中一共展示了quaternion.as_float_array方法的14个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: rotmat2quat
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def rotmat2quat(rotmats):
"""
Convert rotation matrices to quaternions. It ensures that there's no switch to the antipodal representation
within this sequence of rotations.
Args:
oris: np array of shape (seq_length, n_joints*9).
Returns: np array of shape (seq_length, n_joints*4)
"""
seq_length = rotmats.shape[0]
assert rotmats.shape[1] % 9 == 0
ori = np.reshape(rotmats, [seq_length, -1, 3, 3])
ori_q = quaternion.as_float_array(quaternion.from_rotation_matrix(ori))
ori_qc = correct_antipodal_quaternions(ori_q)
ori_qc = np.reshape(ori_qc, [seq_length, -1])
return ori_qc
示例2: interpolate_quaternion_linear
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def interpolate_quaternion_linear(data, ts_in, ts_out):
"""
This function interpolate the input quaternion array into another time stemp.
Args:
data: Nx4 array containing N quaternions.
ts_in: input_timestamp- N-sized array containing time stamps for each of the input quaternion.
ts_out: output_timestamp- M-sized array containing output time stamps.
Return:
Mx4 array containing M quaternions.
"""
assert np.amin(ts_in) <= np.amin(ts_out), 'Input time range must cover output time range'
assert np.amax(ts_in) >= np.amax(ts_out), 'Input time range must cover output time range'
pt = np.searchsorted(ts_in, ts_out)
d_left = quaternion.from_float_array(data[pt - 1])
d_right = quaternion.from_float_array(data[pt])
ts_left, ts_right = ts_in[pt - 1], ts_in[pt]
d_out = quaternion.quaternion_time_series.slerp(d_left, d_right, ts_left, ts_right, ts_out)
return quaternion.as_float_array(d_out)
示例3: recon_traj_with_preds
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def recon_traj_with_preds(dataset, preds, seq_id=0, **kwargs):
ind = np.array([i[1] for i in dataset.index_map if i[0] == seq_id], dtype=np.int)
ts = dataset.ts[seq_id]
dt = np.mean(ts[ind[1:]] - ts[ind[:-1]])
rot_local_to_grav = get_rotation_compensate_gravity(dataset.gravity[seq_id])
ori = quaternion.from_float_array(dataset.orientations[seq_id])
rot_grav_to_glob = (ori * quaternion.from_float_array(rot_local_to_grav).conj())[ind]
za = np.zeros(preds.shape[0])
preds_q = quaternion.from_float_array(np.stack([za, preds[:, 0], za, preds[:, 1]], axis=1))
glob_v = quaternion.as_float_array(rot_grav_to_glob * preds_q * rot_grav_to_glob.conj())[:, 1:] * dt
pos = np.zeros([glob_v.shape[0] + 2, 2])
pos[0] = dataset.gt_pos[seq_id][0, :2]
pos[1:-1] = np.cumsum(glob_v[:, :2], axis=0) + pos[0]
pos[-1] = pos[-2]
ts_ext = np.concatenate([[ts[0] - 1e-06], ts[ind], [ts[-1] + 1e-06]], axis=0)
pos = interp1d(ts_ext, pos, axis=0)(ts)
return pos
示例4: minimal_rotation
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def minimal_rotation(R, t, iterations=2):
"""Adjust frame so that there is no rotation about z' axis
The output of this function is a frame that rotates the z axis onto the same z' axis as the
input frame, but with minimal rotation about that axis. This is done by pre-composing the input
rotation with a rotation about the z axis through an angle gamma, where
dgamma/dt = 2*(dR/dt * z * R.conjugate()).w
This ensures that the angular velocity has no component along the z' axis.
Note that this condition becomes easier to impose the closer the input rotation is to a
minimally rotating frame, which means that repeated application of this function improves its
accuracy. By default, this function is iterated twice, though a few more iterations may be
called for.
Parameters
==========
R: quaternion array
Time series describing rotation
t: float array
Corresponding times at which R is measured
iterations: int [defaults to 2]
Repeat the minimization to refine the result
"""
from scipy.interpolate import InterpolatedUnivariateSpline as spline
if iterations == 0:
return R
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
halfgammadot = quaternion.as_float_array(Rdot * quaternion.z * np.conjugate(R))[:, 0]
halfgamma = spline(t, halfgammadot).antiderivative()(t)
Rgamma = np.exp(quaternion.z * halfgamma)
return minimal_rotation(R * Rgamma, t, iterations=iterations-1)
示例5: angular_velocity
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def angular_velocity(R, t):
from scipy.interpolate import InterpolatedUnivariateSpline as spline
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return quaternion.as_float_array(2*Rdot/R)[:, 1:]
示例6: test_as_float_quat
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def test_as_float_quat(Qs):
qs = Qs[Qs_nonnan]
for quats in [qs, np.vstack((qs,)*3), np.vstack((qs,)*(3*5)).reshape((3, 5)+qs.shape),
np.vstack((qs,)*(3*5*6)).reshape((3, 5, 6)+qs.shape)]:
floats = quaternion.as_float_array(quats)
assert floats.shape == quats.shape+(4,)
assert allclose(quaternion.as_quat_array(floats), quats)
assert allclose(quaternion.from_float_array(floats), quats)
# Test that we can handle a list just like an array
assert np.array_equal(quaternion.as_quat_array(floats), quaternion.as_quat_array(floats.tolist()))
a = np.arange(12).reshape(3, 4)
assert np.array_equal(quaternion.as_float_array(quaternion.as_quat_array(a)),
a.astype(float))
assert quaternion.as_float_array(quaternion.x).ndim == 1
示例7: test_as_rotation_matrix
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def test_as_rotation_matrix(Rs):
def quat_mat(quat):
return np.array([(quat * v * quat.inverse()).vec for v in [quaternion.x, quaternion.y, quaternion.z]]).T
def quat_mat_vec(quats):
mat_vec = np.array([quaternion.as_float_array(quats * v * np.reciprocal(quats))[..., 1:]
for v in [quaternion.x, quaternion.y, quaternion.z]])
return np.transpose(mat_vec, tuple(range(mat_vec.ndim))[1:-1]+(-1, 0))
with pytest.raises(ZeroDivisionError):
quaternion.as_rotation_matrix(quaternion.zero)
for R in Rs:
# Test correctly normalized rotors:
assert allclose(quat_mat(R), quaternion.as_rotation_matrix(R), atol=2*eps)
# Test incorrectly normalized rotors:
assert allclose(quat_mat(R), quaternion.as_rotation_matrix(1.1*R), atol=2*eps)
Rs0 = Rs.copy()
Rs0[Rs.shape[0]//2] = quaternion.zero
with pytest.raises(ZeroDivisionError):
quaternion.as_rotation_matrix(Rs0)
# Test correctly normalized rotors:
assert allclose(quat_mat_vec(Rs), quaternion.as_rotation_matrix(Rs), atol=2*eps)
# Test incorrectly normalized rotors:
assert allclose(quat_mat_vec(Rs), quaternion.as_rotation_matrix(1.1*Rs), atol=2*eps)
# Simply test that this function succeeds and returns the right shape
assert quaternion.as_rotation_matrix(Rs.reshape((2, 5, 10))).shape == (2, 5, 10, 3, 3)
示例8: SWSH
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def SWSH(R, s, indices):
"""Spin-weighted spherical harmonic calculation from rotor
Note that this function is more general than standard Ylm and sYlm functions of angles because it uses quaternion
rotors instead of angle, and is slightly faster as a result.
This function can be called in either of two ways:
1) With an array of quaternions, and a single (ell,m) pair, or
2) with a single quaternion, and an array of (ell,m) values
Parameters
----------
R : unit quaternion or array of unit quaternions
Rotor on which to evaluate the SWSH function.
s : int
Spin weight of the field to evaluate
indices : 2-d array of int or pair of ints
Array of (ell,m) values to evaluate
Returns
-------
array of complex
The shape of this array is `indices.shape[0]`, and contains the values of the SWSH for the (ell,m) values
specified in `indices`.
"""
indices = np.asarray(indices)
if indices.size > 2 or not isinstance(R, np.ndarray):
values = np.empty((indices.shape[0],), dtype=complex)
_SWSH(R.a, R.b, s, indices, values)
else:
values = np.empty((R.size,), dtype=complex)
_SWSHs(quaternion.as_float_array(R.flatten()), s, indices[0], indices[1], values)
values = values.reshape(R.shape)
return values
示例9: load
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def load(self, path):
if path[-1] == '/':
path = path[:-1]
self.info['path'] = osp.split(path)[-1]
self.info['ori_source'] = 'game_rv'
if osp.exists(osp.join(path, 'processed/data.csv')):
imu_all = pandas.read_csv(osp.join(path, 'processed/data.csv'))
else:
imu_all = pandas.read_pickle(osp.join(path, 'processed/data.pkl'))
ts = imu_all[['time']].values / 1e09
gyro = imu_all[['gyro_x', 'gyro_y', 'gyro_z']].values
acce = imu_all[['acce_x', 'acce_y', 'acce_z']].values
tango_pos = imu_all[['pos_x', 'pos_y', 'pos_z']].values
# Use game rotation vector as device orientation.
init_tango_ori = quaternion.quaternion(*imu_all[['ori_w', 'ori_x', 'ori_y', 'ori_z']].values[0])
game_rv = quaternion.from_float_array(imu_all[['rv_w', 'rv_x', 'rv_y', 'rv_z']].values)
init_rotor = init_tango_ori * game_rv[0].conj()
ori = init_rotor * game_rv
nz = np.zeros(ts.shape)
gyro_q = quaternion.from_float_array(np.concatenate([nz, gyro], axis=1))
acce_q = quaternion.from_float_array(np.concatenate([nz, acce], axis=1))
gyro_glob = quaternion.as_float_array(ori * gyro_q * ori.conj())[:, 1:]
acce_glob = quaternion.as_float_array(ori * acce_q * ori.conj())[:, 1:]
self.ts = ts
self.features = np.concatenate([gyro_glob, acce_glob], axis=1)
self.targets = (tango_pos[self.w:, :2] - tango_pos[:-self.w, :2]) / (ts[self.w:] - ts[:-self.w])
self.gt_pos = tango_pos
self.orientations = quaternion.as_float_array(game_rv)
print(self.ts.shape, self.features.shape, self.targets.shape, self.gt_pos.shape, self.orientations.shape,
self.w)
示例10: load
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
self.info['path'] = osp.split(data_path)[-1]
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source(
data_path, self.max_ori_error, self.grv_only)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
gyro_uncalib = f['synced/gyro_uncalib']
acce_uncalib = f['synced/acce']
gyro = gyro_uncalib - np.array(self.info['imu_init_gyro_bias'])
acce = np.array(self.info['imu_acce_scale']) * (acce_uncalib - np.array(self.info['imu_acce_bias']))
ts = np.copy(f['synced/time'])
tango_pos = np.copy(f['pose/tango_pos'])
init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0])
# Compute the IMU orientation in the Tango coordinate frame.
ori_q = quaternion.from_float_array(ori)
rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration'])
init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj()
ori_q = init_rotor * ori_q
dt = (ts[self.w:] - ts[:-self.w])[:, None]
glob_v = (tango_pos[self.w:] - tango_pos[:-self.w]) / dt
gyro_q = quaternion.from_float_array(np.concatenate([np.zeros([gyro.shape[0], 1]), gyro], axis=1))
acce_q = quaternion.from_float_array(np.concatenate([np.zeros([acce.shape[0], 1]), acce], axis=1))
glob_gyro = quaternion.as_float_array(ori_q * gyro_q * ori_q.conj())[:, 1:]
glob_acce = quaternion.as_float_array(ori_q * acce_q * ori_q.conj())[:, 1:]
start_frame = self.info.get('start_frame', 0)
self.ts = ts[start_frame:]
self.features = np.concatenate([glob_gyro, glob_acce], axis=1)[start_frame:]
self.targets = glob_v[start_frame:, :2]
self.orientations = quaternion.as_float_array(ori_q)[start_frame:]
self.gt_pos = tango_pos[start_frame:]
示例11: angle_between_quaternions
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def angle_between_quaternions(q1: np.quaternion, q2: np.quaternion) -> float:
r"""Returns the angle (in radians) between two quaternions. This angle will
always be positive.
"""
q1_inv = np.conjugate(q1)
dq = quaternion.as_float_array(q1_inv * q2)
return 2 * np.arctan2(np.linalg.norm(dq[1:]), np.abs(dq[0]))
示例12: q_from_rotation_matrix
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def q_from_rotation_matrix(R):
q = quaternion.as_float_array(quaternion.from_rotation_matrix(R))
return q
示例13: test_numpy_array_conversion
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def test_numpy_array_conversion(Qs):
"Check conversions between array as quaternions and array as floats"
# First, just check 1-d array
Q = Qs[Qs_nonnan][:12] # Select first 3x4=12 non-nan elements in Qs
assert Q.dtype == np.dtype(np.quaternion)
q = quaternion.as_float_array(Q) # View as array of floats
assert q.dtype == np.dtype(np.float)
assert q.shape == (12, 4) # This is the expected shape
for j in range(12):
for k in range(4): # Check each component individually
assert q[j][k] == Q[j].components[k]
assert np.array_equal(quaternion.as_quat_array(q), Q) # Check that we can go backwards
# Next, see how that works if I flatten the q array
q = q.flatten()
assert q.dtype == np.dtype(np.float)
assert q.shape == (48,)
for j in range(48):
assert q[j] == Q[j // 4].components[j % 4]
assert np.array_equal(quaternion.as_quat_array(q), Q) # Check that we can go backwards
# Now, reshape into 2-d array, and re-check
P = Q.reshape(3, 4) # Reshape into 3x4 array of quaternions
p = quaternion.as_float_array(P) # View as array of floats
assert p.shape == (3, 4, 4) # This is the expected shape
for j in range(3):
for k in range(4):
for l in range(4): # Check each component individually
assert p[j][k][l] == Q[4 * j + k].components[l]
assert np.array_equal(quaternion.as_quat_array(p), P) # Check that we can go backwards
# Check that we get an exception if the final dimension is not divisible by 4
with pytest.raises(ValueError):
quaternion.as_quat_array(np.random.rand(4, 1))
with pytest.raises(ValueError):
quaternion.as_quat_array(np.random.rand(4, 2))
with pytest.raises(ValueError):
quaternion.as_quat_array(np.random.rand(4, 3))
with pytest.raises(ValueError):
quaternion.as_quat_array(np.random.rand(4, 5))
with pytest.raises(ValueError):
quaternion.as_quat_array(np.random.rand(4, 5, 3, 2, 1))
# Finally, check that it works on non-contiguous arrays, by adding random padding and then slicing
q = quaternion.as_float_array(Q)
q = np.concatenate((np.random.rand(q.shape[0], 3), q, np.random.rand(q.shape[0], 3)), axis=1)
assert np.array_equal(quaternion.as_quat_array(q[:, 3:7]), Q)
示例14: load
# 需要导入模块: import quaternion [as 别名]
# 或者: from quaternion import as_float_array [as 别名]
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
self.info['path'] = osp.split(data_path)[-1]
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source(
data_path, self.max_ori_error, self.grv_only)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
ts = np.copy(f['synced/time'])
if 'synced/grav' in f:
gravity = np.copy(f['synced/grav'])
else:
gravity = np.copy(f['synced/gravity'])
tango_pos = np.copy(f['pose/tango_pos'])
init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0])
gyro = np.copy(f['synced/gyro'])
linacce = np.copy(f['synced/linacce'])
# Compute the IMU orientation in the Tango coordinate frame.
ori_q = quaternion.from_float_array(ori)
rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration'])
init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj()
ori_q = init_rotor * ori_q
dt = (ts[self.w:] - ts[:-self.w])[:, None]
glob_v = np.concatenate(
[np.zeros([dt.shape[0], 1]), tango_pos[self.w:] - tango_pos[:-self.w]], axis=1) / dt
local_v = ori_q[:-self.w].conj() * quaternion.from_float_array(glob_v) * ori_q[:-self.w]
local_v = quaternion.as_float_array(local_v)[:, 1:]
local_v_g = align_3dvector_with_gravity(local_v, gravity[:-self.w])
start_frame = self.info.get('start_frame', 0)
self.ts = ts[start_frame:]
self.features = np.concatenate([gyro, linacce], axis=1)[start_frame:]
self.targets = local_v_g[start_frame:, [0, 2]]
self.orientations = quaternion.as_float_array(ori_q)[start_frame:]
self.gt_pos = tango_pos[start_frame:]
self.gravity = gravity[start_frame:]