test_JacobianFactor.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 JacobianFactor unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 import unittest
12 
13 import numpy as np
14 
15 import gtsam
16 from gtsam.utils.test_case import GtsamTestCase
17 
18 
20 
21  def test_eliminate(self):
22  # Recommended way to specify a matrix (see python/README)
23  Ax2 = np.array(
24  [[-5., 0.],
25  [+0., -5.],
26  [10., 0.],
27  [+0., 10.]], order='F')
28 
29  # This is good too
30  Al1 = np.array(
31  [[5, 0],
32  [0, 5],
33  [0, 0],
34  [0, 0]], dtype=float, order = 'F')
35 
36  # Not recommended for performance reasons, but should still work
37  # as the wrapper should convert it to the correct type and storage order
38  Ax1 = np.array(
39  [[0, 0], # f4
40  [0, 0], # f4
41  [-10, 0], # f2
42  [0, -10]]) # f2
43 
44  x2 = 1
45  l1 = 2
46  x1 = 3
47 
48  # the RHS
49  b2 = np.array([-1., 1.5, 2., -1.])
50  sigmas = np.array([1., 1., 1., 1.])
51  model4 = gtsam.noiseModel.Diagonal.Sigmas(sigmas)
52  combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
53 
54  # eliminate the first variable (x2) in the combined factor, destructive
55  # !
56  ord = gtsam.Ordering()
57  ord.push_back(x2)
58  actualCG, lf = combined.eliminate(ord)
59 
60  # create expected Conditional Gaussian
61  R11 = np.array([[11.1803, 0.00],
62  [0.00, 11.1803]])
63  S12 = np.array([[-2.23607, 0.00],
64  [+0.00, -2.23607]])
65  S13 = np.array([[-8.94427, 0.00],
66  [+0.00, -8.94427]])
67  d = np.array([2.23607, -1.56525])
68  expectedCG = gtsam.GaussianConditional(
69  x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2))
70  # check if the result matches
71  self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
72 
73  # the expected linear factor
74  Bl1 = np.array([[4.47214, 0.00],
75  [0.00, 4.47214]])
76 
77  Bx1 = np.array(
78  # x1
79  [[-4.47214, 0.00],
80  [+0.00, -4.47214]])
81 
82  # the RHS
83  b1 = np.array([0.0, 0.894427])
84 
85  model2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([1., 1.]))
86  expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
87 
88  # check if the result matches the combined (reduced) factor
89  self.gtsamAssertEquals(lf, expectedLF, 1e-4)
90 
91 if __name__ == "__main__":
92  unittest.main()
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270


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