From 69130fda7ace354b7f9096237e76c2c87fd0d5e6 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Thu, 9 Apr 2020 09:24:55 -0400 Subject: [PATCH 1/6] Implemented fisheye camera calibration based on Cal3DS2 --- gtsam/geometry/Cal3Fisheye.cpp | 53 +++++++ gtsam/geometry/Cal3Fisheye.h | 121 ++++++++++++++ gtsam/geometry/Cal3Fisheye_Base.cpp | 184 ++++++++++++++++++++++ gtsam/geometry/Cal3Fisheye_Base.h | 180 +++++++++++++++++++++ gtsam/geometry/tests/testCal3DFisheye.cpp | 120 ++++++++++++++ 5 files changed, 658 insertions(+) create mode 100644 gtsam/geometry/Cal3Fisheye.cpp create mode 100644 gtsam/geometry/Cal3Fisheye.h create mode 100644 gtsam/geometry/Cal3Fisheye_Base.cpp create mode 100644 gtsam/geometry/Cal3Fisheye_Base.h create mode 100644 gtsam/geometry/tests/testCal3DFisheye.cpp 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); } +/* ************************************************************************* */ From a488888d4043b8a685a27023f8871de982520f43 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Thu, 9 Apr 2020 09:49:34 -0400 Subject: [PATCH 2/6] Added example with fisheye camera using the SFM data --- examples/FisheyeExample.cpp | 128 ++++++++++++++++++++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 examples/FisheyeExample.cpp diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp new file mode 100644 index 000000000..2f79eb667 --- /dev/null +++ b/examples/FisheyeExample.cpp @@ -0,0 +1,128 @@ +/* ---------------------------------------------------------------------------- + + * 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 VisualBatchExample.cpp + * @brief A visualSLAM example for the structure-from-motion problem on a + * simulated dataset. This version uses a fisheye camera model and a GaussNewton + * solver to solve the graph in one batch + * @author ghaggin + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// For loading the data +#include "SFMdata.h" + +// Camera observations of landmarks will be stored as Point2 (x, y). +#include + +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols +#include + +// Use GaussNewtonOptimizer to solve graph +#include +#include +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common +// factors have been provided with the library for solving robotics/SLAM/Bundle +// Adjustment problems. Here we will use Projection factors to model the +// camera's landmark observations. Also, we will initialize the robot at some +// location using a Prior factor. +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main(int argc, char *argv[]) +{ + // Define the camera calibration parameters + // Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + boost::shared_ptr K(new Cal3Fisheye( + 278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625)); + + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + + // Create the set of ground-truth landmarks + vector points = createPoints(); + + // Create the set of ground-truth poses + vector poses = createPoses(); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); + graph.emplace_shared>(Symbol('x', 0), poses[0], kPosePrior); + + // Add a prior on landmark l0 + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(Symbol('l', 0), points[0], kPointPrior); + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); + + // Loop over the poses, adding the observations to the graph + for (size_t i = 0; i < poses.size(); ++i) + { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) + { + PinholeCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); + } + + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + params.maxIterations = 10000; + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" << graph.error(initialEstimate) << std::endl; + std::cout << "final error=" << graph.error(result) << std::endl; + + std::ofstream os("examples/vio_batch.dot"); + graph.saveGraph(os, result); + + return 0; +} +/* ************************************************************************* */ From 3ee552c7c69d9f7987d0ad5485fa5313904a8158 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Thu, 9 Apr 2020 13:24:32 -0400 Subject: [PATCH 3/6] Changed cacheing in jac func and fixed comments per PR discussion --- examples/FisheyeExample.cpp | 3 ++- gtsam/geometry/Cal3Fisheye.h | 2 +- gtsam/geometry/Cal3Fisheye_Base.cpp | 11 ++++++----- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp index 2f79eb667..b03c9a71d 100644 --- a/examples/FisheyeExample.cpp +++ b/examples/FisheyeExample.cpp @@ -10,11 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file VisualBatchExample.cpp + * @file FisheyeExample.cpp * @brief A visualSLAM example for the structure-from-motion problem on a * simulated dataset. This version uses a fisheye camera model and a GaussNewton * solver to solve the graph in one batch * @author ghaggin + * @Date Apr 9,2020 */ /** diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index df2978350..8fd22cedc 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -11,7 +11,7 @@ /** * @file Cal3Fisheye.h - * @brief Calibration of a fisheye, calculations in base class Cal3Fisheye_Base + * @brief Calibration of a fisheye camera, calculations in base class Cal3Fisheye_Base * @date Apr 8, 2020 * @author ghaggin */ diff --git a/gtsam/geometry/Cal3Fisheye_Base.cpp b/gtsam/geometry/Cal3Fisheye_Base.cpp index 19f7d010b..4b51276df 100644 --- a/gtsam/geometry/Cal3Fisheye_Base.cpp +++ b/gtsam/geometry/Cal3Fisheye_Base.cpp @@ -81,11 +81,12 @@ static Matrix2 D2dintrinsic(const double xi, const double yi, const double r, co 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; + 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; From 5bb0bbdcc4381617ad52cc8b3969be38bc5ad414 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Tue, 21 Apr 2020 20:11:01 -0400 Subject: [PATCH 4/6] Combine Cal3Fisheye and Cal3Fisheye_Base into one class --- gtsam/geometry/Cal3Fisheye.cpp | 181 +++++++++++++++++++++++++-- gtsam/geometry/Cal3Fisheye.h | 160 ++++++++++++++++++------ gtsam/geometry/Cal3Fisheye_Base.cpp | 185 ---------------------------- gtsam/geometry/Cal3Fisheye_Base.h | 180 --------------------------- 4 files changed, 297 insertions(+), 409 deletions(-) delete mode 100644 gtsam/geometry/Cal3Fisheye_Base.cpp delete mode 100644 gtsam/geometry/Cal3Fisheye_Base.h diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index d2bc6d679..97931f588 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -15,24 +15,189 @@ * @author ghaggin */ -#include #include +#include +#include #include #include -#include namespace gtsam { +/* ************************************************************************* */ +Cal3Fisheye::Cal3Fisheye(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]) {} + +/* ************************************************************************* */ +Vector9 Cal3Fisheye::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_; + return v; +} + +/* ************************************************************************* */ +Matrix3 Cal3Fisheye::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; +} + +/* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +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 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::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::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 { - Base::print(s_); + gtsam::print((Matrix)K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); + ; } /* ************************************************************************* */ 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) + 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; } @@ -47,7 +212,5 @@ Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const { return T2.vector() - vector(); } -} +} // namespace gtsam /* ************************************************************************* */ - - diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 8fd22cedc..5fb196047 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -11,41 +11,66 @@ /** * @file Cal3Fisheye.h - * @brief Calibration of a fisheye camera, calculations in base class Cal3Fisheye_Base + * @brief Calibration of a fisheye camera * @date Apr 8, 2020 * @author ghaggin */ #pragma once -#include +#include namespace gtsam { /** - * @brief Calibration of a fisheye camera that also supports - * Lie-group behaviors for optimization. - * \sa Cal3Fisheye_Base + * @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 : public Cal3Fisheye_Base { - - typedef Cal3Fisheye_Base Base; - -public: +class GTSAM_EXPORT Cal3Fisheye { + protected: + double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point + double k1_, k2_, k3_, k4_; // fisheye distortion coefficients + public: enum { dimension = 9 }; + typedef boost::shared_ptr + shared_ptr; ///< shared pointer to fisheye calibration object /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3Fisheye() : Base() {} + Cal3Fisheye() + : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} - 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) {} + 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) + : fx_(fx), + fy_(fy), + s_(s), + u0_(u0), + v0_(v0), + k1_(k1), + k2_(k2), + k3_(k3), + k4_(k4) {} virtual ~Cal3Fisheye() {} @@ -53,14 +78,77 @@ public: /// @name Advanced Constructors /// @{ - Cal3Fisheye(const Vector &v) : Base(v) {} + Cal3Fisheye(const Vector& v); + + /// @} + /// @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; + + /** + * @brief 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 Testable /// @{ /// print with optional string - virtual void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const Cal3Fisheye& K, double tol = 10e-9) const; @@ -70,52 +158,54 @@ public: /// @{ /// Given delta vector, update calibration - Cal3Fisheye retract(const Vector& d) const ; + Cal3Fisheye retract(const Vector& d) const; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Fisheye& T2) const ; + 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) + virtual size_t dim() const { return 9; } /// Return dimensions of calibration manifold object - static size_t Dim() { return 9; } //TODO: make a final dimension variable + static size_t Dim() { return 9; } /// @} /// @name Clone /// @{ /// @return a deep copy of this object - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(new Cal3Fisheye(*this)); + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3Fisheye(*this)); } /// @} - -private: - + 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 + 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_); } /// @} - }; -template<> +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; -} - +} // namespace gtsam diff --git a/gtsam/geometry/Cal3Fisheye_Base.cpp b/gtsam/geometry/Cal3Fisheye_Base.cpp deleted file mode 100644 index 4b51276df..000000000 --- a/gtsam/geometry/Cal3Fisheye_Base.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 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; -} - -/* ************************************************************************* */ -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 deleted file mode 100644 index 0313aadb5..000000000 --- a/gtsam/geometry/Cal3Fisheye_Base.h +++ /dev/null @@ -1,180 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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_); - } - - /// @} - -}; - -} - From fd0ea0e5e4a9132a6d0ec22929528e4500264730 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Wed, 22 Apr 2020 12:28:47 -0400 Subject: [PATCH 5/6] Formatted testCal3DFIsheye.cpp --- gtsam/geometry/tests/testCal3DFisheye.cpp | 101 +++++++++++----------- 1 file changed, 52 insertions(+), 49 deletions(-) diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index fbe08a5a3..50fae5a8b 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -15,7 +15,6 @@ * @author ghaggin */ - #include #include #include @@ -26,95 +25,99 @@ 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); +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); /* ************************************************************************* */ -TEST( Cal3Fisheye, uncalibrate) -{ +TEST(Cal3Fisheye, uncalibrate1) { // Calculate the solution const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi*xi + yi*yi); + 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; + 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_sol(v[0] / v[2], v[1] / v[2]); Point2 uv = K.uncalibrate(p); CHECK(assert_equal(uv, uv_sol)); } -TEST( Cal3Fisheye, calibrate ) -{ +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.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)); + 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(-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)); + 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); } +Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) { + return k.uncalibrate(pt); +} /* ************************************************************************* */ -TEST( Cal3Fisheye, Duncalibrate1) -{ +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)); + CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_calibration(p); - CHECK(assert_equal(numerical,separate,1e-5)); + CHECK(assert_equal(numerical, separate, 1e-5)); } /* ************************************************************************* */ -TEST( Cal3Fisheye, Duncalibrate2) -{ - Matrix computed; +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)); + CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_intrinsic(p); - CHECK(assert_equal(numerical,separate,1e-5)); + CHECK(assert_equal(numerical, separate, 1e-5)); } /* ************************************************************************* */ -TEST( Cal3Fisheye, assert_equal) -{ - CHECK(assert_equal(K,K,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); +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; + 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)); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From d827d3ebadd9d3f38b699028b2719c35e2211170 Mon Sep 17 00:00:00 2001 From: Glen Haggin Date: Wed, 22 Apr 2020 13:51:01 -0400 Subject: [PATCH 6/6] Added test cases for fisheye calibration verified in OpenCv --- gtsam/geometry/Cal3Fisheye.cpp | 3 +- gtsam/geometry/tests/testCal3DFisheye.cpp | 85 ++++++++++++++++++++++- 2 files changed, 86 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 97931f588..c6b43004e 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -105,7 +105,8 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, 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; + const double td_o_r = r > 1e-8 ? td / r : 1; + const double xd = td_o_r * xi, yd = td_o_r * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); Matrix2 DK; diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 50fae5a8b..9203b5438 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace gtsam; @@ -49,7 +50,63 @@ TEST(Cal3Fisheye, uncalibrate1) { CHECK(assert_equal(uv, uv_sol)); } -TEST(Cal3Fisheye, calibrate) { +/* ************************************************************************* */ +/** + * 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))); +} + +/* ************************************************************************* */ +/** + * 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 +/* +=========================================================== + +import numpy as np +import cv2 + +objpts = np.float64([[23,27,31]]).reshape(1,-1,3) + +cameraMatrix = np.float64([ + [250, 0, 320], + [0, 260, 240], + [0,0,1] +]) +alpha = 0.1/250 +distCoeffs = np.float64([-0.013721808247486035, 0.020727425669427896,-0.012786476702685545, 0.0025242267320687625]) + +rvec = np.float64([[0.,0.,0.]]) +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]]] + */ +// clang-format on +TEST(Cal3Fisheye, uncalibrate3) { + Point3 p3(23, 27, 31); + Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); + auto uv = K.uncalibrate(xi); + CHECK(assert_equal(uv, Point2(457.82638130304935, 408.18905848512986))); +} + +/* ************************************************************************* */ +TEST(Cal3Fisheye, calibrate1) { Point2 pi; Point2 uv; Point2 pi_hat; @@ -76,6 +133,32 @@ TEST(Cal3Fisheye, calibrate) { CHECK(traits::Equals(pi, pi_hat, 1e-5)); } +/* ************************************************************************* */ +/** + * 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) + */ +TEST(Cal3Fisheye, calibrate3) { + Point3 p3(23, 27, 31); + Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); + Point2 uv(457.82638130304935, 408.18905848512986); + auto xi_hat = K.calibrate(uv); + CHECK(assert_equal(xi_hat, xi)); +} + +/* ************************************************************************* */ +// For numerical derivatives Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }