From 921dc848a08ba08dc369ef461d9646a9a11f63cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 22 Sep 2020 20:17:09 -0400 Subject: [PATCH 1/9] added calibrate with jacobians for Cal3Bundler --- gtsam/geometry/Cal3Bundler.cpp | 33 ++++++++++++++++++++++ gtsam/geometry/Cal3Bundler.h | 10 +++++++ gtsam/geometry/tests/testPinholeCamera.cpp | 9 ++++++ 3 files changed, 52 insertions(+) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 0d64d0184..56f62e3ac 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -121,6 +121,39 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { return pn; } +/* ************************************************************************* */ +Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp) const { + Point2 pn = calibrate(p, 1e-5); + + // Approximate the jacobians via a single iteration of g. + if (Dcal) { + const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; + const double xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double inv_f = 1 / f_, inv_g = 1 / g; + + *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, + -y * inv_g * rr, -y * inv_g * rr * rr; + } + if (Dp) { + const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; + const double xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double inv_f = 1 / f_, inv_g = 1 / g; + + const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); + const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); + + *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv, + -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv); + } + + return pn; +} + /* ************************************************************************* */ Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { Matrix2 Dp; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4787f565b..0042b710f 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -120,6 +120,16 @@ public: /// Convert a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; + /** + * Convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; + /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index a9b68bdec..ad6f4e921 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) { EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } +/* ************************************************************************* */ +TEST( PinholeCamera, Cal3Bundler) { + Cal3Bundler calibration; + Pose3 wTc; + PinholeCamera camera(wTc, calibration); + Point2 p(50, 100); + camera.backproject(p, 10); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 3fab304191b04b80e7525f980c28b6244ef14bef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 23 Sep 2020 09:59:46 -0400 Subject: [PATCH 2/9] combine the calibrate functions into one --- gtsam/geometry/Cal3Bundler.cpp | 28 ++++++++-------------------- gtsam/geometry/Cal3Bundler.h | 11 +++++------ 2 files changed, 13 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 56f62e3ac..4677dbe45 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -94,7 +94,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp, + const double tol) const { // Copied from Cal3DS2 :-( // but specialized with k1,k2 non-zero only and fx=fy and s=0 const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); @@ -118,32 +120,18 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - return pn; -} - -/* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal, - OptionalJacobian<2, 2> Dp) const { - Point2 pn = calibrate(p, 1e-5); + const double x = invKPi.x(), y = invKPi.y(); + const double xx = x * x, yy = y * y; + const double rr = xx + yy; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double inv_f = 1 / f_, inv_g = 1 / g; // Approximate the jacobians via a single iteration of g. if (Dcal) { - const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; - const double xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double inv_f = 1 / f_, inv_g = 1 / g; - *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, -y * inv_g * rr, -y * inv_g * rr * rr; } if (Dp) { - const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_; - const double xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double inv_f = 1 / f_, inv_g = 1 / g; - const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0042b710f..0636399a2 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -117,18 +117,17 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; - /// Convert a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; - /** - * Convert image coordinates uv to intrinsic coordinates xy + * Convert a pixel coordinate to ideal coordinate xy * @param p point in image coordinates * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @param tol optional tolerance threshold value for iterative minimization * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none, + const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; From 611974c63ad7469973fa847138979a09e9a102f7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 23 Sep 2020 10:45:57 -0400 Subject: [PATCH 3/9] put the jacobians at the end so that the calibrate function can be wrapped --- gtsam/geometry/Cal3Bundler.cpp | 6 +++--- gtsam/geometry/Cal3Bundler.h | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 4677dbe45..837ce35e0 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -94,9 +94,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, - OptionalJacobian<2, 2> Dp, - const double tol) const { +Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, + 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 const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0636399a2..35398303f 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -120,14 +120,14 @@ public: /** * Convert a pixel coordinate to ideal coordinate xy * @param p point in image coordinates + * @param tol optional tolerance threshold value for iterative minimization * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @param tol optional tolerance threshold value for iterative minimization * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none, - const double tol = 1e-5) const; + Point2 calibrate(const Point2& pi, const double tol = 1e-5, + OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; From b499006b12dbab9b1d7b1af366a74c1c554f2437 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 23 Sep 2020 13:48:40 -0400 Subject: [PATCH 4/9] make tolerance as a constructor param --- gtsam/geometry/Cal3Bundler.cpp | 21 +++++++++++---------- gtsam/geometry/Cal3Bundler.h | 8 ++++++-- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 837ce35e0..470c97c75 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -25,13 +25,13 @@ namespace gtsam { /* ************************************************************************* */ Cal3Bundler::Cal3Bundler() : - f_(1), k1_(0), k2_(0), u0_(0), v0_(0) { + f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) { } /* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : - f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { -} +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0, + double tol) + : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {} /* ************************************************************************* */ Matrix3 Cal3Bundler::K() const { @@ -94,23 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, +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 - const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); + double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_; + const Point2 invKPi(x, y); // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; + Point2 pn(x, 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) + if (distance2(uncalibrate(pn), pi) <= tol_) break; - const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; + 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); pn = invKPi / g; @@ -120,7 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - const double x = invKPi.x(), y = invKPi.y(); + x = invKPi.x(), y = invKPi.y(); const double xx = x * x, yy = y * y; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 35398303f..96765f899 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -33,6 +33,7 @@ private: double f_; ///< focal length double k1_, k2_; ///< radial distortion double u0_, v0_; ///< image center, not a parameter to be optimized but a constant + double tol_; ///< tolerance value when calibrating public: @@ -51,8 +52,10 @@ public: * @param k2 second radial distortion coefficient (quartic) * @param u0 optional image center (default 0), considered a constant * @param v0 optional image center (default 0), considered a constant + * @param tol optional calibration tolerance value */ - Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); + Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0, + double tol = 1e-5); virtual ~Cal3Bundler() {} @@ -125,7 +128,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, const double tol = 1e-5, + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; @@ -173,6 +176,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(tol_); } /// @} From cb78b281a57f125ccbdd5383aa454425ed3dbbfe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 24 Sep 2020 00:09:39 -0400 Subject: [PATCH 5/9] update calibrate in wrapper --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d8297c43b..369ea983c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1053,7 +1053,7 @@ class Cal3_S2Stereo { class Cal3Bundler { // Standard Constructors Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable void print(string s) const; @@ -1066,7 +1066,7 @@ class Cal3Bundler { Vector localCoordinates(const gtsam::Cal3Bundler& c) const; // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; // Standard Interface From 3743202a0daa2f1c356ef8ee9f47fd04593d4482 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 01:50:04 -0400 Subject: [PATCH 6/9] use implicit function theorem to compute jacobians of Cal3Bundler::calibrate --- gtsam/geometry/Cal3Bundler.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 470c97c75..dad2a78d1 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -121,23 +121,23 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - x = invKPi.x(), y = invKPi.y(); - const double xx = x * x, yy = y * y; - const double rr = xx + yy; - const double g = (1 + k1_ * rr + k2_ * rr * rr); - const double inv_f = 1 / f_, inv_g = 1 / g; + // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate + // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians + // df/pi = -I (pn and pi are independent args) + // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + Matrix H_uncal_K, H_uncal_pn; - // Approximate the jacobians via a single iteration of g. - if (Dcal) { - *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y, - -y * inv_g * rr, -y * inv_g * rr * rr; + if (Dcal || Dp) { + // Compute uncalibrate Jacobians + uncalibrate(pn, H_uncal_K, H_uncal_pn); + } + + if (Dcal) { + *Dcal = -H_uncal_pn.inverse() * H_uncal_K; } if (Dp) { - const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f); - const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f); - - *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv, - -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv); + *Dp = H_uncal_pn.inverse(); } return pn; From c52c217592d9195b16b232354ef78e5d3f398dc8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 01:50:14 -0400 Subject: [PATCH 7/9] added test for Cal3Bundler::calibrate Jacobians1 --- gtsam/geometry/tests/testCal3Bundler.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 8de049fa4..448600266 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( traits::Equals(pn, pn_hat, 1e-5)); + CHECK(traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } + /* ************************************************************************* */ TEST( Cal3Bundler, Duncalibrate) { @@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate) Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical1,Dcal,1e-7)); CHECK(assert_equal(numerical2,Dp,1e-7)); - CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7)); - CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7)); - Matrix Dcombined(2,5); - Dcombined << Dp, Dcal; - CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, Dcalibrate) +{ + 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)); } /* ************************************************************************* */ From 8634d3f42ea2202d34a16f4d295935f03a28873a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 01:50:21 -0400 Subject: [PATCH 8/9] make Cal3Bundler wrapper constructor backwards compatible --- gtsam/gtsam.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 369ea983c..5be275133 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1053,6 +1053,7 @@ class Cal3_S2Stereo { class Cal3Bundler { // Standard Constructors Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable From 7dab718861880fcdec4417ceb5133843f9e58a52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Sep 2020 10:49:44 -0400 Subject: [PATCH 9/9] fixed sized matrices and minor improvements --- gtsam/geometry/Cal3Bundler.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index dad2a78d1..e986bc820 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -126,20 +126,17 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, // df/pi = -I (pn and pi are independent args) // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K - Matrix H_uncal_K, H_uncal_pn; + Matrix23 H_uncal_K; + Matrix22 H_uncal_pn; if (Dcal || Dp) { // Compute uncalibrate Jacobians - uncalibrate(pn, H_uncal_K, H_uncal_pn); - } + uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); - if (Dcal) { - *Dcal = -H_uncal_pn.inverse() * H_uncal_K; - } - if (Dp) { - *Dp = H_uncal_pn.inverse(); - } + if (Dp) *Dp = H_uncal_pn.inverse(); + if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K; + } return pn; }