From 012820b7fa6d7c5f5339036f6f2da38e9335e3ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 1 Dec 2020 00:39:32 -0500 Subject: [PATCH] Common function to compute Jacobians of calibrate method --- gtsam/geometry/Cal3Bundler.cpp | 19 +--------- gtsam/geometry/Cal3Bundler.h | 1 + gtsam/geometry/Cal3DS2_Base.cpp | 22 ++---------- gtsam/geometry/Cal3DS2_Base.h | 44 ++++++++++++++++++++--- gtsam/geometry/Cal3Fisheye.cpp | 9 +++-- gtsam/geometry/Cal3Fisheye.h | 13 ++++--- gtsam/geometry/Cal3Unified.cpp | 19 +--------- gtsam/geometry/tests/testCal3DFisheye.cpp | 17 +++++++++ 8 files changed, 76 insertions(+), 68 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 36e7bf62d..a73bfec52 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -121,24 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); - // 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) - // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) - // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K - Matrix23 H_uncal_K; - Matrix22 H_uncal_pn, H_uncal_pn_inv; - - if (Dcal || Dp) { - // Compute uncalibrate Jacobians - uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); - - H_uncal_pn_inv = H_uncal_pn.inverse(); - - if (Dp) *Dp = H_uncal_pn_inv; - if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; - - } + calibrateJacobians(*this, pn, Dcal, Dp); return pn; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 2e3fab002..da43112d9 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index c5ef117a7..d175259f2 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -34,8 +34,7 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector& v) k1_(v(5)), k2_(v(6)), p1_(v(7)), - p2_(v(8)), - tol_(1e-5) {} + p2_(v(8)) {} /* ************************************************************************* */ Matrix3 Cal3DS2_Base::K() const { @@ -173,24 +172,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal, throw std::runtime_error( "Cal3DS2::calibrate fails to converge. need a better initialization"); - // 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) - // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) - // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K - Matrix29 H_uncal_K; - Matrix22 H_uncal_pn, H_uncal_pn_inv; - - if (Dcal || Dp) { - // Compute uncalibrate Jacobians - uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); - - H_uncal_pn_inv = H_uncal_pn.inverse(); - - if (Dp) *Dp = H_uncal_pn_inv; - if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; - - } + calibrateJacobians(*this, pn, Dcal, Dp); return pn; } diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index b6d27cda1..c9b53c29b 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -23,6 +23,39 @@ namespace gtsam { +/** + * Function which makes use of the Implicit Function Theorem to compute the + * Jacobians of `calibrate` using `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) + * Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + * Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + * + * @tparam T Calibration model. + * @tparam Dim The number of parameters in the calibration model. + * @param p Calibrated point. + * @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters. + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates. + */ +template +void calibrateJacobians(const T& calibration, const Point2& pn, + OptionalJacobian<2, Dim> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) { + if (Dcal || Dp) { + Eigen::Matrix H_uncal_K; + Matrix22 H_uncal_pn, H_uncal_pn_inv; + + // Compute uncalibrate Jacobians + calibration.uncalibrate(pn, H_uncal_K, H_uncal_pn); + + H_uncal_pn_inv = H_uncal_pn.inverse(); + + if (Dp) *Dp = H_uncal_pn_inv; + if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; + } +} + /** * @brief Calibration of a camera with radial distortion * @addtogroup geometry @@ -40,14 +73,15 @@ namespace gtsam { class GTSAM_EXPORT Cal3DS2_Base { protected: - - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double p1_, p2_ ; // tangential distortion - double tol_; // tolerance value when calibrating + double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point + double k1_, k2_; // radial 2nd-order and 4th-order + double p1_, p2_; // tangential distortion + double tol_ = 1e-5; // tolerance value when calibrating public: + enum { dimension = 9 }; + /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index f7794fafb..1ed1826ad 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -122,14 +122,15 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, } /* ************************************************************************* */ -Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { +Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, + OptionalJacobian<2, 2> Dp) const { // initial gues just inverts the pinhole model 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); - // Perform newtons method, break when solution converges past tol, + // Perform newtons method, break when solution converges past tol_, // throw exception if max iterations are reached const int maxIterations = 10; int iteration; @@ -140,7 +141,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { const Point2 uv_hat = uncalibrate(pi, boost::none, jac); // Test convergence - if ((uv_hat - uv).norm() < tol) break; + if ((uv_hat - uv).norm() < tol_) break; // Newton's method update step pi = pi - jac.inverse() * (uv_hat - uv); @@ -151,6 +152,8 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { "Cal3Fisheye::calibrate fails to converge. need a better " "initialization"); + calibrateJacobians(*this, pi, Dcal, Dp); + return pi; } diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index e24fe074f..5487019f6 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -14,10 +14,12 @@ * @brief Calibration of a fisheye camera * @date Apr 8, 2020 * @author ghaggin + * @author Varun Agrawal */ #pragma once +#include #include #include @@ -48,6 +50,7 @@ class GTSAM_EXPORT Cal3Fisheye { private: double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point double k1_, k2_, k3_, k4_; // fisheye distortion coefficients + double tol_ = 1e-5; // tolerance value when calibrating public: enum { dimension = 9 }; @@ -59,11 +62,11 @@ class GTSAM_EXPORT Cal3Fisheye { /// Default Constructor with only unit focal length Cal3Fisheye() - : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {} Cal3Fisheye(const double fx, const double fy, const double s, const double u0, const double v0, const double k1, const double k2, - const double k3, const double k4) + const double k3, const double k4, double tol = 1e-5) : fx_(fx), fy_(fy), s_(s), @@ -72,7 +75,8 @@ class GTSAM_EXPORT Cal3Fisheye { k1_(k1), k2_(k2), k3_(k3), - k4_(k4) {} + k4_(k4), + tol_(tol) {} virtual ~Cal3Fisheye() {} @@ -139,7 +143,8 @@ class GTSAM_EXPORT Cal3Fisheye { /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, /// y_i] - Point2 calibrate(const Point2& p, const double tol = 1e-5) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// @} /// @name Testable diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 247e77ae1..f4ce0ed75 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -111,24 +111,7 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal, // call nplane to space Point2 pn = this->nPlaneToSpace(pnplane); - // 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) - // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) - // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K - Eigen::Matrix H_uncal_K; - Matrix22 H_uncal_pn, H_uncal_pn_inv; - - if (Dcal || Dp) { - // Compute uncalibrate Jacobians - uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); - - H_uncal_pn_inv = H_uncal_pn.inverse(); - - if (Dp) *Dp = H_uncal_pn_inv; - if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; - - } + calibrateJacobians(*this, pn, Dcal, Dp); return pn; } diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 9317fb737..6bfbe3e46 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -181,6 +181,23 @@ TEST(Cal3Fisheye, calibrate3) { CHECK(assert_equal(xi_hat, xi)); } +Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) { + return k.calibrate(pt); +} + +/* ************************************************************************* */ +TEST(Cal3Fisheye, Dcalibrate) +{ + Point2 p(0.5, 0.5); + Point2 pi = K.uncalibrate(p); + Matrix Dcal, Dp; + K.calibrate(pi, Dcal, Dp); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + CHECK(assert_equal(numerical1,Dcal,1e-5)); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical2,Dp,1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr;