From d9c770d2296c41536241c9683b39604ed0e67e82 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 8 Jul 2021 14:23:46 +0200 Subject: [PATCH 01/12] Forward declaration of PinholeCameraCal3Fisheye Forward declaration of PinholeCameraCal3Fisheye needed by python wrapper. --- gtsam/geometry/SimpleCamera.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c7..119e9d1a3 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** From 0a1fead551fcf7bd8790414d5091c6f9bbd118c8 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 22 Oct 2021 19:33:06 +0200 Subject: [PATCH 02/12] Test of jacobian of Cal3Fisheye for on-axis point --- python/gtsam/tests/test_Cal3Fisheye.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 298c6e57b..326c8682f 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -105,6 +105,28 @@ class TestCal3Fisheye(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + def test_jacobian(self): + """Evaluate jacobian at optical axis""" + obj_point_on_axis = np.array([0, 0, 1]) + img_point = np.array([0.0, 0.0]) + pose = gtsam.Pose3() + camera = gtsam.Cal3Fisheye() + state = gtsam.Values() + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_pose3(pose_key, pose) + state.insert_cal3fisheye(camera_key, camera) + g = gtsam.NonlinearFactorGraph() + noise_model = gtsam.noiseModel.Unit.Create(2) + factor = gtsam.GeneralSFMFactor2Cal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera_key) + g.add(factor) + f = g.error(state) + gaussian_factor_graph = g.linearize(state) + H, z = gaussian_factor_graph.jacobian() + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" From 1640f172e6ad56d107506daf4c46947a8908d7f5 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 22 Oct 2021 19:34:27 +0200 Subject: [PATCH 03/12] Test jacobian of Cal3Unified for on-axis point --- python/gtsam/tests/test_Cal3Unified.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index dab1ae446..bafbacfa4 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -117,6 +117,28 @@ class TestCal3Unified(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + def test_jacobian(self): + """Evaluate jacobian at optical axis""" + obj_point_on_axis = np.array([0, 0, 1]) + img_point = np.array([0.0, 0.0]) + pose = gtsam.Pose3() + camera = gtsam.Cal3Unified() + state = gtsam.Values() + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + state.insert_cal3unified(camera_key, camera) + state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_pose3(pose_key, pose) + g = gtsam.NonlinearFactorGraph() + noise_model = gtsam.noiseModel.Unit.Create(2) + factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key) + g.add(factor) + f = g.error(state) + gaussian_factor_graph = g.linearize(state) + H, z = gaussian_factor_graph.jacobian() + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" From 8df2c7086641892c88124f3b0a7af9ee6fabefaf Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Fri, 22 Oct 2021 19:39:09 +0200 Subject: [PATCH 04/12] Avoid division by zero in jacobian calculation --- gtsam/geometry/Cal3Fisheye.cpp | 42 +++++++++++++++++++--------------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 52d475d5d..f889342b6 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -76,28 +76,32 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, // Derivative for points in intrinsic coords (2 by 2) if (H2) { - const double dtd_dt = - 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double dt_dr = 1 / (1 + r2); - const double rinv = 1 / r; - const double dr_dxi = xi * rinv; - const double dr_dyi = yi * rinv; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + if (r2==0) { + *H2 = DK; + } else { + const double dtd_dt = + 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; + const double dt_dr = 1 / (1 + r2); + const double rinv = 1 / r; + const double dr_dxi = xi * rinv; + const double dr_dyi = yi * rinv; + const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; + const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - const double td = t * K.dot(T); - const double rrinv = 1 / r2; - const double dxd_dxi = - dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; - const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + const double td = t * K.dot(T); + const double rrinv = 1 / r2; + const double dxd_dxi = + dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; + const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; + const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; + const double dyd_dyi = + dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; - Matrix2 DR; - DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; + Matrix2 DR; + DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; - *H2 = DK * DR; + *H2 = DK * DR; + } } return uv; From 91103d5f477b09b063f50cd0620b716a60b80a8d Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 11:20:12 +0200 Subject: [PATCH 05/12] Check numeric stability close to optical axis --- python/gtsam/tests/test_Cal3Fisheye.py | 63 ++++++++++++++++++++++---- 1 file changed, 54 insertions(+), 9 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 326c8682f..81ba33631 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -17,6 +17,15 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase from gtsam.symbol_shorthand import K, L, P + +def ulp(ftype=np.float64): + """ + Unit in the last place of floating point datatypes + """ + f = np.finfo(ftype) + return f.tiny / ftype(1 << f.nmant) + + class TestCal3Fisheye(GtsamTestCase): @classmethod @@ -105,27 +114,63 @@ class TestCal3Fisheye(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) - def test_jacobian(self): - """Evaluate jacobian at optical axis""" + def test_jacobian_on_axis(self): + """Check of jacobian at optical axis""" obj_point_on_axis = np.array([0, 0, 1]) - img_point = np.array([0.0, 0.0]) + img_point = np.array([0, 0]) + f, z, H = self.evaluate_jacobian(obj_point_on_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + def test_jacobian_convergence(self): + """Test stability of jacobian close to optical axis""" + t = ulp(np.float64) + obj_point_close_to_axis = np.array([t, 0, 1]) + img_point = np.array([np.sqrt(t), 0]) + f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + # With a height of sqrt(ulp), this may cause an overflow + t = ulp(np.float64) + obj_point_close_to_axis = np.array([np.sqrt(t), 0, 1]) + img_point = np.array([np.sqrt(t), 0]) + f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + def test_scaling_factor(self): + "Check convergence of atan(r, z)/r for small r" + r = ulp(np.float64) + s = np.arctan(r) / r + self.assertEqual(s, 1.0) + z = 1 + s = np.arctan2(r, z) / r + self.assertEqual(s, 1.0) + z = 2 + s = np.arctan2(r, z) / r if r/z != 0 else 1.0 + self.assertEqual(s, 1.0) + + @staticmethod + def evaluate_jacobian(obj_point, img_point): + """Evaluate jacobian at given object point""" pose = gtsam.Pose3() camera = gtsam.Cal3Fisheye() state = gtsam.Values() camera_key, pose_key, landmark_key = K(0), P(0), L(0) - state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_point3(landmark_key, obj_point) state.insert_pose3(pose_key, pose) - state.insert_cal3fisheye(camera_key, camera) g = gtsam.NonlinearFactorGraph() noise_model = gtsam.noiseModel.Unit.Create(2) - factor = gtsam.GeneralSFMFactor2Cal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera_key) + factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera) g.add(factor) f = g.error(state) gaussian_factor_graph = g.linearize(state) H, z = gaussian_factor_graph.jacobian() - self.assertAlmostEqual(f, 0) - self.gtsamAssertEquals(z, np.zeros(2)) - self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + return f, z, H @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): From c0219c1ad018e2732943c6b775aa6ae7fb3b1d99 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 11:56:42 +0200 Subject: [PATCH 06/12] Numerically stable refactoring of fisheye jacobian --- gtsam/geometry/Cal3Fisheye.cpp | 36 ++++++++++++++++++++++------------ 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index f889342b6..e95165c9a 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -46,7 +46,7 @@ double Cal3Fisheye::Scaling(double r) { /* ************************************************************************* */ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { - const double xi = p.x(), yi = p.y(); + const double xi = p.x(), yi = p.y(), zi = 1; const double r2 = xi * xi + yi * yi, r = sqrt(r2); const double t = atan(r); const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; @@ -81,22 +81,34 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, } else { const double dtd_dt = 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double dt_dr = 1 / (1 + r2); + const double R2 = r2 + zi*zi + const double dt_dr = zi / R2; const double rinv = 1 / r; const double dr_dxi = xi * rinv; const double dr_dyi = yi * rinv; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + const double dtd_dr = dtd_dt * dt_dr + // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; + // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + + const double c2 = dr_dxi * dr_dxi + const double s2 = dr_dyi * dr_dyi + const double cs = dr_dxi * dr_dyi - const double td = t * K.dot(T); - const double rrinv = 1 / r2; - const double dxd_dxi = - dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; - const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + // Following refactoring is numerically stable, even for unnormalized radial + // values by avoiding division with the square radius. + // + // const double td = t * K.dot(T); - note: s = td/r + // const double rrinv = 1 / r2; - division by r2 may cause overflow + const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); + const double dxd_dyi = (dtd_dr - s) * cs; + const double dyd_dxi = dxd_dyi; + const double dyd_dyi = dtd_dr * c2 + s * (1 - s2); + // Derivatives by depth, for future use to support incident + // angles above 90 deg. + // const double dxd_dzi = -dtd_dt * x / R2 + // const double dyd_dzi = -dtd_dt * y / R2 + Matrix2 DR; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; From 0d01e4844f2974d838d2b43272f423e1e8ed7577 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 13:23:18 +0200 Subject: [PATCH 07/12] Fix missing semicolons --- gtsam/geometry/Cal3Fisheye.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index e95165c9a..ee2e66080 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -32,7 +32,7 @@ Vector9 Cal3Fisheye::vector() const { } /* ************************************************************************* */ -double Cal3Fisheye::Scaling(double r) { +double Cal3Fisheye::Scaling(double r, double zi) { static constexpr double threshold = 1e-8; if (r > threshold || r < -threshold) { return atan(r) / r; @@ -48,12 +48,12 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { const double xi = p.x(), yi = p.y(), zi = 1; const double r2 = xi * xi + yi * yi, r = sqrt(r2); - const double t = atan(r); + const double t = atan2(r, zi); const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; Vector5 K, T; K << 1, k1_, k2_, k3_, k4_; T << 1, t2, t4, t6, t8; - const double scaling = Scaling(r); + const double scaling = Scaling(r, zi); const double s = scaling * K.dot(T); const double xd = s * xi, yd = s * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); @@ -81,18 +81,18 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, } else { const double dtd_dt = 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double R2 = r2 + zi*zi + const double R2 = r2 + zi*zi; const double dt_dr = zi / R2; const double rinv = 1 / r; const double dr_dxi = xi * rinv; const double dr_dyi = yi * rinv; - const double dtd_dr = dtd_dt * dt_dr + const double dtd_dr = dtd_dt * dt_dr; // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - const double c2 = dr_dxi * dr_dxi - const double s2 = dr_dyi * dr_dyi - const double cs = dr_dxi * dr_dyi + const double c2 = dr_dxi * dr_dxi; + const double s2 = dr_dyi * dr_dyi; + const double cs = dr_dxi * dr_dyi; // Following refactoring is numerically stable, even for unnormalized radial // values by avoiding division with the square radius. From 8c2ea78b1aede2581fecacccb56526597aeb117b Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 13:27:05 +0200 Subject: [PATCH 08/12] Undo change in scaling function --- gtsam/geometry/Cal3Fisheye.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index ee2e66080..c4ce151ba 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -32,7 +32,7 @@ Vector9 Cal3Fisheye::vector() const { } /* ************************************************************************* */ -double Cal3Fisheye::Scaling(double r, double zi) { +double Cal3Fisheye::Scaling(double r) { static constexpr double threshold = 1e-8; if (r > threshold || r < -threshold) { return atan(r) / r; @@ -53,7 +53,7 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, Vector5 K, T; K << 1, k1_, k2_, k3_, k4_; T << 1, t2, t4, t6, t8; - const double scaling = Scaling(r, zi); + const double scaling = Scaling(r); const double s = scaling * K.dot(T); const double xd = s * xi, yd = s * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); From e1db2be5bdad8ac02320a9488014db55d252ca1c Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 13:54:38 +0200 Subject: [PATCH 09/12] Fix type in extression for dyd_dyi --- gtsam/geometry/Cal3Fisheye.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index c4ce151ba..7bf9ea300 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -102,7 +102,7 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); const double dxd_dyi = (dtd_dr - s) * cs; const double dyd_dxi = dxd_dyi; - const double dyd_dyi = dtd_dr * c2 + s * (1 - s2); + const double dyd_dyi = dtd_dr * s2 + s * (1 - s2); // Derivatives by depth, for future use to support incident // angles above 90 deg. From 2763bd89686d95a70e40349c20d0324a254dc202 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 14:34:04 +0200 Subject: [PATCH 10/12] Convergence of equidistant scaling utilizing atan2 --- python/gtsam/tests/test_Cal3Fisheye.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 81ba33631..6bdaa5a12 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -143,16 +143,23 @@ class TestCal3Fisheye(GtsamTestCase): self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) 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) s = np.arctan(r) / r self.assertEqual(s, 1.0) z = 1 - s = np.arctan2(r, z) / r - self.assertEqual(s, 1.0) + s = scaling_factor(r, z) + self.assertEqual(s, 1.0/z) z = 2 - s = np.arctan2(r, z) / r if r/z != 0 else 1.0 - self.assertEqual(s, 1.0) + s = scaling_factor(r, z) + 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 def evaluate_jacobian(obj_point, img_point): From 296c937ca83af7ced0301da06d38a09b1ec19a65 Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Thu, 28 Oct 2021 15:55:25 +0200 Subject: [PATCH 11/12] Fix calling scaling_factor static method. --- python/gtsam/tests/test_Cal3Fisheye.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 6bdaa5a12..e54afc757 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -148,12 +148,12 @@ class TestCal3Fisheye(GtsamTestCase): s = np.arctan(r) / r self.assertEqual(s, 1.0) z = 1 - s = scaling_factor(r, z) + s = self.scaling_factor(r, z) self.assertEqual(s, 1.0/z) z = 2 - s = scaling_factor(r, z) + s = self.scaling_factor(r, z) self.assertEqual(s, 1.0/z) - s = scaling_factor(2*r, z) + s = self.scaling_factor(2*r, z) self.assertEqual(s, 1.0/z) @staticmethod From 8a6b2aadca4c05380a94805f66691025dc0377fe Mon Sep 17 00:00:00 2001 From: roderick-koehle <50633232+roderick-koehle@users.noreply.github.com> Date: Mon, 22 Nov 2021 21:02:32 +0100 Subject: [PATCH 12/12] Removed comments --- gtsam/geometry/Cal3Fisheye.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 7bf9ea300..fd2c7ab65 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -87,28 +87,16 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, const double dr_dxi = xi * rinv; const double dr_dyi = yi * rinv; const double dtd_dr = dtd_dt * dt_dr; - // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; const double c2 = dr_dxi * dr_dxi; const double s2 = dr_dyi * dr_dyi; const double cs = dr_dxi * dr_dyi; - // Following refactoring is numerically stable, even for unnormalized radial - // values by avoiding division with the square radius. - // - // const double td = t * K.dot(T); - note: s = td/r - // const double rrinv = 1 / r2; - division by r2 may cause overflow const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); const double dxd_dyi = (dtd_dr - s) * cs; const double dyd_dxi = dxd_dyi; const double dyd_dyi = dtd_dr * s2 + s * (1 - s2); - // Derivatives by depth, for future use to support incident - // angles above 90 deg. - // const double dxd_dzi = -dtd_dt * x / R2 - // const double dyd_dzi = -dtd_dt * y / R2 - Matrix2 DR; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;