test_SO4.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 SO4 unit tests.
9 Author: Frank Dellaert
10 """
11 # pylint: disable=no-name-in-module, import-error
12 import unittest
13 
14 import numpy as np
15 from gtsam import SO4
16 
17 I4 = SO4()
18 v1 = np.array([0, 0, 0, .1, 0, 0])
19 v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
20 Q1 = SO4.Expmap(v1)
21 Q2 = SO4.Expmap(v2)
22 
23 
24 class TestSO4(unittest.TestCase):
25  """Test selected SO4 methods."""
26 
27  def test_constructor(self):
28  """Construct from matrix."""
29  matrix = np.eye(4)
30  so4 = SO4(matrix)
31  self.assertIsInstance(so4, SO4)
32 
33  def test_retract(self):
34  """Test retraction to manifold."""
35  v = np.zeros((6,), float)
36  actual = I4.retract(v)
37  self.assertTrue(actual.equals(I4, 1e-9))
38 
39  def test_local(self):
40  """Check localCoordinates for trivial case."""
41  v0 = np.zeros((6,), float)
42  actual = I4.localCoordinates(I4)
43  np.testing.assert_array_almost_equal(actual, v0)
44 
45  def test_compose(self):
46  """Check compose works in subgroup."""
47  expected = SO4.Expmap(2*v1)
48  actual = Q1.compose(Q1)
49  self.assertTrue(actual.equals(expected, 1e-9))
50 
51  def test_vec(self):
52  """Check wrapping of vec."""
53  expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
54  actual = I4.vec()
55  np.testing.assert_array_equal(actual, expected)
56 
57 
58 if __name__ == "__main__":
59  unittest.main()
SO< 4 > SO4
Definition: SO4.h:34
def test_retract(self)
Definition: test_SO4.py:33
def test_local(self)
Definition: test_SO4.py:39
def test_vec(self)
Definition: test_SO4.py:51
def test_constructor(self)
Definition: test_SO4.py:27
def test_compose(self)
Definition: test_SO4.py:45


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:46