From 993095297c4a3c1b2b89f6ce162a74ba98cf9e8d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 30 May 2021 00:20:13 -0400 Subject: [PATCH 1/6] Add Akshay's Cal3Bundler test --- gtsam/geometry/tests/testCal3Bundler.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index b821d295b..59993819e 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -87,6 +87,20 @@ TEST(Cal3Bundler, Dcalibrate) { CHECK(assert_equal(numerical2, Dp, 1e-5)); } +/* ************************************************************************* */ +TEST(Cal3Bundler, DcalibrateDefault) { + Cal3Bundler trueK(1, 0, 0); + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = trueK.uncalibrate(pn); + Point2 actual = trueK.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + /* ************************************************************************* */ TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); } From c252937cee9233b17b5ba28e54f8494fe0dc69d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 30 May 2021 00:20:41 -0400 Subject: [PATCH 2/6] 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; } From c2f4cc82bfd63812d01549e7a38745e9bef6c11e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 2 Jun 2021 12:08:07 -0400 Subject: [PATCH 3/6] initialize with intrinsic coordinates which has radial distortion modeled --- gtsam/geometry/Cal3Bundler.cpp | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 6a22f5d3e..83140a5e0 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -99,21 +99,26 @@ 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 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; - + Point2 pn; + double px = pi.x(), py = pi.y(); + // 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; - px = pn.x(), py = pn.y(); - rr = (px * px) + (py * py); + int iteration = 0; + do { + // initialize pn with distortion included + double rr = (px * px) + (py * py); double g = (1 + k1_ * rr + k2_ * rr * rr); pn = invKPi / g; - } + + if (distance2(uncalibrate(pn), pi) <= tol_) break; + + // Set px and py using intrinsic coordinates since that is where radial + // distortion correction is done. + px = pn.x(), py = pn.y(); + iteration++; + + } while (iteration < maxIterations); if (iteration >= maxIterations) throw std::runtime_error( @@ -149,4 +154,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { return H; } -} // \ namespace gtsam +} // namespace gtsam From fb784eea9c6033ae852ec3af2a9266a568c33830 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 2 Jun 2021 12:12:35 -0400 Subject: [PATCH 4/6] add all of Akshay's tests for default model --- gtsam/geometry/tests/testCal3Bundler.cpp | 41 ++++++++++++++++-------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 59993819e..a0629c83e 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -62,6 +62,33 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } +/* ************************************************************************* */ +TEST(Cal3Bundler, DuncalibrateDefault) { + Cal3Bundler trueK(1, 0, 0); + Matrix Dcal, Dp; + Point2 actual = trueK.uncalibrate(p, Dcal, Dp); + Point2 expected = p; + CHECK(assert_equal(expected, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p); + Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p); + CHECK(assert_equal(numerical1, Dcal, 1e-7)); + CHECK(assert_equal(numerical2, Dp, 1e-7)); +} + +/* ************************************************************************* */ +TEST(Cal3Bundler, DcalibrateDefault) { + Cal3Bundler trueK(1, 0, 0); + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = trueK.uncalibrate(pn); + Point2 actual = trueK.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + /* ************************************************************************* */ TEST(Cal3Bundler, Duncalibrate) { Matrix Dcal, Dp; @@ -87,20 +114,6 @@ TEST(Cal3Bundler, Dcalibrate) { CHECK(assert_equal(numerical2, Dp, 1e-5)); } -/* ************************************************************************* */ -TEST(Cal3Bundler, DcalibrateDefault) { - Cal3Bundler trueK(1, 0, 0); - Matrix Dcal, Dp; - Point2 pn(0.5, 0.5); - Point2 pi = trueK.uncalibrate(pn); - Point2 actual = trueK.calibrate(pi, Dcal, Dp); - CHECK(assert_equal(pn, actual, 1e-7)); - Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi); - Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi); - CHECK(assert_equal(numerical1, Dcal, 1e-5)); - CHECK(assert_equal(numerical2, Dp, 1e-5)); -} - /* ************************************************************************* */ TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); } From b84402695161fb5f5d608149dd5c9c4789a808bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Jun 2021 11:06:13 -0400 Subject: [PATCH 5/6] addressed comments and added an additional test --- gtsam/geometry/Cal3Bundler.cpp | 6 ++---- gtsam/geometry/tests/testCal3Bundler.cpp | 27 ++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 83140a5e0..a95bda6b9 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -96,11 +96,9 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // Copied from Cal3DS2 // but specialized with k1, k2 non-zero only and fx=fy and s=0 - double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_; - const Point2 invKPi(x, y); - + double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_; + const Point2 invKPi(px, py); Point2 pn; - double px = pi.x(), py = pi.y(); // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index a0629c83e..cd576f900 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -89,6 +89,33 @@ TEST(Cal3Bundler, DcalibrateDefault) { CHECK(assert_equal(numerical2, Dp, 1e-5)); } +/* ************************************************************************* */ +TEST(Cal3Bundler, DuncalibratePrincipalPoint) { + Cal3Bundler K(5, 0, 0, 2, 2); + Matrix Dcal, Dp; + Point2 actual = K.uncalibrate(p, Dcal, Dp); + Point2 expected(12, 17); + CHECK(assert_equal(expected, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); + Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); + CHECK(assert_equal(numerical1, Dcal, 1e-7)); + CHECK(assert_equal(numerical2, Dp, 1e-7)); +} + +/* ************************************************************************* */ +TEST(Cal3Bundler, DcalibratePrincipalPoint) { + Cal3Bundler K(2, 0, 0, 2, 2); + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Point2 actual = K.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + /* ************************************************************************* */ TEST(Cal3Bundler, Duncalibrate) { Matrix Dcal, Dp; From 2d739dd5e86da69b0519b145f652ef102153795c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Jun 2021 14:05:34 -0400 Subject: [PATCH 6/6] make rr and g as const --- gtsam/geometry/Cal3Bundler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index a95bda6b9..3ae624bbc 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -105,8 +105,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, int iteration = 0; do { // initialize pn with distortion included - double rr = (px * px) + (py * py); - double g = (1 + k1_ * rr + k2_ * rr * rr); + const double rr = (px * px) + (py * py); + const double g = (1 + k1_ * rr + k2_ * rr * rr); pn = invKPi / g; if (distance2(uncalibrate(pn), pi) <= tol_) break;