def testBoxMinus(self):
for i in range(100):
q1 = Quaternion.random()
q2 = Quaternion.random()
R1 = SO3.fromQuaternion(q1.q)
R2 = SO3.fromQuaternion(q2.q)
w1 = q1.boxminusr(q2)
w2 = R1.boxminusr(R2)
np.testing.assert_allclose(w1, w2)
def testLog(self):
for i in range(100):
q = Quaternion.random()
R = SO3.fromQuaternion(q.q)
w_true = SO3.Log(R)
w = Quaternion.Log(q)
np.testing.assert_allclose(w_true, w)
def test_boxplusl(self):
for i in range(100):
q = Quaternion.random()
R = SO3.fromQuaternion(q.q)
w = np.random.uniform(-np.pi, np.pi, size=3)
q2 = q.boxplusl(w)
R2 = R.boxplusl(w)
np.testing.assert_allclose(R2.R, q2.R.T)
def testRotatingVector(self):
for i in range(100):
v = np.random.uniform(-10, 10, size=3)
q = Quaternion.random()
R = SO3.fromQuaternion(q.q)
vp_true = R.rota(v)
vp = q.rota(v)
np.testing.assert_allclose(vp_true, vp)
for i in range(100):
v = np.random.uniform(-10, 10, size=3)
q = Quaternion.random()
R = SO3.fromQuaternion(q.q)
vp_true = R.rotp(v)
vp = q.rotp(v)
np.testing.assert_allclose(vp_true, vp)
def testFromQuaternion(self):
for i in range(100):
theta = np.random.uniform(-np.pi, np.pi)
vec = np.random.uniform(-10.0, 10.0, size=3)
vec = vec / np.linalg.norm(vec)
q = Quaternion.fromAxisAngle(vec * theta)
R = SO3.fromQuaternion(q.q)
R2 = SO3.fromAxisAngle(vec * theta)
np.testing.assert_allclose(R.R, R2.R)
def test_rotation_from_quaternion(self):
for T in self.transforms:
R = T.R.T # because from quaternion
R_true = SO3.fromQuaternion(T.q_arr).R
np.testing.assert_allclose(R_true, R)
def testRotationMatrixFromQuaternion(self):
for i in range(100):
q = Quaternion.random()
R = SO3.fromQuaternion(q.q)
np.testing.assert_allclose(q.R.T, R.R)