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

account for radial distortion in initial guess for `calibrate`

release/4.3a0
Varun Agrawal 2021-05-30 00:20:41 -04:00
parent 993095297c
commit c252937cee
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Cal3Bundler.cpp
View File

@ -99,18 +99,19 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_; double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y); const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for // initialize pn with distortion included
// pixels around boundary double px = pi.x(), py = pi.y(), rr = px * px + py * py;
Point2 pn(x, y); double g = (1 + k1_ * rr + k2_ * rr * rr);
Point2 pn = invKPi / g;
// iterate until the uncalibrate is close to the actual pixel coordinate // iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10; const int maxIterations = 10;
int iteration; int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) { for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_) break; if (distance2(uncalibrate(pn), pi) <= tol_) break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; px = pn.x(), py = pn.y();
const double rr = xx + yy; rr = (px * px) + (py * py);
const double g = (1 + k1_ * rr + k2_ * rr * rr); double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g; pn = invKPi / g;
} }