Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix cal3 fisheye jacobian #902

Merged
merged 16 commits into from
Nov 28, 2021
Merged

Fix cal3 fisheye jacobian #902

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
d9c770d
Forward declaration of PinholeCameraCal3Fisheye
roderick-koehle Jul 8, 2021
ab92ba7
Merge pull request #3 from roderick-koehle/patch-1
roderick-koehle Jul 8, 2021
9440ff6
Merge branch 'borglab:develop' into develop
roderick-koehle Oct 22, 2021
0a1fead
Test of jacobian of Cal3Fisheye for on-axis point
roderick-koehle Oct 22, 2021
1640f17
Test jacobian of Cal3Unified for on-axis point
roderick-koehle Oct 22, 2021
8df2c70
Avoid division by zero in jacobian calculation
roderick-koehle Oct 22, 2021
f844481
Merge branch 'borglab:develop' into Fix-Cal3Fisheye-Jacobian
roderick-koehle Oct 28, 2021
91103d5
Check numeric stability close to optical axis
roderick-koehle Oct 28, 2021
c0219c1
Numerically stable refactoring of fisheye jacobian
roderick-koehle Oct 28, 2021
0d01e48
Fix missing semicolons
roderick-koehle Oct 28, 2021
8c2ea78
Undo change in scaling function
roderick-koehle Oct 28, 2021
e1db2be
Fix type in extression for dyd_dyi
roderick-koehle Oct 28, 2021
2763bd8
Convergence of equidistant scaling utilizing atan2
roderick-koehle Oct 28, 2021
296c937
Fix calling scaling_factor static method.
roderick-koehle Oct 28, 2021
8a6b2aa
Removed comments
roderick-koehle Nov 22, 2021
8846324
Merge branch 'borglab:develop' into Fix-Cal3Fisheye-Jacobian
roderick-koehle Nov 22, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 26 additions & 22 deletions gtsam/geometry/Cal3Fisheye.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

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;

*H2 = DK * DR;
if (r2==0) {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe this should be r2 < 1e-7 or some other tolerance.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The condition r2>0 implies the resolution of the image radius r to be above sqrt(std::numeric_limits<double>::denorm_min()) ~ 1.49e-154. By avoiding to compute the square or divisor of two small values, the resulting gradient should be reasonably accurate.

I implemented a python testcase test_jacobian_convergence in test_Cal3Fisheye.py to cover this situation.

In the current implementation of the jacobian, this results in a overflow or division by zero error, if the square radius r2 is of the size of 1 ulp (unit of least precision in C++ std::numeric_limits::denorm_min() ~ 4.94e-324).

I suggest the refactoring in provided patch below, avoiding the offending division by 1/r2 (or multiplication with rrdiv).

Eventually, the interface design might be extended supporting incident angles above 90 deg. It may be beneficial to generalize the formulation of derivatives, to cover the case zi != 1. The scaling function needs to be generalized to use atan2(r, z)/r instead of atan(r)/r. From numeric tests, is seems fine to implement the scaling for (r/z != 0). See the test_scaling_factor unit test to cover this case.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@roderick-koehle I believe @ProfFan was referring to the fact that doing a r2==0 check is numerically unstable since that check will fail for r2=1e-15 but the value of r2 in this case is still small enough to cause various issues like overflow and bad jacobians.

This is a recommendation for doing a numerical check rather than an algebraic check.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry for the late reply.

An arbitrary fixed threshold to select the paraxial approximation I would not recommend.

The critical numerically unstable calculation is the calculation of the unit directions.
c, s = x/r, y/r
this problem is related to computing the givens rotation, e.g. used in the blas library.
Once the unit direction is known, the sine and cosine will be a normalised number and no overflow/underflow issues can occur.

For a more in depth discussion about the issues, see:
Bindel, Demmel, Kahan, "On Computing Givens Rotations Reliably and Efficiently"

The current suggested implementation is simple and numerically stable, this is demonstrated and covered by the provided python tests. In this case, using (r2==0)is a well defined lower threshold imposed on r2 being independent of the floating point datatype being used.

Alternatively, for using a higher threshold, e.g. for 1/3 r2 < 0.5 eps with eps being the machine precision, the distortion induced by the arctan(xi)/xi ~ 1+1/3 x^2 becomes equal to one and the paraxial approximation can be used. For this choice, k1 should not exceed 1/3 (the fist taylor coefficient of the tan series development) and z is assumed 1. See the boost sinc_pi implementation to illustrate this technique. Note the discussion, https://stackoverflow.com/questions/47215765/is-boostmathsinc-pi-unnecessarily-complicated
about using taylor bounds.

My suggestion is to either stay with the simple type-independent scheme using the conditionr2==0, or compute the direction cosines using a stable unit direction implementation, similar to the givens rotation implementation as proposed by the publication by Blink. Then, no paraxial case is necessary.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay this makes sense. Thanks for the explanation.

Can you please address the other comments as well?

*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;

Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;

*H2 = DK * DR;
}
}

return uv;
Expand Down
22 changes: 22 additions & 0 deletions python/gtsam/tests/test_Cal3Fisheye.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,28 @@ def test_sfm_factor2(self):
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"""
Expand Down
22 changes: 22 additions & 0 deletions python/gtsam/tests/test_Cal3Unified.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,28 @@ def test_sfm_factor2(self):
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"""
Expand Down