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

Add comment about initial guess in undistortion 

For the equidistant fisheye model, r/f = tan(theta), the Gauss-Newton search to model radial distortion is expected to converge faster by mapping the angular coordinate space into the respective tangent space of the perspective plane. This is consistent to the nPlaneToSpace initial projection used in the calibrate() function of the omnidirectional model (Cal3Unified).
release/4.3a0
roderick-koehle 2021-07-10 15:21:22 +02:00 committed by GitHub
parent 66af0079ba
commit 03049929a5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -106,12 +106,20 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// initial gues just inverts the pinhole model
// Apply inverse camera matrix to map the pixel coordinate (u, v)
// of the equidistant fisheye image to angular coordinate space (xd, yd)
// with radius theta given in radians.
const double u = uv.x(), v = uv.y();
const double yd = (v - v0_) / fy_;
const double xd = (u - s_ * yd - u0_) / fx_;
const double theta = sqrt(xd * xd + yd * yd);
const double scale = (theta > 0) ? tan(theta) / theta;
// Provide initial guess for the Gauss-Newton search.
// The angular coordinates given by (xd, yd) are mapped back to
// the focal plane of the perspective undistorted projection pi.
// See Cal3Unified.calibrate() using the same pattern for the
// undistortion of omnidirectional fisheye projection.
const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
Point2 pi(scale * xd, scale * yd);
// Perform newtons method, break when solution converges past tol_,