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()
gtsam::SO4
SO< 4 > SO4
Definition: SO4.h:34
gtsam::reshape
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType reshape(const Eigen::Matrix< double, InM, InN, InOptions > &m)
Definition: base/Matrix.h:281
test_FrobeniusFactor.TestFrobeniusFactorSO4.test_frobenius_wormhole_factor
def test_frobenius_wormhole_factor(self)
Definition: test_FrobeniusFactor.py:42
test_FrobeniusFactor.TestFrobeniusFactorSO4
Definition: test_FrobeniusFactor.py:25
gtsam::SOn
SO< Eigen::Dynamic > SOn
Definition: SOn.h:342
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
test_FrobeniusFactor.TestFrobeniusFactorSO4.test_frobenius_factor
def test_frobenius_factor(self)
Definition: test_FrobeniusFactor.py:28
gtsam::ShonanFactor3
ShonanFactor< 3 > ShonanFactor3
Definition: ShonanFactor.h:90
test_FrobeniusFactor.TestFrobeniusFactorSO4.test_frobenius_between_factor
def test_frobenius_between_factor(self)
Definition: test_FrobeniusFactor.py:35


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:27