2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
21 """Test various GTSAM utilities."""
24 """Test createKeyList."""
27 self.assertEqual(kl.size(), 3)
30 self.assertEqual(kl.size(), 3)
33 """Tests for KeyList iteration"""
37 self.assertEqual(
len(kl),
len(I))
39 for i, key
in enumerate(kl):
40 self.assertTrue(key
in kl)
41 self.assertEqual(I[i], key)
44 """Test createKeyVector."""
47 self.assertEqual(
len(kl), 3)
50 self.assertEqual(
len(kl), 3)
53 """Tests for KeyVector iteration"""
57 self.assertEqual(
len(kv),
len(I))
59 for i, key
in enumerate(kv):
60 self.assertTrue(key
in kv)
61 self.assertEqual(I[i], key)
64 """Test createKeySet."""
67 self.assertEqual(kl.size(), 3)
70 self.assertEqual(kl.size(), 3)
73 """Tests for KeySet iteration"""
77 self.assertEqual(
len(ks),
len(I))
79 for i, key
in enumerate(ks):
80 self.assertTrue(key
in ks)
81 self.assertEqual(I[i], key)
84 """Test extractPoint2."""
88 initial.insert(2, point2)
90 point2.reshape(-1, 2))
93 """Test extractPoint3."""
97 initial.insert(2, point3)
99 point3.reshape(-1, 3))
102 """Test allPose2s."""
109 self.assertEqual(result.size(), 2)
112 """Test extractPose2."""
114 pose2 = np.asarray((0.0, 0.1, 0.1))
119 pose2.reshape(-1, 3))
122 """Test allPose3s."""
129 self.assertEqual(result.size(), 2)
132 """Test extractPose3."""
134 pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.])
138 pose3.reshape(-1, 12))
141 """Test perturbPoint2."""
147 not np.allclose(values.atPoint2(1),
gtsam.Point2(1, 1)))
150 """Test perturbPose2."""
155 self.assertTrue(values.atPose2(0) !=
gtsam.Pose2())
158 """Test perturbPoint3."""
162 values.insert(1, point3)
164 self.assertTrue(
not np.allclose(values.atPoint3(1), point3))
167 """Test insertBackprojections."""
171 values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
173 np.testing.assert_allclose(values.atPoint3(0),
177 """Test insertProjectionFactors."""
180 graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
182 self.assertEqual(graph.size(), 2)
186 graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
189 self.assertEqual(graph.size(), 2)
192 """Test reprojectionErrors."""
193 pixels = np.asarray([[20, 30], [20, 30]])
204 np.testing.assert_allclose(errors, np.zeros((2, 2)))
207 """Test localToWorld."""
216 np.testing.assert_allclose(world.atPoint2(0), expected_point2)
217 np.testing.assert_allclose(
218 world.atPose2(1).
matrix(), expected_pose2.matrix())
222 np.testing.assert_allclose(
223 world.atPose2(1).
matrix(), expected_pose2.matrix())
226 self.assertRaises(RuntimeError, world.atPoint2, 0)
229 if __name__ ==
"__main__":