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

Improved accuracy for analytic undistortion

release/4.3a0
roderick-koehle 2021-07-09 12:39:31 +02:00 committed by GitHub
parent a411b664a1
commit 66af0079ba
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -110,7 +110,9 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
const double u = uv.x(), v = uv.y();
const double yd = (v - v0_) / fy_;
const double xd = (u - s_ * yd - u0_) / fx_;
Point2 pi(xd, yd);
const double theta = sqrt(xd * xd + yd * yd);
const double scale = (theta > 0) ? tan(theta) / theta;
Point2 pi(scale * xd, scale * yd);
// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached