12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Convergence of equidistant scaling utilizing atan2

release/4.3a0
roderick-koehle 2021-10-28 14:34:04 +02:00 committed by GitHub
parent e1db2be5bd
commit 2763bd8968
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
python/gtsam/tests/test_Cal3Fisheye.py
View File

@ -143,16 +143,23 @@ class TestCal3Fisheye(GtsamTestCase):
self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) self.gtsamAssertEquals(H @ H.T, 3*np.eye(2))
def test_scaling_factor(self): def test_scaling_factor(self):
"Check convergence of atan(r, z)/r for small r" """Check convergence of atan2(r, z)/r ~ 1/z for small r"""
r = ulp(np.float64) r = ulp(np.float64)
s = np.arctan(r) / r s = np.arctan(r) / r
self.assertEqual(s, 1.0) self.assertEqual(s, 1.0)
z = 1 z = 1
s = np.arctan2(r, z) / r s = scaling_factor(r, z)
self.assertEqual(s, 1.0) self.assertEqual(s, 1.0/z)
z = 2 z = 2
s = np.arctan2(r, z) / r if r/z != 0 else 1.0 s = scaling_factor(r, z)
self.assertEqual(s, 1.0) self.assertEqual(s, 1.0/z)
s = scaling_factor(2*r, z)
self.assertEqual(s, 1.0/z)
@staticmethod
def scaling_factor(r, z):
"""Projection factor theta/r for equidistant fisheye lens model"""
return np.arctan2(r, z) / r if r/z != 0 else 1.0/z
@staticmethod @staticmethod
def evaluate_jacobian(obj_point, img_point): def evaluate_jacobian(obj_point, img_point):