From c252937cee9233b17b5ba28e54f8494fe0dc69d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 30 May 2021 00:20:41 -0400 Subject: [PATCH] account for radial distortion in initial guess for `calibrate` --- gtsam/geometry/Cal3Bundler.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index e03562452..6a22f5d3e 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -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_; const Point2 invKPi(x, y); - // initialize by ignoring the distortion at all, might be problematic for - // pixels around boundary - Point2 pn(x, y); - + // initialize pn with distortion included + double px = pi.x(), py = pi.y(), rr = px * px + py * py; + double g = (1 + k1_ * rr + k2_ * rr * rr); + Point2 pn = invKPi / g; + // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { if (distance2(uncalibrate(pn), pi) <= tol_) break; - const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); + px = pn.x(), py = pn.y(); + rr = (px * px) + (py * py); + double g = (1 + k1_ * rr + k2_ * rr * rr); pn = invKPi / g; }