def test_homogeneous_conversion(self):
# 1. starting from a homogeneous matrix
theta1 = np.pi / 2 # 90 deg
trans = [10., 5., 0.]
H1 = np.array([[1., 0., 0., trans[0]],
[0., np.cos(theta1), -np.sin(theta1), trans[1]],
[0., np.sin(theta1),
np.cos(theta1), trans[2]], [0., 0., 0., 1.]])
# check that if we convert to DQ and back to homogeneous matrix, we get the same result
double_conv1 = DualQuaternion.from_homogeneous_matrix(
H1).homogeneous_matrix()
try:
np.testing.assert_array_almost_equal(H1, double_conv1)
except AssertionError as e:
self.fail(e)
# check that dual quaternions are also equal
dq1 = DualQuaternion.from_homogeneous_matrix(H1)
dq_double1 = DualQuaternion.from_homogeneous_matrix(double_conv1)
self.assertEqual(dq1, dq_double1)
# 2. starting from a DQ
dq_trans = DualQuaternion.from_translation_vector([10, 5, 0])
dq_rot = DualQuaternion.from_dq_array(
[np.cos(theta1 / 2),
np.sin(theta1 / 2), 0, 0, 0, 0, 0, 0])
dq2 = dq_trans * dq_rot
# check that this is the same as the previous DQ
self.assertEqual(dq2, dq1)
# check that if we convert to homogeneous matrix and back, we get the same result
double_conv2 = DualQuaternion.from_homogeneous_matrix(
dq2.homogeneous_matrix())
self.assertEqual(dq2, double_conv2)
def test_inverse(self):
# use known matrix inversion
T_1_2 = np.array([[0, 1, 0, 2], [-1, 0, 0, 4], [0, 0, 1, 6],
[0, 0, 0, 1]])
T_2_1 = np.array([[0, -1, 0, 4], [1, 0, 0, -2], [0, 0, 1, -6],
[0, 0, 0, 1]])
dq_1_2 = DualQuaternion.from_homogeneous_matrix(T_1_2)
dq_2_1 = DualQuaternion.from_homogeneous_matrix(T_2_1)
try:
np.testing.assert_array_almost_equal(
dq_2_1.homogeneous_matrix(),
dq_1_2.inverse().homogeneous_matrix())
except AssertionError as e:
self.fail(e)
def test_sclerp_orientation(self):
"""test Screw Linear Interpolation for diff orientation, same position"""
T_id = DualQuaternion.identity().homogeneous_matrix()
T_id[0:2, 0:2] = np.array([[0, -1], [1, 0]]) # rotate 90 around z
dq2 = DualQuaternion.from_homogeneous_matrix(T_id)
interpolated1 = DualQuaternion.sclerp(self.identity_dq, dq2, 0.5)
T_exp = DualQuaternion.identity().homogeneous_matrix()
sq22 = np.sqrt(2) / 2
T_exp[0:2, 0:2] = np.array([[sq22, -sq22],
[sq22, sq22]]) # rotate 45 around z
expected1 = DualQuaternion.from_homogeneous_matrix(T_exp)
self.assertEqual(interpolated1, expected1)
interpolated2 = DualQuaternion.sclerp(self.identity_dq, dq2, 0)
interpolated3 = DualQuaternion.sclerp(self.identity_dq, dq2, 1)
self.assertEqual(interpolated2, self.identity_dq)
self.assertEqual(interpolated3, dq2)
def test_creation(self):
# from dual quaternion array: careful, need to supply a normalized DQ
dql = np.array([
0.7071067811, 0.7071067811, 0, 0, -3.535533905, 3.535533905,
1.767766952, -1.767766952
])
dq1 = DualQuaternion.from_dq_array(dql)
dq2 = DualQuaternion.from_dq_array(dql)
self.assertEqual(dq1, dq2)
# from quaternion + translation array
dq3 = DualQuaternion.from_quat_pose_array(
np.array([1, 2, 3, 4, 5, 6, 7]))
dq4 = DualQuaternion.from_quat_pose_array([1, 2, 3, 4, 5, 6, 7])
self.assertEqual(dq3, dq4)
# from homogeneous transformation matrix
T = np.array([[1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 1, 1], [0, 0, 0, 1]])
dq7 = DualQuaternion.from_homogeneous_matrix(T)
self.assertEqual(dq7.q_r, quaternion.one)
self.assertEqual(dq7.translation(), [2, 3, 1])
try:
np.testing.assert_array_almost_equal(dq7.homogeneous_matrix(), T)
except AssertionError as e:
self.fail(e)
# from a point
dq8 = DualQuaternion.from_translation_vector([4, 6, 8])
self.assertEqual(dq8.translation(), [4, 6, 8])
def test_equal(self):
self.assertEqual(self.identity_dq, DualQuaternion.identity())
self.assertEqual(
self.identity_dq,
DualQuaternion(-np.quaternion(1, 0, 0, 0),
-np.quaternion(0, 0, 0, 0)))
self.assertFalse(self.identity_dq == DualQuaternion(
np.quaternion(1, 0, 0, 1), -np.quaternion(0, 0, 0, 0)))
theta1 = np.pi / 180 * 20 # 20 deg
T_pure_rot = np.array([[1., 0., 0., 0.],
[0., np.cos(theta1), -np.sin(theta1), 0.],
[0., np.sin(theta1),
np.cos(theta1), 0.], [0., 0., 0., 1.]])
dq_pure_rot = DualQuaternion.from_homogeneous_matrix(T_pure_rot)
# manually flip sign on terms
dq_pure_rot.q_r = -dq_pure_rot.q_r
dq_pure_rot.q_d = -dq_pure_rot.q_d
try:
np.testing.assert_array_almost_equal(
dq_pure_rot.homogeneous_matrix(), T_pure_rot)
except AssertionError as e:
self.fail(e)
dq_pure_rot.q_d = -dq_pure_rot.q_d
try:
np.testing.assert_array_almost_equal(
dq_pure_rot.homogeneous_matrix(), T_pure_rot)
except AssertionError as e:
self.fail(e)
dq_pure_rot.q_r = -dq_pure_rot.q_r
try:
np.testing.assert_array_almost_equal(
dq_pure_rot.homogeneous_matrix(), T_pure_rot)
except AssertionError as e:
self.fail(e)
def test_mult(self):
# quaternion multiplication. Compare with homogeneous transformation matrices
theta1 = np.pi / 180 * 20 # 20 deg
T_pure_rot = np.array([[1., 0., 0., 0.],
[0., np.cos(theta1), -np.sin(theta1), 0.],
[0., np.sin(theta1),
np.cos(theta1), 0.], [0., 0., 0., 1.]])
dq_pure_rot = DualQuaternion.from_homogeneous_matrix(T_pure_rot)
T_pure_trans = np.array([[1., 0., 0., 1.], [0., 1., 0., 2.],
[0., 0., 1., 3.], [0., 0., 0., 1.]])
dq_pure_trans = DualQuaternion.from_homogeneous_matrix(T_pure_trans)
T_double_rot = np.dot(T_pure_rot, T_pure_rot)
dq_double_rot = dq_pure_rot * dq_pure_rot
try:
np.testing.assert_array_almost_equal(
T_double_rot, dq_double_rot.homogeneous_matrix())
except AssertionError as e:
self.fail(e)
T_double_trans = np.dot(T_pure_trans, T_pure_trans)
dq_double_trans = dq_pure_trans * dq_pure_trans
try:
np.testing.assert_array_almost_equal(
T_double_trans, dq_double_trans.homogeneous_matrix())
except AssertionError as e:
self.fail(e)
# composed: trans and rot
T_composed = np.dot(T_pure_rot, T_pure_trans)
dq_composed = dq_pure_rot * dq_pure_trans
dq_composed = dq_pure_rot * dq_pure_trans
try:
np.testing.assert_array_almost_equal(
T_composed, dq_composed.homogeneous_matrix())
except AssertionError as e:
self.fail(e)
def test_quaternion_conjugate(self):
dq = self.normalized_dq * self.normalized_dq.quaternion_conjugate()
# a normalized quaternion multiplied with its quaternion conjugate should yield unit dual quaternion
self.assertEqual(dq, DualQuaternion.identity())
# test that the conjugate corresponds to the inverse of it's matrix representation
matr = self.normalized_dq.homogeneous_matrix()
inv = np.linalg.inv(matr)
self.assertEqual(DualQuaternion.from_homogeneous_matrix(inv),
self.normalized_dq.quaternion_conjugate())
# (dq1 @ dq2)* ?= dq2* @ dq1*
res1 = (self.random_dq * self.other_random_dq).quaternion_conjugate()
res2 = self.other_random_dq.quaternion_conjugate(
) * self.random_dq.quaternion_conjugate()
self.assertEqual(res1, res2)
def blend_poses(poses: Sequence[np.ndarray]) -> np.ndarray:
"""Blend a list of 4x4 transformation matrices together using dual
quaternion blending.
See: https://www.cs.utah.edu/~ladislav/kavan06dual/kavan06dual.pdf
:param poses: A list of poses to blend.
:return: The result of DQB applied on the input poses.
"""
dq = DualQuaternion.from_dq_array([0, 0, 0, 0, 0, 0, 0])
# We use a constant weight for all poses.
weight = 1.0 / len(poses)
for p in poses:
dq += weight * DualQuaternion.from_homogeneous_matrix(p)
dq.normalize()
return dq.homogeneous_matrix()
def test_transform(self):
# transform a point from one frame (f2) to another (f1)
point_f2 = [1, 1, 0]
self.assertEqual(self.identity_dq.transform_point(point_f2), point_f2)
# test that quaternion transform and matrix transform yield the same result
T_f1_f2 = np.array([[1, 0, 0, 2], [0, 0.5403, -0.8415, 3],
[0, 0.8415, 0.5403, 1], [0, 0, 0, 1]])
dq_f1_f2 = DualQuaternion.from_homogeneous_matrix(T_f1_f2)
# point is in f2, transformation will express it in f1
point_f1_matrix = np.dot(T_f1_f2,
np.expand_dims(np.array(point_f2 + [1]), 1))
point_f1_dq = np.array(dq_f1_f2.transform_point(point_f2))
try:
np.testing.assert_array_almost_equal(
point_f1_matrix[:3].T.flatten(),
point_f1_dq.flatten(),
decimal=3)
except AssertionError as e:
self.fail(e)
import numpy as np
import quaternion as nq
from dual_quaternions import DualQuaternion
from util_affine import * # spm_matrix, spm_imatrix
np.random.seed(4)
nb_mean=500
euler_mean, euler_choral, euler_exp, euler_pol, euler_slerp, euler_qr_slerp = np.zeros((nb_mean,3)), np.zeros((nb_mean,3)), np.zeros((nb_mean,3)), np.zeros((nb_mean,3)), np.zeros((nb_mean,3)), np.zeros((nb_mean,3))
for i in range(nb_mean):
#rot_euler = np.random.normal(size=(10, 3),loc=20,scale=5) #in degree
rot_euler = np.random.uniform(-10,10,size=(10, 3)) #in degree
#print(f'min {np.min(rot_euler)} max {np.max(rot_euler)}')
aff_list = get_affine_rot_from_euler(rot_euler) #4*4 affine matrix
qr_list = [ nq.from_rotation_matrix(aff) for aff in aff_list] #unit quaternion
dq_list = [ DualQuaternion.from_homogeneous_matrix(aff) for aff in aff_list] #unit quaternion
euler_mean[i,:] = np.mean(rot_euler,axis=0)
qr_cm = nq.mean_rotor_in_chordal_metric(qr_list); #print(get_info_from_quat(qr_cm)); print(get_euler_for_qr(qr_cm))
euler_choral[i,:] = get_euler_from_qr(qr_cm)
aff_exp_mean = exp_mean_affine(aff_list); #print(spm_imatrix(aff_exp_mean)[3:6])
euler_exp[i,:] = get_euler_from_affine(aff_exp_mean)
aff_polar_mean = polar_mean_affin(aff_list); #print(spm_imatrix(aff_exp_mean)[3:6])
euler_pol[i,:] = get_euler_from_affine(aff_polar_mean)
qr_mean = qr_slerp_mean(qr_list)
euler_qr_slerp[i,:] = get_euler_from_qr(qr_mean)
#qr_mean = qr_euclidian_mean(qr_list) #arg diff 180 ??