diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index c6b43004e..f7794fafb 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -24,7 +24,7 @@ namespace gtsam { /* ************************************************************************* */ -Cal3Fisheye::Cal3Fisheye(const Vector& v) +Cal3Fisheye::Cal3Fisheye(const Vector9& v) : fx_(v[0]), fy_(v[1]), s_(v[2]), @@ -50,76 +50,73 @@ Matrix3 Cal3Fisheye::K() const { } /* ************************************************************************* */ -static Matrix29 D2dcalibration(const double xd, const double yd, - const double xi, const double yi, - const double t3, const double t5, - const double t7, const double t9, const double r, - Matrix2& DK) { - // order: fx, fy, s, u0, v0 - Matrix25 DR1; - DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0; - - // order: k1, k2, k3, k4 - Matrix24 DR2; - DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi; - DR2 /= r; - Matrix29 D; - D << DR1, DK * DR2; - return D; -} - -/* ************************************************************************* */ -static Matrix2 D2dintrinsic(const double xi, const double yi, const double r, - const double td, const double t, const double tt, - const double t4, const double t6, const double t8, - const double k1, const double k2, const double k3, - const double k4, const Matrix2& DK) { - const double dr_dxi = xi / sqrt(xi * xi + yi * yi); - const double dr_dyi = yi / sqrt(xi * xi + yi * yi); - const double dt_dr = 1 / (1 + r * r); - const double dtd_dt = - 1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - - const double rinv = 1 / r; - const double rrinv = 1 / (r * r); - const double dxd_dxi = - dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi; - const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi; - - Matrix2 DR; - DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; - - return DK * DR; +double Cal3Fisheye::Scaling(double r) { + static constexpr double threshold = 1e-8; + if (r > threshold || r < -threshold) { + return atan(r) / r; + } else { + // Taylor expansion close to 0 + double r2 = r * r, r4 = r2 * r2; + return 1.0 - r2 / 3 + r4 / 5; + } } /* ************************************************************************* */ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi * xi + yi * yi); + const double r2 = xi * xi + yi * yi, r = sqrt(r2); const double t = atan(r); - const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; - const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8); - const double td_o_r = r > 1e-8 ? td / r : 1; - const double xd = td_o_r * xi, yd = td_o_r * yi; + const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; + Vector5 K, T; + K << 1, k1_, k2_, k3_, k4_; + T << 1, t2, t4, t6, t8; + const double scaling = Scaling(r); + const double s = scaling * K.dot(T); + const double xd = s * xi, yd = s * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); Matrix2 DK; if (H1 || H2) DK << fx_, s_, 0.0, fy_; // Derivative for calibration parameters (2 by 9) - if (H1) - *H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK); + if (H1) { + Matrix25 DR1; + // order: fx, fy, s, u0, v0 + DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0; + + // order: k1, k2, k3, k4 + Matrix24 DR2; + auto T4 = T.tail<4>().transpose(); + DR2 << xi * T4, yi * T4; + *H1 << DR1, DK * scaling * DR2; + } // Derivative for points in intrinsic coords (2 by 2) - if (H2) - *H2 = - D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK); + if (H2) { + const double dtd_dt = + 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; + const double dt_dr = 1 / (1 + r2); + const double rinv = 1 / r; + const double dr_dxi = xi * rinv; + const double dr_dyi = yi * rinv; + const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; + const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + + const double td = t * K.dot(T); + const double rrinv = 1 / r2; + const double dxd_dxi = + dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; + const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; + const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; + const double dyd_dyi = + dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + + Matrix2 DR; + DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; + + *H2 = DK * DR; + } return uv; } @@ -157,39 +154,10 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { return pi; } -/* ************************************************************************* */ -Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const { - const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi * xi + yi * yi); - const double t = atan(r); - const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4; - const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8); - - Matrix2 DK; - DK << fx_, s_, 0.0, fy_; - - return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK); -} - -/* ************************************************************************* */ -Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const { - const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi * xi + yi * yi); - const double t = atan(r); - const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; - const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8); - const double xd = td / r * xi, yd = td / r * yi; - - Matrix2 DK; - DK << fx_, s_, 0.0, fy_; - return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK); -} - /* ************************************************************************* */ void Cal3Fisheye::print(const std::string& s_) const { gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print(Vector(k()), s_ + ".k"); - ; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 5fb196047..e24fe074f 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -20,6 +20,8 @@ #include +#include + namespace gtsam { /** @@ -43,7 +45,7 @@ namespace gtsam { * [u; v; 1] = K*[x_d; y_d; 1] */ class GTSAM_EXPORT Cal3Fisheye { - protected: + private: double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point double k1_, k2_, k3_, k4_; // fisheye distortion coefficients @@ -78,7 +80,7 @@ class GTSAM_EXPORT Cal3Fisheye { /// @name Advanced Constructors /// @{ - Cal3Fisheye(const Vector& v); + explicit Cal3Fisheye(const Vector9& v); /// @} /// @name Standard Interface @@ -120,6 +122,9 @@ class GTSAM_EXPORT Cal3Fisheye { /// Return all parameters as a vector Vector9 vector() const; + /// Helper function that calculates atan(r)/r + static double Scaling(double r); + /** * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image * coordinates [u; v] @@ -136,13 +141,6 @@ class GTSAM_EXPORT Cal3Fisheye { /// y_i] Point2 calibrate(const Point2& p, const double tol = 1e-5) const; - /// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i] - Matrix2 D2d_intrinsic(const Point2& p) const; - - /// Derivative of uncalibrate wrpt the calibration parameters - /// [fx, fy, s, u0, v0, k1, k2, k3, k4] - Matrix29 D2d_calibration(const Point2& p) const; - /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 9203b5438..9317fb737 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -10,17 +10,18 @@ * -------------------------------------------------------------------------- */ /** - * @file testCal3Fisheye.cpp + * @file testCal3DFisheye.cpp * @brief Unit tests for fisheye calibration class * @author ghaggin */ -#include #include #include #include #include +#include + using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye) @@ -30,12 +31,27 @@ static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240; static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625); -static Point2 p(2, 3); +static Point2 kTestPoint2(2, 3); + +/* ************************************************************************* */ +TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } + +/* ************************************************************************* */ +TEST(Cal3Fisheye, retract) { + Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4, + K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8, + K.k4() + 9); + Vector d(9); + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + Cal3Fisheye actual = K.retract(d); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); +} /* ************************************************************************* */ TEST(Cal3Fisheye, uncalibrate1) { // Calculate the solution - const double xi = p.x(), yi = p.y(); + const double xi = kTestPoint2.x(), yi = kTestPoint2.y(); const double r = sqrt(xi * xi + yi * yi); const double t = atan(r); const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; @@ -46,32 +62,42 @@ TEST(Cal3Fisheye, uncalibrate1) { Point2 uv_sol(v[0] / v[2], v[1] / v[2]); - Point2 uv = K.uncalibrate(p); + Point2 uv = K.uncalibrate(kTestPoint2); CHECK(assert_equal(uv, uv_sol)); } /* ************************************************************************* */ -/** - * Check that a point at (0,0) projects to the - * image center. - */ -TEST(Cal3Fisheye, uncalibrate2) { - Point2 pz(0, 0); - auto uv = K.uncalibrate(pz); - CHECK(assert_equal(uv, Point2(u0, v0))); +// For numerical derivatives +Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST(Cal3Fisheye, Derivatives) { + Matrix H1, H2; + K.uncalibrate(kTestPoint2, H1, H2); + CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5)); + CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 1e-7), H2, 1e-5)); } /* ************************************************************************* */ -/** - * This test uses cv2::fisheye::projectPoints to test that uncalibrate - * properly projects a point into the image plane. One notable difference - * between opencv and the Cal3Fisheye::uncalibrate function is the skew - * parameter. The equivalence is alpha = s/fx. - * - * - * Python script to project points with fisheye model in OpenCv - * (script run with OpenCv version 4.2.0 and Numpy version 1.18.2) - */ +// Check that a point at (0,0) projects to the image center. +TEST(Cal3Fisheye, uncalibrate2) { + Point2 pz(0, 0); + Matrix H1, H2; + auto uv = K.uncalibrate(pz, H1, H2); + CHECK(assert_equal(uv, Point2(u0, v0))); + CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5)); + // TODO(frank): the second jacobian is all NaN for the image center! + // CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5)); +} + +/* ************************************************************************* */ +// This test uses cv2::fisheye::projectPoints to test that uncalibrate +// properly projects a point into the image plane. One notable difference +// between opencv and the Cal3Fisheye::uncalibrate function is the skew +// parameter. The equivalence is alpha = s/fx. +// +// Python script to project points with fisheye model in OpenCv +// (script run with OpenCv version 4.2.0 and Numpy version 1.18.2) // clang-format off /* =========================================================== @@ -94,6 +120,7 @@ tvec = np.float64([[0.,0.,0.]]); imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha) np.set_printoptions(precision=14) print(imagePoints) + =========================================================== * Script output: [[[457.82638130304935 408.18905848512986]]] */ @@ -134,21 +161,18 @@ TEST(Cal3Fisheye, calibrate1) { } /* ************************************************************************* */ -/** - * Check that calibrate returns (0,0) for the image center - */ +// Check that calibrate returns (0,0) for the image center TEST(Cal3Fisheye, calibrate2) { Point2 uv(u0, v0); auto xi_hat = K.calibrate(uv); CHECK(assert_equal(xi_hat, Point2(0, 0))) } -/** - * Run calibrate on OpenCv test from uncalibrate3 - * (script shown above) - * 3d point: (23, 27, 31) - * 2d point in image plane: (457.82638130304935, 408.18905848512986) - */ +/* ************************************************************************* */ +// Run calibrate on OpenCv test from uncalibrate3 +// (script shown above) +// 3d point: (23, 27, 31) +// 2d point in image plane: (457.82638130304935, 408.18905848512986) TEST(Cal3Fisheye, calibrate3) { Point3 p3(23, 27, 31); Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); @@ -157,47 +181,6 @@ TEST(Cal3Fisheye, calibrate3) { CHECK(assert_equal(xi_hat, xi)); } -/* ************************************************************************* */ -// For numerical derivatives -Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) { - return k.uncalibrate(pt); -} - -/* ************************************************************************* */ -TEST(Cal3Fisheye, Duncalibrate1) { - Matrix computed; - K.uncalibrate(p, computed, boost::none); - Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical, computed, 1e-5)); - Matrix separate = K.D2d_calibration(p); - CHECK(assert_equal(numerical, separate, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Cal3Fisheye, Duncalibrate2) { - Matrix computed; - K.uncalibrate(p, boost::none, computed); - Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical, computed, 1e-5)); - Matrix separate = K.D2d_intrinsic(p); - CHECK(assert_equal(numerical, separate, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } - -/* ************************************************************************* */ -TEST(Cal3Fisheye, retract) { - Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4, - K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8, - K.k4() + 9); - Vector d(9); - d << 1, 2, 3, 4, 5, 6, 7, 8, 9; - Cal3Fisheye actual = K.retract(d); - CHECK(assert_equal(expected, actual, 1e-7)); - CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); -} - /* ************************************************************************* */ int main() { TestResult tr;