class testOrienationMatrix(unittest.TestCase): def setUp(self): tmp = np.array([0.2623303, 0.8853649, 0.1311652, -0.3607042]) self.q = Quaternion(tmp) self.g = OrientationMatrix(self.q.toOrientationMatrix()) def test_toQuaternion(self): q = self.q.toOrientationMatrix() g = self.g.tondarray() np.testing.assert_almost_equal(q, g)
class testOrienationMatrix(unittest.TestCase): def setUp(self): tmp = np.array([0.2623303, 0.8853649, 0.1311652, -0.3607042]) self.q = Quaternion(tmp) self.g = OrientationMatrix(self.q.toOrientationMatrix()) def test_toQuaternion(self): q = self.q.toOrientationMatrix() g = self.g.tondarray() np.testing.assert_almost_equal(q,g)
def test_avgQuaternions(self): qs = [self.q5, self.q6, self.q7] average = Quaternion.average(qs) e_avg = average.toEulers() t_avg = np.array([42.12201286, 0.0, 0.0]) target = np.array([0.9331983, 0.000000, 0.000000, 0.3593618]) np.testing.assert_almost_equal(average.tondarray(), target) np.testing.assert_almost_equal(e_avg, t_avg)
def setUp(self): self.q1 = Quaternion(np.array([-1.0, 0.0, 0.0, 0.0 ])) self.q2 = Quaternion(np.array([ 1.0, 2.0, 3.0, 4.0 ])) self.q3 = Quaternion(np.array([ 3.0, 7.0, 9.0, 2.0 ])) self.q4 = Quaternion(np.array([ 1.0, 2.0,-5.0, 1.0 ])) self.q5 = Quaternion(np.array([ 0.7071, 0, 0, 0.7071])) self.q6 = Quaternion(np.array([0.9961946980917455, 0.000000, 0.000000, 0.08715574274765817])) self.q7 = Quaternion(np.array([0.9659258262890683, 0.000000, 0.000000, 0.25881904510252074])) self.u1 = self.q1.unitary() self.u2 = self.q2.unitary() self.u3 = self.q3.unitary() self.u4 = self.q4.unitary() self.u5 = self.q5.unitary() self.p1 = np.array([1.0,0.0,0.0])
def setUp(self): self.q1 = Quaternion(np.array([-1.0, 0.0, 0.0, 0.0])) self.q2 = Quaternion(np.array([1.0, 2.0, 3.0, 4.0])) self.q3 = Quaternion(np.array([3.0, 7.0, 9.0, 2.0])) self.q4 = Quaternion(np.array([1.0, 2.0, -5.0, 1.0])) self.q5 = Quaternion(np.array([0.7071, 0, 0, 0.7071])) self.q6 = Quaternion( np.array( [0.9961946980917455, 0.000000, 0.000000, 0.08715574274765817])) self.q7 = Quaternion( np.array( [0.9659258262890683, 0.000000, 0.000000, 0.25881904510252074])) self.u1 = self.q1.unitary() self.u2 = self.q2.unitary() self.u3 = self.q3.unitary() self.u4 = self.q4.unitary() self.u5 = self.q5.unitary() self.p1 = np.array([1.0, 0.0, 0.0])
class testQuaternion(unittest.TestCase): def setUp(self): self.q1 = Quaternion(np.array([-1.0, 0.0, 0.0, 0.0])) self.q2 = Quaternion(np.array([1.0, 2.0, 3.0, 4.0])) self.q3 = Quaternion(np.array([3.0, 7.0, 9.0, 2.0])) self.q4 = Quaternion(np.array([1.0, 2.0, -5.0, 1.0])) self.q5 = Quaternion(np.array([0.7071, 0, 0, 0.7071])) self.q6 = Quaternion( np.array( [0.9961946980917455, 0.000000, 0.000000, 0.08715574274765817])) self.q7 = Quaternion( np.array( [0.9659258262890683, 0.000000, 0.000000, 0.25881904510252074])) self.u1 = self.q1.unitary() self.u2 = self.q2.unitary() self.u3 = self.q3.unitary() self.u4 = self.q4.unitary() self.u5 = self.q5.unitary() self.p1 = np.array([1.0, 0.0, 0.0]) def test_add(self): calc1 = self.q1 + self.q2 target1 = [2.0, 2.0, 3.0, 4.0] np.testing.assert_almost_equal(calc1.tolist(), target1) def test_sub(self): calc1 = self.q1 - self.q2 target1 = [0.0, -2.0, -3.0, -4.0] np.testing.assert_almost_equal(calc1.tolist(), target1) def test_mul(self): calc1 = self.u1 * self.u2 np.testing.assert_almost_equal(calc1.tolist(), self.u2.tolist()) calc2 = self.u2 * self.u4 target2 = [0.2623303, 0.8853649, 0.1311652, -0.3607042] np.testing.assert_almost_equal(calc2.tolist(), target2) def test_rotate(self): calc1 = Quaternion.rotate(self.u5, self.p1) target = [0.0, 1.0, 0.0] np.testing.assert_almost_equal(calc1, target) def test_div(self): calc1 = self.q2 / self.q3 target1 = [52, 29, -24, 13] np.testing.assert_almost_equal(calc1.tolist(), target1) calc2 = self.u2 / self.u3 target2 = [0.7939163, 0.4427610, -0.3664228, 0.1984791] np.testing.assert_almost_equal(calc2.tolist(), target2) def test_scale(self): calc1 = Quaternion.scale(self.q3, 5) target = np.array([3.0, 7.0, 9.0, 2.0]) * 5 np.testing.assert_almost_equal(calc1.tondarray(), target) def test_2Eulers(self): e1 = np.array([-0., 0., 0.]) e2 = np.array([132.27368901, 82.33774434, 19.65382406]) e3 = np.array([85.81508387, 144.90319877, 341.56505118]) np.testing.assert_almost_equal(self.q1.toEulers(), e1) np.testing.assert_almost_equal(self.q2.toEulers(), e2) np.testing.assert_almost_equal(self.q3.toEulers(), e3) def test_2Rodrigues(self): r1 = np.array([-0., -0., -0.]) r2 = np.array([2., 3., 4.]) r3 = np.array([2.33333333, 3., 0.66666667]) np.testing.assert_almost_equal(self.q1.toRodrigues(), r1) np.testing.assert_almost_equal(self.q2.toRodrigues(), r2) np.testing.assert_almost_equal(self.q3.toRodrigues(), r3) def test_2OrientationMatrix(self): m1 = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) m2 = np.array([[-0.66666667, 0.13333333, 0.73333333], [0.66666667, -0.33333333, 0.66666667], [0.33333333, 0.93333333, 0.13333333]]) m3 = np.array([[-0.18881119, 0.7972028, 0.57342657], [0.96503497, 0.25874126, -0.04195804], [-0.18181818, 0.54545455, -0.81818182]]) np.testing.assert_almost_equal(self.q1.toOrientationMatrix(), m1) np.testing.assert_almost_equal(self.q2.toOrientationMatrix(), m2) np.testing.assert_almost_equal(self.q3.toOrientationMatrix(), m3) def test_2AngleAxis(self): a1, v1 = (0.0, np.array([1., 0., 0.])) a2, v2 = (2.774384633031956, np.array([0.37139068, 0.55708601, 0.74278135])) a3, v3 = (2.6344294922932665, np.array([0.6047079, 0.77748158, 0.17277369])) mya1, myv1 = self.q1.toAngleAxis() mya2, myv2 = self.q2.toAngleAxis() mya3, myv3 = self.q3.toAngleAxis() np.testing.assert_almost_equal(a1, mya1) np.testing.assert_almost_equal(v1, myv1) np.testing.assert_almost_equal(a2, mya2) np.testing.assert_almost_equal(v2, myv2) np.testing.assert_almost_equal(a3, mya3) np.testing.assert_almost_equal(v3, myv3) def test_cmp(self): self.assertTrue(self.q2 == self.q2) self.assertTrue(self.q3 == -self.q3) self.assertFalse(self.q1 == self.q2) def test_avgQuaternions(self): qs = [self.q5, self.q6, self.q7] average = Quaternion.average(qs) e_avg = average.toEulers() t_avg = np.array([42.12201286, 0.0, 0.0]) target = np.array([0.9331983, 0.000000, 0.000000, 0.3593618]) np.testing.assert_almost_equal(average.tondarray(), target) np.testing.assert_almost_equal(e_avg, t_avg)
def setUp(self): tmp = np.array([0.2623303, 0.8853649, 0.1311652, -0.3607042]) self.q = Quaternion(tmp) self.g = OrientationMatrix(self.q.toOrientationMatrix())
def test_scale(self): calc1 = Quaternion.scale(self.q3, 5) target = np.array([3.0, 7.0, 9.0, 2.0]) * 5 np.testing.assert_almost_equal(calc1.tondarray(), target)
def test_rotate(self): calc1 = Quaternion.rotate(self.u5, self.p1) target = [0.0, 1.0, 0.0] np.testing.assert_almost_equal(calc1, target)
class testQuaternion(unittest.TestCase): def setUp(self): self.q1 = Quaternion(np.array([-1.0, 0.0, 0.0, 0.0 ])) self.q2 = Quaternion(np.array([ 1.0, 2.0, 3.0, 4.0 ])) self.q3 = Quaternion(np.array([ 3.0, 7.0, 9.0, 2.0 ])) self.q4 = Quaternion(np.array([ 1.0, 2.0,-5.0, 1.0 ])) self.q5 = Quaternion(np.array([ 0.7071, 0, 0, 0.7071])) self.q6 = Quaternion(np.array([0.9961946980917455, 0.000000, 0.000000, 0.08715574274765817])) self.q7 = Quaternion(np.array([0.9659258262890683, 0.000000, 0.000000, 0.25881904510252074])) self.u1 = self.q1.unitary() self.u2 = self.q2.unitary() self.u3 = self.q3.unitary() self.u4 = self.q4.unitary() self.u5 = self.q5.unitary() self.p1 = np.array([1.0,0.0,0.0]) def test_add(self): calc1 = self.q1 + self.q2 target1 = [2.0, 2.0, 3.0, 4.0] np.testing.assert_almost_equal(calc1.tolist(), target1) def test_sub(self): calc1 = self.q1 - self.q2 target1 = [0.0, -2.0, -3.0, -4.0] np.testing.assert_almost_equal(calc1.tolist(), target1) def test_mul(self): calc1 = self.u1 * self.u2 np.testing.assert_almost_equal(calc1.tolist(), self.u2.tolist()) calc2 = self.u2 * self.u4 target2 = [0.2623303, 0.8853649, 0.1311652, -0.3607042] np.testing.assert_almost_equal(calc2.tolist(), target2) def test_rotate(self): calc1 = Quaternion.rotate(self.u5, self.p1) target = [0.0, 1.0, 0.0] np.testing.assert_almost_equal(calc1, target) def test_div(self): calc1 = self.q2 / self.q3 target1 = [52, 29, -24, 13] np.testing.assert_almost_equal(calc1.tolist(), target1) calc2 = self.u2 / self.u3 target2 = [0.7939163, 0.4427610, -0.3664228, 0.1984791] np.testing.assert_almost_equal(calc2.tolist(), target2) def test_scale(self): calc1 = Quaternion.scale(self.q3, 5) target = np.array([3.0, 7.0, 9.0, 2.0]) * 5 np.testing.assert_almost_equal(calc1.tondarray(), target) def test_2Eulers(self): e1 = np.array([ -0., 0., 0. ]) e2 = np.array([ 132.27368901, 82.33774434, 19.65382406]) e3 = np.array([ 85.81508387, 144.90319877, 341.56505118]) np.testing.assert_almost_equal(self.q1.toEulers(), e1) np.testing.assert_almost_equal(self.q2.toEulers(), e2) np.testing.assert_almost_equal(self.q3.toEulers(), e3) def test_2Rodrigues(self): r1 = np.array([-0., -0., -0.]) r2 = np.array([ 2., 3., 4.]) r3 = np.array([ 2.33333333, 3. , 0.66666667]) np.testing.assert_almost_equal(self.q1.toRodrigues(), r1) np.testing.assert_almost_equal(self.q2.toRodrigues(), r2) np.testing.assert_almost_equal(self.q3.toRodrigues(), r3) def test_2OrientationMatrix(self): m1 = np.array([[ 1., 0., 0.], [ 0., 1., 0.], [ 0., 0., 1.]]) m2 = np.array([[-0.66666667, 0.13333333, 0.73333333], [ 0.66666667, -0.33333333, 0.66666667], [ 0.33333333, 0.93333333, 0.13333333]]) m3 = np.array([[-0.18881119, 0.7972028 , 0.57342657], [ 0.96503497, 0.25874126, -0.04195804], [-0.18181818, 0.54545455, -0.81818182]]) np.testing.assert_almost_equal(self.q1.toOrientationMatrix(), m1) np.testing.assert_almost_equal(self.q2.toOrientationMatrix(), m2) np.testing.assert_almost_equal(self.q3.toOrientationMatrix(), m3) def test_2AngleAxis(self): a1, v1 = (0.0, np.array([ 1., 0., 0.])) a2, v2 = (2.774384633031956, np.array([ 0.37139068, 0.55708601, 0.74278135])) a3, v3 = (2.6344294922932665, np.array([ 0.6047079 , 0.77748158, 0.17277369])) mya1, myv1 = self.q1.toAngleAxis() mya2, myv2 = self.q2.toAngleAxis() mya3, myv3 = self.q3.toAngleAxis() np.testing.assert_almost_equal(a1, mya1) np.testing.assert_almost_equal(v1, myv1) np.testing.assert_almost_equal(a2, mya2) np.testing.assert_almost_equal(v2, myv2) np.testing.assert_almost_equal(a3, mya3) np.testing.assert_almost_equal(v3, myv3) def test_cmp(self): self.assertTrue( self.q2 == self.q2) self.assertTrue( self.q3 == -self.q3) self.assertFalse(self.q1 == self.q2) def test_avgQuaternions(self): qs = [self.q5, self.q6, self.q7] average = Quaternion.average(qs) e_avg = average.toEulers() t_avg = np.array([42.12201286, 0.0, 0.0]) target = np.array([0.9331983, 0.000000, 0.000000, 0.3593618]) np.testing.assert_almost_equal(average.tondarray(), target) np.testing.assert_almost_equal(e_avg, t_avg)