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)