2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 FrobeniusFactor unit tests.
15 from gtsam
import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
19 v1 = np.array([0, 0, 0, 0.1, 0, 0])
21 v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
26 """Test FrobeniusFactor factors."""
29 """Test creation of a factor that calculates the Frobenius norm."""
30 factor = FrobeniusFactorSO4(1, 2)
31 actual = factor.evaluateError(Q1, Q2)
32 expected = (Q2.matrix()-Q1.matrix()).transpose().
reshape((16,))
33 np.testing.assert_array_equal(actual, expected)
36 """Test creation of a Frobenius BetweenFactor."""
37 factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
38 actual = factor.evaluateError(Q1, Q2)
39 expected = np.zeros((16,))
40 np.testing.assert_array_almost_equal(actual, expected)
43 """Test creation of a factor that calculates Shonan error."""
44 R1 = SO3.Expmap(v1[3:])
45 R2 = SO3.Expmap(v2[3:])
50 actual = factor.evaluateError(Q1, Q2)
51 expected = np.zeros((12,))
52 np.testing.assert_array_almost_equal(actual, expected, decimal=4)
55 if __name__ ==
"__main__":