2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 CustomFactor unit tests.
12 from typing
import List
18 from gtsam
import CustomFactor, Pose2, Values
24 """Test the creation of a new CustomFactor"""
28 """Minimal error function stub"""
29 return np.array([1, 0, 0]), H
35 """Test the creation of a new CustomFactor"""
39 """Minimal error function stub"""
40 return np.array([1, 0, 0])
46 """Test if calling the factor works (only error)"""
47 expected_pose =
Pose2(1, 1, 0)
50 H: List[np.ndarray]) -> np.ndarray:
51 """Minimal error function with no Jacobian"""
53 error = -v.atPose2(key0).localCoordinates(expected_pose)
59 v.insert(0,
Pose2(1, 0, 0))
62 self.assertEqual(e, 0.5)
65 """Tests if the factor result matches the GTSAM Pose2 unit test"""
67 gT1 =
Pose2(1, 2, np.pi / 2)
68 gT2 =
Pose2(-1, 4, np.pi)
70 expected =
Pose2(2, 2, np.pi / 2)
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.
82 gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
83 error = expected.localCoordinates(gT1.between(gT2))
86 result = gT1.between(gT2)
87 H[0] = -result.inverse().AdjointMap()
97 bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
100 gf_b = bf.linearize(v)
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)
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)
113 _: List[np.ndarray]):
114 """Minimal error function stub"""
115 return np.array([1, 0, 0])
121 cf_string =
"""CustomFactor on x0, x1
122 noise model: unit (3)
124 self.assertEqual(cf_string,
repr(cf))
127 """Tests that we will not calculate the Jacobian if not requested"""
129 gT1 =
Pose2(1, 2, np.pi / 2)
130 gT2 =
Pose2(-1, 4, np.pi)
132 expected =
Pose2(2, 2, np.pi / 2)
135 H: List[np.ndarray]):
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
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))
152 result = gT1.between(gT2)
153 H[0] = -result.inverse().AdjointMap()
163 bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
167 np.testing.assert_allclose(e_cf, e_bf)
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)
174 expected =
Pose2(2, 2, np.pi / 2)
177 H: List[np.ndarray]):
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
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))
191 result = gT1.between(gT2)
192 H[0] = -result.inverse().AdjointMap()
201 fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
204 v.insert(0,
Pose2(0, 0, 0))
205 v.insert(1,
Pose2(0, 0, 0))
209 result = optimizer.optimize()
215 if __name__ ==
"__main__":