test_FrobeniusFactor.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 FrobeniusFactor unit tests.
9 Author: Frank Dellaert
10 """
11 # pylint: disable=no-name-in-module, import-error, invalid-name
12 import unittest
13 
14 import numpy as np
15 from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
16  ShonanFactor3, SOn)
17 
18 id = SO4()
19 v1 = np.array([0, 0, 0, 0.1, 0, 0])
20 Q1 = SO4.Expmap(v1)
21 v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
22 Q2 = SO4.Expmap(v2)
23 
24 
25 class TestFrobeniusFactorSO4(unittest.TestCase):
26  """Test FrobeniusFactor factors."""
27 
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)
34 
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)
41 
43  """Test creation of a factor that calculates Shonan error."""
44  R1 = SO3.Expmap(v1[3:])
45  R2 = SO3.Expmap(v2[3:])
46  factor = ShonanFactor3(1, 2, Rot3(R1.between(R2).matrix()), p=4)
47  I4 = SOn(4)
48  Q1 = I4.retract(v1)
49  Q2 = I4.retract(v2)
50  actual = factor.evaluateError(Q1, Q2)
51  expected = np.zeros((12,))
52  np.testing.assert_array_almost_equal(actual, expected, decimal=4)
53 
54 
55 if __name__ == "__main__":
56  unittest.main()
SO< 4 > SO4
Definition: SO4.h:34
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType reshape(const Eigen::Matrix< double, InM, InN, InOptions > &m)
Definition: base/Matrix.h:281
ShonanFactor< 3 > ShonanFactor3
Definition: ShonanFactor.h:90
SO< Eigen::Dynamic > SOn
Definition: SOn.h:342
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)


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