Fix cal3 fisheye jacobian #902
Conversation
Forward declaration of PinholeCameraCal3Fisheye needed by python wrapper.
Forward declaration of PinholeCameraCal3Fisheye
| DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; | ||
|
|
||
| *H2 = DK * DR; | ||
| if (r2==0) { |
There was a problem hiding this comment.
Maybe this should be r2 < 1e-7 or some other tolerance.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
@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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
Okay this makes sense. Thanks for the explanation.
Can you please address the other comments as well?
| Unit in the last place of floating point datatypes | ||
| """ | ||
| f = np.finfo(ftype) | ||
| return f.tiny / ftype(1 << f.nmant) |
There was a problem hiding this comment.
This is awesome but is it physically plausible? I don't expect the radial value to be 1e-154 given today's manufacturing precisions. Using a reasonable value of say 1e-20 should be equivalent?
There was a problem hiding this comment.
The goal here is to use the ulp value to test for a possible overflow in the implementation, independent of the floating point datatype.
The value of ulp() for half float, float and double is given by,
julia> eps(nextfloat(Float16(0.0)))
Float16(6.0e-8)
julia> eps(nextfloat(Float32(0.0)))
1.0f-45
julia> eps(nextfloat(Float64(0.0)))
5.0e-324The f.tiny provides the smallest value, at which the floating point number can still be represented as normalized value.
Since python 3.9, numpy provides the numpy.ulp() function (see https://bugs.python.org/issue39310 or https://www.boost.org/doc/libs/1_77_0/libs/math/doc/html/math_toolkit/next_float/ulp.html).
In our case, it defines the smallest denormalized value ulp, such that 0+ulp > 0. In simple terms, when numbers are denormalized, calculations are done in fixed-point arithmetic, using the smallest exponent for the mantissa.
This value is used in the test to check whether overflow occurs. In the original implementation, setting x=ulp() and y=0, resulted in an overflow returning a NaN Jacobian.
In terms of the numeric resolution of double float, a value of 1e-20 is a huge number. For half-float, it is a too small number to represent.
The test would not catch any critical overflow condition. (Same for using f.tiny - it does not catch an overflow for computing the Jacobian)
Physically, thinking in terms of taylor expansion, a radius of 1e-20 the Jacobian would be sufficient to be in the paraxial region - as noted, the implementation then depends on the datatype.
There was a problem hiding this comment.
Awesome, this is a great explanation.
gtsam/geometry/Cal3Fisheye.cpp
Outdated
| 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; |
There was a problem hiding this comment.
Delete these commented lines if not used?
gtsam/geometry/Cal3Fisheye.cpp
Outdated
| // 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 |
There was a problem hiding this comment.
Delete these commented lines if not used?
gtsam/geometry/Cal3Fisheye.cpp
Outdated
| // 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 |
There was a problem hiding this comment.
Delete these commented lines if not used?
There was a problem hiding this comment.
Comments are removed.
|
@roderick-koehle could you address the comments so we can merge this great PR? |
In the calculation of the jacobian for the Cal3Fisheye calibration model the resulting jacobian contain NaN due to an unhandled division by zero. The problem occurs in case the spatial object point is located on the optical axis.
Provided unit test in test_Cal3Fisheye illustrate the issue by projecting a point at (0, 0, 1) with the camera being located at the origin.