diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp new file mode 100644 index 000000000..d2bc6d679 --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye.cpp + * @date Apr 8, 2020 + * @author ghaggin + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void Cal3Fisheye::print(const std::string& s_) const { + Base::print(s_); +} + +/* ************************************************************************* */ +bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const { + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || std::abs(k4_ - K.k4_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const { + return Cal3Fisheye(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h new file mode 100644 index 000000000..df2978350 --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye.h @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye.h + * @brief Calibration of a fisheye, calculations in base class Cal3Fisheye_Base + * @date Apr 8, 2020 + * @author ghaggin + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @brief Calibration of a fisheye camera that also supports + * Lie-group behaviors for optimization. + * \sa Cal3Fisheye_Base + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Fisheye : public Cal3Fisheye_Base { + + typedef Cal3Fisheye_Base Base; + +public: + + enum { dimension = 9 }; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Fisheye() : Base() {} + + 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) : + Base(fx, fy, s, u0, v0, k1, k2, k3, k4) {} + + virtual ~Cal3Fisheye() {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Fisheye(const Vector &v) : Base(v) {} + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + virtual void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Fisheye& K, double tol = 10e-9) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Fisheye retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Fisheye& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 9; } //TODO: make a final dimension variable + + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3Fisheye(*this)); + } + + /// @} + + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) + { + ar & boost::serialization::make_nvp("Cal3Fisheye", + boost::serialization::base_object(*this)); + } + + /// @} + +}; + +template<> +struct traits : public internal::Manifold {}; + +template<> +struct traits : public internal::Manifold {}; + +} + diff --git a/gtsam/geometry/Cal3Fisheye_Base.cpp b/gtsam/geometry/Cal3Fisheye_Base.cpp new file mode 100644 index 000000000..19f7d010b --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye_Base.cpp @@ -0,0 +1,184 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye_Base.cpp + * @date Apr 8, 2020 + * @author ghaggin + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Fisheye_Base::Cal3Fisheye_Base(const Vector &v): + fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} + +/* ************************************************************************* */ +Matrix3 Cal3Fisheye_Base::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; +} + +/* ************************************************************************* */ +Vector9 Cal3Fisheye_Base::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_; + return v; +} + +/* ************************************************************************* */ +void Cal3Fisheye_Base::print(const std::string& s_) const { + gtsam::print((Matrix)K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); +} + +/* ************************************************************************* */ +bool Cal3Fisheye_Base::equals(const Cal3Fisheye_Base& K, double tol) const { + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || std::abs(k4_ - K.k4_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +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 rr = r*r; + const double dxd_dxi = dtd_dxi*xi*(1/r) + td*(1/r) + td*xi*(-1/rr)*dr_dxi; + const double dxd_dyi = dtd_dyi*xi*(1/r) + td*xi*(-1/rr)*dr_dyi; + const double dyd_dxi = dtd_dxi*yi*(1/r) + td*yi*(-1/rr)*dr_dxi; + const double dyd_dyi = dtd_dyi*yi*(1/r) + td*(1/r) + td*yi*(-1/rr)*dr_dyi; + + Matrix2 DR; + DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; + + return DK * DR; +} + +/* ************************************************************************* */ +Point2 Cal3Fisheye_Base::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 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; + 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); + + // 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); + + return uv; +} + +/* ************************************************************************* */ +Point2 Cal3Fisheye_Base::calibrate(const Point2& uv, const double tol) 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, + // throw exception if max iterations are reached + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + Matrix2 jac; + + // Calculate the current estimate (uv_hat) and the jacobian + const Point2 uv_hat = uncalibrate(pi, boost::none, jac); + + // Test convergence + if((uv_hat - uv).norm() < tol) break; + + // Newton's method update step + pi = pi - jac.inverse()*(uv_hat - uv); + } + + if ( iteration >= maxIterations ) + throw std::runtime_error("Cal3Fisheye::calibrate fails to converge. need a better initialization"); + + return pi; +} + +/* ************************************************************************* */ +Matrix2 Cal3Fisheye_Base::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_Base::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); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Fisheye_Base.h b/gtsam/geometry/Cal3Fisheye_Base.h new file mode 100644 index 000000000..0313aadb5 --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye_Base.h @@ -0,0 +1,180 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye_Base.h + * @brief Calibration of a fisheye camera. + * @date Apr 8, 2020 + * @author ghaggin + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @brief Calibration of a fisheye camera + * @addtogroup geometry + * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html + * 3D point in camera frame + * p = (x, y, z) + * Intrinsic coordinates: + * [x_i;y_i] = [x/z; y/z] + * Distorted coordinates: + * r^2 = (x_i)^2 + (y_i)^2 + * th = atan(r) + * th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8) + * [x_d; y_d] = (th_d / r)*[x_i; y_i] + * Pixel coordinates: + * K = [fx s u0; 0 fy v0 ;0 0 1] + * [u; v; 1] = K*[x_d; y_d; 1] + */ +class GTSAM_EXPORT Cal3Fisheye_Base { + +protected: + + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_, k3_, k4_ ; // fisheye distortion coefficients + +public: + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Fisheye_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + + Cal3Fisheye_Base(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3, double k4) : + fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + + virtual ~Cal3Fisheye_Base() {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Fisheye_Base(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + virtual void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Fisheye_Base& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length x + inline double fy() const { return fy_;} + + /// skew + inline double skew() const { return s_;} + + /// image center in x + inline double px() const { return u0_;} + + /// image center in y + inline double py() const { return v0_;} + + /// First distortion coefficient + inline double k1() const { return k1_;} + + /// Second distortion coefficient + inline double k2() const { return k2_;} + + /// First tangential distortion coefficient + inline double k3() const { return k3_;} + + /// Second tangential distortion coefficient + inline double k4() const { return k4_;} + + /// return calibration matrix + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); } + + /// Return all parameters as a vector + Vector9 vector() const; + + /** + * convert intrinsic coordinates [x_i; y_i] to (distorted) image coordinates [u; v] + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ..., k4) + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) + * @return point in (distorted) image coordinates + */ + Point2 uncalibrate(const Point2& p, + OptionalJacobian<2,9> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; + + /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, 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 Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3Fisheye_Base(*this)); + } + + /// @} + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) + { + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(k3_); + ar & BOOST_SERIALIZATION_NVP(k4_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp new file mode 100644 index 000000000..fbe08a5a3 --- /dev/null +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCal3Fisheye.cpp + * @brief Unit tests for fisheye calibration class + * @author ghaggin + */ + + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Fisheye) + +static Cal3Fisheye K(500, 100, 0.1, 320, 240, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625); +static Point2 p(2,3); + +/* ************************************************************************* */ +TEST( Cal3Fisheye, uncalibrate) +{ + // Calculate the solution + 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 + K.k1()*tt + K.k2()*t4 + K.k3()*t6 + K.k4()*t8); + Vector3 pd(td/r*xi, td/r*yi, 1); + Vector3 v = K.K()*pd; + + Point2 uv_sol(v[0]/v[2], v[1]/v[2]); + + Point2 uv = K.uncalibrate(p); + CHECK(assert_equal(uv, uv_sol)); +} + +TEST( Cal3Fisheye, calibrate ) +{ + Point2 pi; + Point2 uv; + Point2 pi_hat; + + pi = Point2(0.5, 0.5); // point in intrinsic coordinates + uv = K.uncalibrate(pi); // map intrinsic coord to image plane (pi) + pi_hat = K.calibrate(uv); // map image coords (pi) back to intrinsic coords + CHECK( traits::Equals(pi, pi_hat, 1e-5)); // check that the inv mapping works + + pi = Point2(-0.7, -1.2); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK( traits::Equals(pi, pi_hat, 1e-5)); + + pi = Point2(-3, 5); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK( traits::Equals(pi, pi_hat, 1e-5)); + + pi = Point2(7, -12); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK( traits::Equals(pi, pi_hat, 1e-5)); +} + +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; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */