60 lines
1.6 KiB
Python
60 lines
1.6 KiB
Python
"""
|
|
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,), np.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,), np.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()
|