Loading autocnet/transformation/fundamental_matrix.py +77 −15 Original line number Diff line number Diff line import warnings import numpy as np import pandas as pd from scipy import optimize from autocnet.camera import camera from autocnet.camera import utils as camera_utils Loading @@ -8,26 +9,23 @@ from autocnet.utils.utils import make_homogeneous, normalize_vector try: import cv2 cv2_avail = True except: except: # pragma: no cover cv_avail = False def compute_error(F, x, x1): def compute_reprojection_error(F, x, x1): """ Given a set of matches and a known fundamental matrix, compute distance between all match points and the associated compute distance between match points and the associated epipolar lines. Ideal error is defined by $x^{\intercal}Fx = 0$, where $x$ are all matchpoints in a given image and $x^{\intercal}F$ defines the standard form of the epipolar line in the second image. The distance between a point and the associated epipolar line is computed as: $d = \frac{\lvert ax_{0} + by_{0} + c \rvert}{\sqrt{a^{2} + b^{2}}}$. Parameters ---------- F : ndarray (3,3) Fundamental matrix x : arraylike (n,2) or (n,3) array of homogeneous coordinates Loading @@ -53,7 +51,53 @@ def compute_error(F, x, x1): return F_error def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None): def compute_fundamental_error(F, x, x1): """ Compute the fundamental error using the idealized error metric. Ideal error is defined by $x^{\intercal}Fx = 0$, where $x$ are all matchpoints in a given image and $x^{\intercal}F$ defines the standard form of the epipolar line in the second image. This method assumes that x and x1 are ordered such that x[0] correspondes to x1[0]. Parameters ---------- F : ndarray (3,3) Fundamental matrix x : arraylike (n,2) or (n,3) array of homogeneous coordinates x1 : arraylike (n,2) or (n,3) array of homogeneous coordinates with the same length as argument x Returns ------- F_error : ndarray n,1 vector of reprojection errors """ # TODO: Can this be vectorized for performance? if x.shape[1] != 3: x = make_homogeneous(x) if x1.shape[1] != 3: x1 = make_homogeneous(x1) if isinstance(x, pd.DataFrame): x = x.values if isinstance(x1, pd.DataFrame): x1 = x1.values err = np.empty(len(x)) for i in range(len(x)): err[i] = x1[i].T.dot(F).dot(x[i]) return err def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None, method='reprojection'): """ Given a Fundamental matrix and two sets of points, compute the reprojection error between x1 and x2. A mask is returned with all Loading @@ -71,7 +115,10 @@ def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None): (n,2) or (n,3) array of homogeneous coordinates threshold : float The new upper, reprojective error limit, in pixels. The new upper limit for error. If using reprojection this is measured in pixels (the default). If using fundamental, the idealized error is 0. Values +- 0.05 should be good. index : ndarray Optional index for mapping between reprojective error Loading @@ -82,10 +129,16 @@ def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None): mask : dataframe """ error = compute_error(F, x1, x2) mask = error <= threshold if method == 'reprojection': error = compute_reprojection_error(F, x1, x2) elif method == 'fundamental': error = compute_fundamental_error(F, x1, x2) else: warnings.warn('Unknown error method. Options are "reprojection" or "fundamental".') mask = pd.DataFrame(np.abs(error) <= threshold, index=index, columns=['fundamental']) if index != None: mask = pd.DataFrame(mask, index=index, columns='F_Error') mask.index = index return mask def enforce_singularity_constraint(F): Loading Loading @@ -182,13 +235,18 @@ def compute_fundamental_matrix(kp1, kp2, method='mle', reproj_threshold=2.0, if method == 'mle': # Now apply the gold standard algorithm to refine F if kp1.shape[1] != 3: kp1 = make_homogeneous(kp1) if kp2.shape[1] != 3: kp2 = make_homogeneous(kp2) # Generate an idealized and to be updated camera model p1 = camera.estimated_camera_from_f(F) p = camera.idealized_camera() # Grab the points used to estimate F pt = kp1.loc[mask] pt1 = kp2.loc[mask] pt = kp1.loc[mask].T pt1 = kp2.loc[mask].T if pt.shape[1] < 9 or pt1.shape[1] < 9: warnings.warn("Unable to apply MLE. Not enough correspondences. Returning with a RANSAC computed F matrix.") Loading @@ -206,4 +264,8 @@ def compute_fundamental_matrix(kp1, kp2, method='mle', reproj_threshold=2.0, F = gold_standard_f mask = update_fundamental_mask(F, kp1, kp2, threshold=reproj_threshold).values return F, mask autocnet/transformation/tests/test_fundamental_matrix.py +94 −14 Original line number Diff line number Diff line Loading @@ -14,24 +14,85 @@ class TestFundamentalMatrix(unittest.TestCase): @classmethod def setUpClass(cls): nbr_inliers = 20 np.random.seed(12345) fp = np.array(np.random.standard_normal((nbr_inliers, 2))) # inliers static_F = np.array([[4, 0.5, 10], [0.25, 1, 5], [0.2, 0.1, 1]]) # Make homogeneous fph = np.hstack((fp, np.ones((nbr_inliers, 1)))) tp = static_F.dot(fph.T) # normalize hom. coordinates tp /= tp[-1, :np.newaxis] tp = np.empty((nbr_inliers, 3)) for i, j in enumerate(fph): proj = j.dot(static_F) proj /= proj[2] tp[i] = proj cls.static_F = static_F cls.F = np.array([[-0.685892, -5.870193, 2.268333], [-0.704199, 12.88776, -3.040341], [-0.231815, -2.806056, 1.]]) cls.x1 = pd.DataFrame(fph, columns=['x', 'y', 'h']) cls.x2 = pd.DataFrame(tp.T, columns=['x', 'y', 'h']) cls.x2 = pd.DataFrame(tp, columns=['x', 'y', 'h']) cls.fixed_x1 = np.array([[ 438.394104 , 846.43518066, 1. ], [ 767.89105225, 380.79367065, 1.], [ 63.80842972, 815.14257812, 1. ], [ 283.96408081, 901.07287598, 1. ], [ 421.63833618, 841.66619873, 1. ], [ 181.8278656 , 706.01611328, 1. ], [ 650.27160645, 416.72653198, 1. ], [ 650.27160645, 416.72653198, 1. ], [ 721.18585205, 368.2802124 , 1. ], [ 88.97966003, 962.11322021, 1. ]]) cls.fixed_x2 = np.array([[ 652.32714844, 847.51605225, 1. ], [ 985.95928955, 384.58950806, 1. ], [ 281.5947876 , 819.9956665 , 1. ], [ 501.13912964, 904.06054688, 1. ], [ 637.31488037, 842.93652344, 1. ], [ 398.35501099, 708.86029053, 1. ], [ 875.11975098, 419.54541016, 1. ], [ 875.11975098, 419.54541016, 1. ], [ 943.69946289, 372.12527466, 1. ], [ 299.27636719, 968.44104004, 1. ]]) cls.fixed_f = np.array([[ -2.85373973e-08, 3.02728824e-06, -2.41915056e-03], [ -4.53237187e-06, 1.38905788e-07, -4.14644099e-02], [ 3.25687216e-03, 4.11777575e-02, 3.61272746e-01]]) def test_compute_f(self): # The F matrix is good if the sum of the error is within some threshold. F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='ransac') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='lmeds') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='normal') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='8point') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='mle') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) def test_compute_mle_f(self): #TODO: Write a better test for MLE the data here is too clean. pass def test_f_reprojection_error(self): err = fm.compute_reprojection_error(self.fixed_f, self.fixed_x1, self.fixed_x2) self.assertTrue(err.mean() < 0.5) def test_f_fundamental_error(self): err = fm.compute_fundamental_error(self.fixed_f, self.fixed_x1, self.fixed_x2) self.assertTrue(abs(sum(err)) < 0.03) def test_update_fundamental_mask(self): np.random.seed(12345) nbr_inliers = 20 fp = np.array(np.random.standard_normal((nbr_inliers, 2))) Loading @@ -39,19 +100,38 @@ class TestFundamentalMatrix(unittest.TestCase): F, mask = fm.compute_fundamental_matrix(fp, tp, method='ransac') np.testing.assert_array_almost_equal(F, self.F) new_mask = fm.update_fundamental_mask(F, fp, tp, threshold=0.5, method='reprojection') self.assertEqual(10, new_mask['fundamental'].sum()) def test_compute_mle_f(self): #TODO: Write a better test for MLE the data here is too clean. def test_update_fundamental_mask_with_index(self): np.random.seed(12345) nbr_inliers = 20 fp = pd.DataFrame(np.array(np.random.standard_normal((nbr_inliers, 2)))) tp = pd.DataFrame(np.array(np.random.standard_normal((nbr_inliers, 2)))) fp = np.array(np.random.standard_normal((nbr_inliers, 2))) tp = np.array(np.random.standard_normal((nbr_inliers, 2))) F, mask = fm.compute_fundamental_matrix(fp, tp, method='mle') F, mask = fm.compute_fundamental_matrix(fp, tp, method='ransac') new_index = np.arange(20)[::-1] #Just reverse the index new_mask = fm.update_fundamental_mask(F, fp, tp, threshold=0.5, index=new_index) np.testing.assert_array_equal(new_index, new_mask.index.values) np.testing.assert_array_almost_equal(F, self.F) def test_update_fundamental_mask_with_fundamental(self): new_mask = fm.update_fundamental_mask(self.fixed_f, self.fixed_x1, self.fixed_x2, threshold=0.05, method='fundamental') def test_f_error(self): #TODO: This is a stochastic process - how to test? pass self.assertTrue(new_mask['fundamental'].sum() == 10) new_mask = fm.update_fundamental_mask(self.fixed_f, self.fixed_x1, self.fixed_x2, threshold=0.005, method='fundamental') print(new_mask['fundamental'].sum()) self.assertTrue(new_mask['fundamental'].sum() == 9) def test_enforce_singularity_constraint(self): r3 = np.array([[1, 0, 1],[-2, -3, 1],[2, -3, 1]]) F = fm.enforce_singularity_constraint(r3) self.assertEqual(2, np.linalg.matrix_rank(F)) Loading
autocnet/transformation/fundamental_matrix.py +77 −15 Original line number Diff line number Diff line import warnings import numpy as np import pandas as pd from scipy import optimize from autocnet.camera import camera from autocnet.camera import utils as camera_utils Loading @@ -8,26 +9,23 @@ from autocnet.utils.utils import make_homogeneous, normalize_vector try: import cv2 cv2_avail = True except: except: # pragma: no cover cv_avail = False def compute_error(F, x, x1): def compute_reprojection_error(F, x, x1): """ Given a set of matches and a known fundamental matrix, compute distance between all match points and the associated compute distance between match points and the associated epipolar lines. Ideal error is defined by $x^{\intercal}Fx = 0$, where $x$ are all matchpoints in a given image and $x^{\intercal}F$ defines the standard form of the epipolar line in the second image. The distance between a point and the associated epipolar line is computed as: $d = \frac{\lvert ax_{0} + by_{0} + c \rvert}{\sqrt{a^{2} + b^{2}}}$. Parameters ---------- F : ndarray (3,3) Fundamental matrix x : arraylike (n,2) or (n,3) array of homogeneous coordinates Loading @@ -53,7 +51,53 @@ def compute_error(F, x, x1): return F_error def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None): def compute_fundamental_error(F, x, x1): """ Compute the fundamental error using the idealized error metric. Ideal error is defined by $x^{\intercal}Fx = 0$, where $x$ are all matchpoints in a given image and $x^{\intercal}F$ defines the standard form of the epipolar line in the second image. This method assumes that x and x1 are ordered such that x[0] correspondes to x1[0]. Parameters ---------- F : ndarray (3,3) Fundamental matrix x : arraylike (n,2) or (n,3) array of homogeneous coordinates x1 : arraylike (n,2) or (n,3) array of homogeneous coordinates with the same length as argument x Returns ------- F_error : ndarray n,1 vector of reprojection errors """ # TODO: Can this be vectorized for performance? if x.shape[1] != 3: x = make_homogeneous(x) if x1.shape[1] != 3: x1 = make_homogeneous(x1) if isinstance(x, pd.DataFrame): x = x.values if isinstance(x1, pd.DataFrame): x1 = x1.values err = np.empty(len(x)) for i in range(len(x)): err[i] = x1[i].T.dot(F).dot(x[i]) return err def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None, method='reprojection'): """ Given a Fundamental matrix and two sets of points, compute the reprojection error between x1 and x2. A mask is returned with all Loading @@ -71,7 +115,10 @@ def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None): (n,2) or (n,3) array of homogeneous coordinates threshold : float The new upper, reprojective error limit, in pixels. The new upper limit for error. If using reprojection this is measured in pixels (the default). If using fundamental, the idealized error is 0. Values +- 0.05 should be good. index : ndarray Optional index for mapping between reprojective error Loading @@ -82,10 +129,16 @@ def update_fundamental_mask(F, x1, x2, threshold=1.0, index=None): mask : dataframe """ error = compute_error(F, x1, x2) mask = error <= threshold if method == 'reprojection': error = compute_reprojection_error(F, x1, x2) elif method == 'fundamental': error = compute_fundamental_error(F, x1, x2) else: warnings.warn('Unknown error method. Options are "reprojection" or "fundamental".') mask = pd.DataFrame(np.abs(error) <= threshold, index=index, columns=['fundamental']) if index != None: mask = pd.DataFrame(mask, index=index, columns='F_Error') mask.index = index return mask def enforce_singularity_constraint(F): Loading Loading @@ -182,13 +235,18 @@ def compute_fundamental_matrix(kp1, kp2, method='mle', reproj_threshold=2.0, if method == 'mle': # Now apply the gold standard algorithm to refine F if kp1.shape[1] != 3: kp1 = make_homogeneous(kp1) if kp2.shape[1] != 3: kp2 = make_homogeneous(kp2) # Generate an idealized and to be updated camera model p1 = camera.estimated_camera_from_f(F) p = camera.idealized_camera() # Grab the points used to estimate F pt = kp1.loc[mask] pt1 = kp2.loc[mask] pt = kp1.loc[mask].T pt1 = kp2.loc[mask].T if pt.shape[1] < 9 or pt1.shape[1] < 9: warnings.warn("Unable to apply MLE. Not enough correspondences. Returning with a RANSAC computed F matrix.") Loading @@ -206,4 +264,8 @@ def compute_fundamental_matrix(kp1, kp2, method='mle', reproj_threshold=2.0, F = gold_standard_f mask = update_fundamental_mask(F, kp1, kp2, threshold=reproj_threshold).values return F, mask
autocnet/transformation/tests/test_fundamental_matrix.py +94 −14 Original line number Diff line number Diff line Loading @@ -14,24 +14,85 @@ class TestFundamentalMatrix(unittest.TestCase): @classmethod def setUpClass(cls): nbr_inliers = 20 np.random.seed(12345) fp = np.array(np.random.standard_normal((nbr_inliers, 2))) # inliers static_F = np.array([[4, 0.5, 10], [0.25, 1, 5], [0.2, 0.1, 1]]) # Make homogeneous fph = np.hstack((fp, np.ones((nbr_inliers, 1)))) tp = static_F.dot(fph.T) # normalize hom. coordinates tp /= tp[-1, :np.newaxis] tp = np.empty((nbr_inliers, 3)) for i, j in enumerate(fph): proj = j.dot(static_F) proj /= proj[2] tp[i] = proj cls.static_F = static_F cls.F = np.array([[-0.685892, -5.870193, 2.268333], [-0.704199, 12.88776, -3.040341], [-0.231815, -2.806056, 1.]]) cls.x1 = pd.DataFrame(fph, columns=['x', 'y', 'h']) cls.x2 = pd.DataFrame(tp.T, columns=['x', 'y', 'h']) cls.x2 = pd.DataFrame(tp, columns=['x', 'y', 'h']) cls.fixed_x1 = np.array([[ 438.394104 , 846.43518066, 1. ], [ 767.89105225, 380.79367065, 1.], [ 63.80842972, 815.14257812, 1. ], [ 283.96408081, 901.07287598, 1. ], [ 421.63833618, 841.66619873, 1. ], [ 181.8278656 , 706.01611328, 1. ], [ 650.27160645, 416.72653198, 1. ], [ 650.27160645, 416.72653198, 1. ], [ 721.18585205, 368.2802124 , 1. ], [ 88.97966003, 962.11322021, 1. ]]) cls.fixed_x2 = np.array([[ 652.32714844, 847.51605225, 1. ], [ 985.95928955, 384.58950806, 1. ], [ 281.5947876 , 819.9956665 , 1. ], [ 501.13912964, 904.06054688, 1. ], [ 637.31488037, 842.93652344, 1. ], [ 398.35501099, 708.86029053, 1. ], [ 875.11975098, 419.54541016, 1. ], [ 875.11975098, 419.54541016, 1. ], [ 943.69946289, 372.12527466, 1. ], [ 299.27636719, 968.44104004, 1. ]]) cls.fixed_f = np.array([[ -2.85373973e-08, 3.02728824e-06, -2.41915056e-03], [ -4.53237187e-06, 1.38905788e-07, -4.14644099e-02], [ 3.25687216e-03, 4.11777575e-02, 3.61272746e-01]]) def test_compute_f(self): # The F matrix is good if the sum of the error is within some threshold. F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='ransac') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='lmeds') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='normal') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='8point') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) F, mask = fm.compute_fundamental_matrix(self.x1, self.x2, method='mle') self.assertTrue(abs(sum(fm.compute_fundamental_error(F, self.x1, self.x2))) < 0.01) def test_compute_mle_f(self): #TODO: Write a better test for MLE the data here is too clean. pass def test_f_reprojection_error(self): err = fm.compute_reprojection_error(self.fixed_f, self.fixed_x1, self.fixed_x2) self.assertTrue(err.mean() < 0.5) def test_f_fundamental_error(self): err = fm.compute_fundamental_error(self.fixed_f, self.fixed_x1, self.fixed_x2) self.assertTrue(abs(sum(err)) < 0.03) def test_update_fundamental_mask(self): np.random.seed(12345) nbr_inliers = 20 fp = np.array(np.random.standard_normal((nbr_inliers, 2))) Loading @@ -39,19 +100,38 @@ class TestFundamentalMatrix(unittest.TestCase): F, mask = fm.compute_fundamental_matrix(fp, tp, method='ransac') np.testing.assert_array_almost_equal(F, self.F) new_mask = fm.update_fundamental_mask(F, fp, tp, threshold=0.5, method='reprojection') self.assertEqual(10, new_mask['fundamental'].sum()) def test_compute_mle_f(self): #TODO: Write a better test for MLE the data here is too clean. def test_update_fundamental_mask_with_index(self): np.random.seed(12345) nbr_inliers = 20 fp = pd.DataFrame(np.array(np.random.standard_normal((nbr_inliers, 2)))) tp = pd.DataFrame(np.array(np.random.standard_normal((nbr_inliers, 2)))) fp = np.array(np.random.standard_normal((nbr_inliers, 2))) tp = np.array(np.random.standard_normal((nbr_inliers, 2))) F, mask = fm.compute_fundamental_matrix(fp, tp, method='mle') F, mask = fm.compute_fundamental_matrix(fp, tp, method='ransac') new_index = np.arange(20)[::-1] #Just reverse the index new_mask = fm.update_fundamental_mask(F, fp, tp, threshold=0.5, index=new_index) np.testing.assert_array_equal(new_index, new_mask.index.values) np.testing.assert_array_almost_equal(F, self.F) def test_update_fundamental_mask_with_fundamental(self): new_mask = fm.update_fundamental_mask(self.fixed_f, self.fixed_x1, self.fixed_x2, threshold=0.05, method='fundamental') def test_f_error(self): #TODO: This is a stochastic process - how to test? pass self.assertTrue(new_mask['fundamental'].sum() == 10) new_mask = fm.update_fundamental_mask(self.fixed_f, self.fixed_x1, self.fixed_x2, threshold=0.005, method='fundamental') print(new_mask['fundamental'].sum()) self.assertTrue(new_mask['fundamental'].sum() == 9) def test_enforce_singularity_constraint(self): r3 = np.array([[1, 0, 1],[-2, -3, 1],[2, -3, 1]]) F = fm.enforce_singularity_constraint(r3) self.assertEqual(2, np.linalg.matrix_rank(F))
