2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
18 v1 = np.array([0, 0, 0, .1, 0, 0])
19 v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
25 """Test selected SO4 methods."""
28 """Construct from matrix."""
31 self.assertIsInstance(so4, SO4)
34 """Test retraction to manifold."""
35 v = np.zeros((6,), float)
36 actual = I4.retract(v)
37 self.assertTrue(actual.equals(I4, 1e-9))
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)
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))
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])
55 np.testing.assert_array_equal(actual, expected)
58 if __name__ ==
"__main__":