text: """
  GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
  Atlanta, Georgia 30332-0415
  All Rights Reserved
  
  See LICENSE for the license information
  
  Dynamic SO(n) unit tests.
  Author: Frank Dellaert
  """
  # pylint: disable=no-name-in-module, import-error
  import unittest
  
  import numpy as np
  from gtsam import SOn
  
  I4 = SOn(4)
  v1 = np.array([0, 0, 0, .1, 0, 0])
  v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
  Q1 = I4.retract(v1)
  Q2 = I4.retract(v2)
  
  
  class TestSO4(unittest.TestCase):
      """Test selected SOn methods."""
  
      def test_constructor(self):
          """Construct from matrix."""
          matrix = np.eye(4)
          so4 = SOn.FromMatrix(matrix)
          self.assertIsInstance(so4, SOn)
  
      def test_retract(self):
          """Test retraction to manifold."""
          v = np.zeros((6,), float)
          actual = I4.retract(v)
          self.assertTrue(actual.equals(I4, 1e-9))
  
      def test_local(self):
          """Check localCoordinates for trivial case."""
          v0 = np.zeros((6,), float)
          actual = I4.localCoordinates(I4)
          np.testing.assert_array_almost_equal(actual, v0)
  
      def test_compose(self):
          """Check compose works in subgroup."""
          expected = I4.retract(2*v1)
          actual = Q1.compose(Q1)
          self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
  
      def test_vec(self):
          """Check wrapping of vec."""
          expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
          actual = I4.vec()
          np.testing.assert_array_equal(actual, expected)
  
  
  if __name__ == "__main__":
      unittest.main()
  