test_FundamentalMatrix.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 FundamentalMatrix unit tests.
9 Author: Frank Dellaert
10 """
11 
12 # pylint: disable=no-name-in-module
13 import unittest
14 
15 import numpy as np
16 from gtsam.examples import SFMdata
17 from numpy.testing import assert_almost_equal
18 
19 import gtsam
20 from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix,
21  PinholeCameraCal3_S2, Point2, Point3, Rot3,
22  SimpleFundamentalMatrix, Unit3)
23 
24 
25 class TestFundamentalMatrix(unittest.TestCase):
26 
27  def setUp(self):
28  # Create two rotations and corresponding fundamental matrix F
29  self.trueU = Rot3.Yaw(np.pi / 2)
30  self.trueV = Rot3.Yaw(np.pi / 4)
31  self.trueS = 0.5
32  self.trueF = FundamentalMatrix(self.trueU.matrix(), self.trueS, self.trueV.matrix())
33 
35  expected = np.zeros(7) # Assuming 7 dimensions for U, V, and s
36  actual = self.trueF.localCoordinates(self.trueF)
37  assert_almost_equal(expected, actual, decimal=8)
38 
39  def test_retract(self):
40  actual = self.trueF.retract(np.zeros(7))
41  self.assertTrue(self.trueF.equals(actual, 1e-9))
42 
43  def test_RoundTrip(self):
44  d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
45  hx = self.trueF.retract(d)
46  actual = self.trueF.localCoordinates(hx)
47  assert_almost_equal(d, actual, decimal=8)
48 
49 
50 class TestSimpleStereo(unittest.TestCase):
51 
52  def setUp(self):
53  # Create the simplest SimpleFundamentalMatrix, a stereo pair
54  self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
55  self.zero = Point2(0.0, 0.0)
56  self.stereoF = SimpleFundamentalMatrix(self.defaultE, 1.0, 1.0, self.zero, self.zero)
57 
58  def test_Conversion(self):
59  convertedF = FundamentalMatrix(self.stereoF.matrix())
60  assert_almost_equal(self.stereoF.matrix(), convertedF.matrix(), decimal=8)
61 
63  expected = np.zeros(7)
64  actual = self.stereoF.localCoordinates(self.stereoF)
65  assert_almost_equal(expected, actual, decimal=8)
66 
67  def test_retract(self):
68  actual = self.stereoF.retract(np.zeros(9))
69  self.assertTrue(self.stereoF.equals(actual, 1e-9))
70 
71  def test_RoundTrip(self):
72  d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
73  hx = self.stereoF.retract(d)
74  actual = self.stereoF.localCoordinates(hx)
75  assert_almost_equal(d, actual, decimal=8)
76 
77  def test_EpipolarLine(self):
78  # Create a point in b
79  p_b = np.array([0, 2, 1])
80  # Convert the point to a horizontal line in a
81  l_a = self.stereoF.matrix() @ p_b
82  # Check if the line is horizontal at height 2
83  expected = np.array([0, -1, 2])
84  assert_almost_equal(l_a, expected, decimal=8)
85 
86 
87 class TestPixelStereo(unittest.TestCase):
88 
89  def setUp(self):
90  # Create a stereo pair in pixels, zero principal points
91  self.focalLength = 1000.0
92  self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0))
93  self.zero = Point2(0.0, 0.0)
95  self.defaultE, self.focalLength, self.focalLength, self.zero, self.zero
96  )
97 
98  def test_Conversion(self):
99  expected = self.pixelStereo.matrix()
100  convertedF = FundamentalMatrix(self.pixelStereo.matrix())
101  # Check equality of F-matrices up to a scale
102  actual = convertedF.matrix()
103  scale = expected[1, 2] / actual[1, 2]
104  actual *= scale
105  assert_almost_equal(expected, actual, decimal=5)
106 
108  # Create a point in b
109  p_b = np.array([0, 300, 1])
110  # Convert the point to a horizontal line in a
111  l_a = self.pixelStereo.matrix() @ p_b
112  # Check if the line is horizontal at height 0.3
113  expected = np.array([0, -0.001, 0.3])
114  assert_almost_equal(l_a, expected, decimal=8)
115 
116 
117 class TestRotatedPixelStereo(unittest.TestCase):
118 
119  def setUp(self):
120  # Create a stereo pair with the right camera rotated 90 degrees
121  self.focalLength = 1000.0
122  self.zero = Point2(0.0, 0.0)
123  self.aRb = Rot3.Rz(np.pi / 2) # Rotate 90 degrees around the Z-axis
124  self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0))
126  self.rotatedE, self.focalLength, self.focalLength, self.zero, self.zero
127  )
128 
129  def test_Conversion(self):
130  expected = self.rotatedPixelStereo.matrix()
131  convertedF = FundamentalMatrix(self.rotatedPixelStereo.matrix())
132  # Check equality of F-matrices up to a scale
133  actual = convertedF.matrix()
134  scale = expected[1, 2] / actual[1, 2]
135  actual *= scale
136  assert_almost_equal(expected, actual, decimal=4)
137 
139  # Create a point in b
140  p_b = np.array([300, 0, 1])
141  # Convert the point to a horizontal line in a
142  l_a = self.rotatedPixelStereo.matrix() @ p_b
143  # Check if the line is horizontal at height 0.3
144  expected = np.array([0, -0.001, 0.3])
145  assert_almost_equal(l_a, expected, decimal=8)
146 
147 
148 class TestStereoWithPrincipalPoints(unittest.TestCase):
149 
150  def setUp(self):
151  # Now check that principal points also survive conversion
152  self.focalLength = 1000.0
153  self.principalPoint = Point2(640 / 2, 480 / 2)
154  self.aRb = Rot3.Rz(np.pi / 2)
155  self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0))
157  self.rotatedE, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
158  )
159 
160  def test_Conversion(self):
161  expected = self.stereoWithPrincipalPoints.matrix()
163  # Check equality of F-matrices up to a scale
164  actual = convertedF.matrix()
165  scale = expected[1, 2] / actual[1, 2]
166  actual *= scale
167  assert_almost_equal(expected, actual, decimal=4)
168 
169 
170 class TestTripleF(unittest.TestCase):
171 
172  def setUp(self):
173  # Generate three cameras on a circle, looking in
174  self.cameraPoses = SFMdata.posesOnCircle(3, 1.0)
175  self.focalLength = 1000.0
176  self.principalPoint = Point2(640 / 2, 480 / 2)
178 
179  def generateTripleF(self, cameraPoses):
180  F = []
181  for i in range(3):
182  j = (i + 1) % 3
183  iPj = cameraPoses[i].between(cameraPoses[j])
184  E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation()))
186  E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
187  )
188  F.append(F_ij)
189  return {"Fab": F[0], "Fbc": F[1], "Fca": F[2]}
190 
191  def transferToA(self, pb, pc):
192  return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix(), pb, self.triplet["Fca"].matrix().transpose(), pc)
193 
194  def transferToB(self, pa, pc):
195  return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix().transpose(), pa, self.triplet["Fbc"].matrix(), pc)
196 
197  def transferToC(self, pa, pb):
198  return gtsam.EpipolarTransfer(self.triplet["Fca"].matrix(), pa, self.triplet["Fbc"].matrix().transpose(), pb)
199 
200  def test_Transfer(self):
201  triplet = self.triplet
202  # Check that they are all equal
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))
206 
207  # Now project a point into the three cameras
208  P = Point3(0.1, 0.2, 0.3)
209  K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1])
210 
211  p = []
212  for i in range(3):
213  # Project the point into each camera
214  camera = PinholeCameraCal3_S2(self.cameraPoses[i], K)
215  p_i = camera.project(P)
216  p.append(p_i)
217 
218  # Check that transfer works
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)
222 
223 
224 class TestManyCamerasCircle(unittest.TestCase):
225  N = 6
226 
227  def setUp(self):
228  # Generate six cameras on a circle, looking in
229  self.cameraPoses = SFMdata.posesOnCircle(self.N, 1.0)
230  self.focalLength = 1000.0
231  self.principalPoint = Point2(640 / 2, 480 / 2)
233 
234  def generateManyFs(self, cameraPoses):
235  F = []
236  for i in range(self.N):
237  j = (i + 1) % self.N
238  iPj = cameraPoses[i].between(cameraPoses[j])
239  E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation()))
241  E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint
242  )
243  F.append(F_ij)
244  return F
245 
246  def test_Conversion(self):
247  for i in range(self.N):
248  expected = self.manyFs[i].matrix()
249  convertedF = FundamentalMatrix(self.manyFs[i].matrix())
250  # Check equality of F-matrices up to a scale
251  actual = convertedF.matrix()
252  scale = expected[1, 2] / actual[1, 2]
253  actual *= scale
254  # print(f"\n{np.round(expected, 3)}", f"\n{np.round(actual, 3)}")
255  assert_almost_equal(expected, actual, decimal=4)
256 
257  def test_Transfer(self):
258  # Now project a point into the six cameras
259  P = Point3(0.1, 0.2, 0.3)
260  K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1])
261 
262  p = []
263  for i in range(self.N):
264  # Project the point into each camera
265  camera = PinholeCameraCal3_S2(self.cameraPoses[i], K)
266  p_i = camera.project(P)
267  p.append(p_i)
268 
269  # Check that transfer works
270  for a in range(self.N):
271  b = (a + 1) % self.N
272  c = (a + 2) % self.N
273  # We transfer from a to b and from c to b,
274  # and check that the result lines up with the projected point in b.
275  transferred = gtsam.EpipolarTransfer(
276  self.manyFs[a].matrix().transpose(), # need to transpose for a->b
277  p[a],
278  self.manyFs[c].matrix(),
279  p[c],
280  )
281  assert_almost_equal(p[b], transferred, decimal=9)
282 
283 
284 if __name__ == "__main__":
285  unittest.main()
test_FundamentalMatrix.TestPixelStereo.pixelStereo
pixelStereo
Definition: test_FundamentalMatrix.py:94
test_FundamentalMatrix.TestRotatedPixelStereo.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:119
test_FundamentalMatrix.TestTripleF.triplet
triplet
Definition: test_FundamentalMatrix.py:177
test_FundamentalMatrix.TestManyCamerasCircle.test_Transfer
def test_Transfer(self)
Definition: test_FundamentalMatrix.py:257
test_FundamentalMatrix.TestRotatedPixelStereo.test_Conversion
def test_Conversion(self)
Definition: test_FundamentalMatrix.py:129
test_FundamentalMatrix.TestRotatedPixelStereo.rotatedE
rotatedE
Definition: test_FundamentalMatrix.py:124
test_FundamentalMatrix.TestManyCamerasCircle.principalPoint
principalPoint
Definition: test_FundamentalMatrix.py:231
test_FundamentalMatrix.TestRotatedPixelStereo.zero
zero
Definition: test_FundamentalMatrix.py:122
test_FundamentalMatrix.TestTripleF.cameraPoses
cameraPoses
Definition: test_FundamentalMatrix.py:174
test_FundamentalMatrix.TestStereoWithPrincipalPoints.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:150
test_FundamentalMatrix.TestPixelStereo.zero
zero
Definition: test_FundamentalMatrix.py:93
test_FundamentalMatrix.TestSimpleStereo.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:52
test_FundamentalMatrix.TestPixelStereo.defaultE
defaultE
Definition: test_FundamentalMatrix.py:92
test_FundamentalMatrix.TestStereoWithPrincipalPoints.test_Conversion
def test_Conversion(self)
Definition: test_FundamentalMatrix.py:160
test_FundamentalMatrix.TestPixelStereo.focalLength
focalLength
Definition: test_FundamentalMatrix.py:91
test_FundamentalMatrix.TestTripleF.generateTripleF
def generateTripleF(self, cameraPoses)
Definition: test_FundamentalMatrix.py:179
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam.examples
Definition: python/gtsam/examples/__init__.py:1
gtsam::EpipolarTransfer
Point2 EpipolarTransfer(const Matrix3 &Fca, const Point2 &pa, const Matrix3 &Fcb, const Point2 &pb)
Transfer projections from cameras a and b to camera c.
Definition: FundamentalMatrix.cpp:15
test_FundamentalMatrix.TestSimpleStereo
Definition: test_FundamentalMatrix.py:50
test_FundamentalMatrix.TestFundamentalMatrix.trueS
trueS
Definition: test_FundamentalMatrix.py:31
test_FundamentalMatrix.TestManyCamerasCircle.manyFs
manyFs
Definition: test_FundamentalMatrix.py:232
test_FundamentalMatrix.TestSimpleStereo.defaultE
defaultE
Definition: test_FundamentalMatrix.py:54
test_FundamentalMatrix.TestPixelStereo.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:89
test_FundamentalMatrix.TestRotatedPixelStereo.test_PointInBToHorizontalLineInA
def test_PointInBToHorizontalLineInA(self)
Definition: test_FundamentalMatrix.py:138
test_FundamentalMatrix.TestTripleF.focalLength
focalLength
Definition: test_FundamentalMatrix.py:175
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
test_FundamentalMatrix.TestManyCamerasCircle.generateManyFs
def generateManyFs(self, cameraPoses)
Definition: test_FundamentalMatrix.py:234
test_FundamentalMatrix.TestFundamentalMatrix.test_RoundTrip
def test_RoundTrip(self)
Definition: test_FundamentalMatrix.py:43
test_FundamentalMatrix.TestFundamentalMatrix.trueU
trueU
Definition: test_FundamentalMatrix.py:29
test_FundamentalMatrix.TestStereoWithPrincipalPoints.rotatedE
rotatedE
Definition: test_FundamentalMatrix.py:155
test_FundamentalMatrix.TestFundamentalMatrix
Definition: test_FundamentalMatrix.py:25
test_FundamentalMatrix.TestStereoWithPrincipalPoints.stereoWithPrincipalPoints
stereoWithPrincipalPoints
Definition: test_FundamentalMatrix.py:156
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
test_FundamentalMatrix.TestSimpleStereo.test_Conversion
def test_Conversion(self)
Definition: test_FundamentalMatrix.py:58
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
test_FundamentalMatrix.TestStereoWithPrincipalPoints.principalPoint
principalPoint
Definition: test_FundamentalMatrix.py:153
test_FundamentalMatrix.TestSimpleStereo.test_EpipolarLine
def test_EpipolarLine(self)
Definition: test_FundamentalMatrix.py:77
test_FundamentalMatrix.TestFundamentalMatrix.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:27
test_FundamentalMatrix.TestFundamentalMatrix.test_retract
def test_retract(self)
Definition: test_FundamentalMatrix.py:39
test_FundamentalMatrix.TestPixelStereo.test_Conversion
def test_Conversion(self)
Definition: test_FundamentalMatrix.py:98
gtsam::PinholeCameraCal3_S2
gtsam::PinholeCamera< gtsam::Cal3_S2 > PinholeCameraCal3_S2
Definition: SimpleCamera.h:34
test_FundamentalMatrix.TestRotatedPixelStereo
Definition: test_FundamentalMatrix.py:117
test_FundamentalMatrix.TestTripleF.transferToB
def transferToB(self, pa, pc)
Definition: test_FundamentalMatrix.py:194
test_FundamentalMatrix.TestManyCamerasCircle.N
int N
Definition: test_FundamentalMatrix.py:225
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
test_FundamentalMatrix.TestSimpleStereo.test_RoundTrip
def test_RoundTrip(self)
Definition: test_FundamentalMatrix.py:71
test_FundamentalMatrix.TestTripleF
Definition: test_FundamentalMatrix.py:170
test_FundamentalMatrix.TestRotatedPixelStereo.aRb
aRb
Definition: test_FundamentalMatrix.py:123
test_FundamentalMatrix.TestManyCamerasCircle.test_Conversion
def test_Conversion(self)
Definition: test_FundamentalMatrix.py:246
test_FundamentalMatrix.TestTripleF.test_Transfer
def test_Transfer(self)
Definition: test_FundamentalMatrix.py:200
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
test_FundamentalMatrix.TestTripleF.transferToC
def transferToC(self, pa, pb)
Definition: test_FundamentalMatrix.py:197
test_FundamentalMatrix.TestTripleF.principalPoint
principalPoint
Definition: test_FundamentalMatrix.py:176
test_FundamentalMatrix.TestStereoWithPrincipalPoints.aRb
aRb
Definition: test_FundamentalMatrix.py:154
test_FundamentalMatrix.TestManyCamerasCircle.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:227
test_FundamentalMatrix.TestTripleF.transferToA
def transferToA(self, pb, pc)
Definition: test_FundamentalMatrix.py:191
test_FundamentalMatrix.TestSimpleStereo.zero
zero
Definition: test_FundamentalMatrix.py:55
test_FundamentalMatrix.TestManyCamerasCircle
Definition: test_FundamentalMatrix.py:224
test_FundamentalMatrix.TestRotatedPixelStereo.focalLength
focalLength
Definition: test_FundamentalMatrix.py:121
test_FundamentalMatrix.TestFundamentalMatrix.test_localCoordinates
def test_localCoordinates(self)
Definition: test_FundamentalMatrix.py:34
gtsam::SimpleFundamentalMatrix
Class for representing a simple fundamental matrix.
Definition: FundamentalMatrix.h:128
test_FundamentalMatrix.TestFundamentalMatrix.trueF
trueF
Definition: test_FundamentalMatrix.py:32
test_FundamentalMatrix.TestSimpleStereo.test_retract
def test_retract(self)
Definition: test_FundamentalMatrix.py:67
test_FundamentalMatrix.TestTripleF.setUp
def setUp(self)
Definition: test_FundamentalMatrix.py:172
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
test_FundamentalMatrix.TestManyCamerasCircle.cameraPoses
cameraPoses
Definition: test_FundamentalMatrix.py:229
test_FundamentalMatrix.TestSimpleStereo.test_localCoordinates
def test_localCoordinates(self)
Definition: test_FundamentalMatrix.py:62
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
test_FundamentalMatrix.TestStereoWithPrincipalPoints.focalLength
focalLength
Definition: test_FundamentalMatrix.py:152
test_FundamentalMatrix.TestStereoWithPrincipalPoints
Definition: test_FundamentalMatrix.py:148
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::FundamentalMatrix
Represents a fundamental matrix in computer vision, which encodes the epipolar geometry between two v...
Definition: FundamentalMatrix.h:28
test_FundamentalMatrix.TestSimpleStereo.stereoF
stereoF
Definition: test_FundamentalMatrix.py:56
test_FundamentalMatrix.TestRotatedPixelStereo.rotatedPixelStereo
rotatedPixelStereo
Definition: test_FundamentalMatrix.py:125
test_FundamentalMatrix.TestManyCamerasCircle.focalLength
focalLength
Definition: test_FundamentalMatrix.py:230
test_FundamentalMatrix.TestFundamentalMatrix.trueV
trueV
Definition: test_FundamentalMatrix.py:30
test_FundamentalMatrix.TestPixelStereo
Definition: test_FundamentalMatrix.py:87
test_FundamentalMatrix.TestPixelStereo.test_PointInBToHorizontalLineInA
def test_PointInBToHorizontalLineInA(self)
Definition: test_FundamentalMatrix.py:107


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:27