2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 FundamentalMatrix unit tests.
17 from numpy.testing
import assert_almost_equal
20 from gtsam
import (Cal3_S2, EssentialMatrix, FundamentalMatrix,
21 PinholeCameraCal3_S2, Point2, Point3, Rot3,
22 SimpleFundamentalMatrix, Unit3)
29 self.
trueU = Rot3.Yaw(np.pi / 2)
30 self.
trueV = Rot3.Yaw(np.pi / 4)
35 expected = np.zeros(7)
36 actual = self.
trueF.localCoordinates(self.
trueF)
37 assert_almost_equal(expected, actual, decimal=8)
41 self.assertTrue(self.
trueF.equals(actual, 1e-9))
44 d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
46 actual = self.
trueF.localCoordinates(hx)
47 assert_almost_equal(d, actual, decimal=8)
60 assert_almost_equal(self.
stereoF.
matrix(), convertedF.matrix(), decimal=8)
63 expected = np.zeros(7)
65 assert_almost_equal(expected, actual, decimal=8)
69 self.assertTrue(self.
stereoF.equals(actual, 1e-9))
72 d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
74 actual = self.
stereoF.localCoordinates(hx)
75 assert_almost_equal(d, actual, decimal=8)
79 p_b = np.array([0, 2, 1])
83 expected = np.array([0, -1, 2])
84 assert_almost_equal(l_a, expected, decimal=8)
102 actual = convertedF.matrix()
103 scale = expected[1, 2] / actual[1, 2]
105 assert_almost_equal(expected, actual, decimal=5)
109 p_b = np.array([0, 300, 1])
113 expected = np.array([0, -0.001, 0.3])
114 assert_almost_equal(l_a, expected, decimal=8)
123 self.
aRb = Rot3.Rz(np.pi / 2)
133 actual = convertedF.matrix()
134 scale = expected[1, 2] / actual[1, 2]
136 assert_almost_equal(expected, actual, decimal=4)
140 p_b = np.array([300, 0, 1])
144 expected = np.array([0, -0.001, 0.3])
145 assert_almost_equal(l_a, expected, decimal=8)
154 self.
aRb = Rot3.Rz(np.pi / 2)
164 actual = convertedF.matrix()
165 scale = expected[1, 2] / actual[1, 2]
167 assert_almost_equal(expected, actual, decimal=4)
183 iPj = cameraPoses[i].
between(cameraPoses[j])
189 return {
"Fab": F[0],
"Fbc": F[1],
"Fca": F[2]}
203 self.assertTrue(triplet[
"Fab"].equals(triplet[
"Fbc"], 1e-9))
204 self.assertTrue(triplet[
"Fbc"].equals(triplet[
"Fca"], 1e-9))
205 self.assertTrue(triplet[
"Fca"].equals(triplet[
"Fab"], 1e-9))
215 p_i = camera.project(P)
219 assert_almost_equal(p[0], self.
transferToA(p[1], p[2]), decimal=9)
220 assert_almost_equal(p[1], self.
transferToB(p[0], p[2]), decimal=9)
221 assert_almost_equal(p[2], self.
transferToC(p[0], p[1]), decimal=9)
238 iPj = cameraPoses[i].
between(cameraPoses[j])
251 actual = convertedF.matrix()
252 scale = expected[1, 2] / actual[1, 2]
255 assert_almost_equal(expected, actual, decimal=4)
266 p_i = camera.project(P)
281 assert_almost_equal(p[b], transferred, decimal=9)
284 if __name__ ==
"__main__":