test_custom_factor.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 CustomFactor unit tests.
9 Author: Fan Jiang
10 """
11 import unittest
12 from typing import List
13 
14 import numpy as np
15 from gtsam.utils.test_case import GtsamTestCase
16 
17 import gtsam
18 from gtsam import CustomFactor, Pose2, Values
19 
20 
22 
23  def test_new(self):
24  """Test the creation of a new CustomFactor"""
25 
26  def error_func(this: CustomFactor, v: gtsam.Values,
27  H: List[np.ndarray]):
28  """Minimal error function stub"""
29  return np.array([1, 0, 0]), H
30 
31  noise_model = gtsam.noiseModel.Unit.Create(3)
32  cf = CustomFactor(noise_model, [0], error_func)
33 
34  def test_new_keylist(self):
35  """Test the creation of a new CustomFactor"""
36 
37  def error_func(this: CustomFactor, v: gtsam.Values,
38  H: List[np.ndarray]):
39  """Minimal error function stub"""
40  return np.array([1, 0, 0])
41 
42  noise_model = gtsam.noiseModel.Unit.Create(3)
43  cf = CustomFactor(noise_model, [0], error_func)
44 
45  def test_call(self):
46  """Test if calling the factor works (only error)"""
47  expected_pose = Pose2(1, 1, 0)
48 
49  def error_func(this: CustomFactor, v: gtsam.Values,
50  H: List[np.ndarray]) -> np.ndarray:
51  """Minimal error function with no Jacobian"""
52  key0 = this.keys()[0]
53  error = -v.atPose2(key0).localCoordinates(expected_pose)
54  return error
55 
56  noise_model = gtsam.noiseModel.Unit.Create(3)
57  cf = CustomFactor(noise_model, [0], error_func)
58  v = Values()
59  v.insert(0, Pose2(1, 0, 0))
60  e = cf.error(v)
61 
62  self.assertEqual(e, 0.5)
63 
64  def test_jacobian(self):
65  """Tests if the factor result matches the GTSAM Pose2 unit test"""
66 
67  gT1 = Pose2(1, 2, np.pi / 2)
68  gT2 = Pose2(-1, 4, np.pi)
69 
70  expected = Pose2(2, 2, np.pi / 2)
71 
72  def error_func(this: CustomFactor, v: gtsam.Values,
73  H: List[np.ndarray]):
74  """
75  the custom error function. One can freely use variables captured
76  from the outside scope. Or the variables can be acquired by indexing `v`.
77  Jacobian is passed by modifying the H array of numpy matrices.
78  """
79 
80  key0 = this.keys()[0]
81  key1 = this.keys()[1]
82  gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
83  error = expected.localCoordinates(gT1.between(gT2))
84 
85  if H is not None:
86  result = gT1.between(gT2)
87  H[0] = -result.inverse().AdjointMap()
88  H[1] = np.eye(3)
89  return error
90 
91  noise_model = gtsam.noiseModel.Unit.Create(3)
92  cf = CustomFactor(noise_model, [0, 1], error_func)
93  v = Values()
94  v.insert(0, gT1)
95  v.insert(1, gT2)
96 
97  bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
98 
99  gf = cf.linearize(v)
100  gf_b = bf.linearize(v)
101 
102  J_cf, b_cf = gf.jacobian()
103  J_bf, b_bf = gf_b.jacobian()
104  np.testing.assert_allclose(J_cf, J_bf)
105  np.testing.assert_allclose(b_cf, b_bf)
106 
107  def test_printing(self):
108  """Tests if the factor result matches the GTSAM Pose2 unit test"""
109  gT1 = Pose2(1, 2, np.pi / 2)
110  gT2 = Pose2(-1, 4, np.pi)
111 
112  def error_func(this: CustomFactor, v: gtsam.Values,
113  _: List[np.ndarray]):
114  """Minimal error function stub"""
115  return np.array([1, 0, 0])
116 
117  noise_model = gtsam.noiseModel.Unit.Create(3)
118  from gtsam.symbol_shorthand import X
119  cf = CustomFactor(noise_model, [X(0), X(1)], error_func)
120 
121  cf_string = """CustomFactor on x0, x1
122  noise model: unit (3)
123 """
124  self.assertEqual(cf_string, repr(cf))
125 
126  def test_no_jacobian(self):
127  """Tests that we will not calculate the Jacobian if not requested"""
128 
129  gT1 = Pose2(1, 2, np.pi / 2)
130  gT2 = Pose2(-1, 4, np.pi)
131 
132  expected = Pose2(2, 2, np.pi / 2)
133 
134  def error_func(this: CustomFactor, v: gtsam.Values,
135  H: List[np.ndarray]):
136  """
137  Error function that mimics a BetweenFactor
138  :param this: reference to the current CustomFactor being evaluated
139  :param v: Values object
140  :param H: list of references to the Jacobian arrays
141  :return: the non-linear error
142  """
143  key0 = this.keys()[0]
144  key1 = this.keys()[1]
145  gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
146  error = expected.localCoordinates(gT1.between(gT2))
147 
148  self.assertTrue(
149  H is None) # Should be true if we only request the error
150 
151  if H is not None:
152  result = gT1.between(gT2)
153  H[0] = -result.inverse().AdjointMap()
154  H[1] = np.eye(3)
155  return error
156 
157  noise_model = gtsam.noiseModel.Unit.Create(3)
158  cf = CustomFactor(noise_model, [0, 1], error_func)
159  v = Values()
160  v.insert(0, gT1)
161  v.insert(1, gT2)
162 
163  bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
164 
165  e_cf = cf.error(v)
166  e_bf = bf.error(v)
167  np.testing.assert_allclose(e_cf, e_bf)
168 
169  def test_optimization(self):
170  """Tests if a factor graph with a CustomFactor can be properly optimized"""
171  gT1 = Pose2(1, 2, np.pi / 2)
172  gT2 = Pose2(-1, 4, np.pi)
173 
174  expected = Pose2(2, 2, np.pi / 2)
175 
176  def error_func(this: CustomFactor, v: gtsam.Values,
177  H: List[np.ndarray]):
178  """
179  Error function that mimics a BetweenFactor
180  :param this: reference to the current CustomFactor being evaluated
181  :param v: Values object
182  :param H: list of references to the Jacobian arrays
183  :return: the non-linear error
184  """
185  key0 = this.keys()[0]
186  key1 = this.keys()[1]
187  gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
188  error = expected.localCoordinates(gT1.between(gT2))
189 
190  if H is not None:
191  result = gT1.between(gT2)
192  H[0] = -result.inverse().AdjointMap()
193  H[1] = np.eye(3)
194  return error
195 
196  noise_model = gtsam.noiseModel.Unit.Create(3)
197  cf = CustomFactor(noise_model, [0, 1], error_func)
198 
200  fg.add(cf)
201  fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
202 
203  v = Values()
204  v.insert(0, Pose2(0, 0, 0))
205  v.insert(1, Pose2(0, 0, 0))
206 
208  optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params)
209  result = optimizer.optimize()
210 
211  self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5)
212  self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
213 
214 
215 if __name__ == "__main__":
216  unittest.main()
test_custom_factor.TestCustomFactor.test_new_keylist
def test_new_keylist(self)
Definition: test_custom_factor.py:34
gtsam::CustomFactor
Definition: CustomFactor.h:45
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
X
#define X
Definition: icosphere.cpp:20
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
test_custom_factor.TestCustomFactor.test_printing
def test_printing(self)
Definition: test_custom_factor.py:107
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
test_custom_factor.TestCustomFactor.test_call
def test_call(self)
Definition: test_custom_factor.py:45
test_custom_factor.TestCustomFactor
Definition: test_custom_factor.py:21
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::utils.test_case
Definition: test_case.py:1
test_custom_factor.TestCustomFactor.test_optimization
def test_optimization(self)
Definition: test_custom_factor.py:169
test_custom_factor.TestCustomFactor.test_new
def test_new(self)
Definition: test_custom_factor.py:23
gtsam::Values
Definition: Values.h:65
test_custom_factor.TestCustomFactor.test_no_jacobian
def test_no_jacobian(self)
Definition: test_custom_factor.py:126
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Pose2
Definition: Pose2.h:39
repr
str repr(handle h)
Definition: pytypes.h:2467
test_custom_factor.TestCustomFactor.test_jacobian
def test_jacobian(self)
Definition: test_custom_factor.py:64


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:05