diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 5453e1481..ced05086b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -30,7 +30,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { -private: +protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp new file mode 100644 index 000000000..39ad221cb --- /dev/null +++ b/gtsam/geometry/Cal3Unified.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unify.cpp + * @date Mar 8, 2014 + * @author Jing Dong + */ + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Unified::Cal3Unified(const Vector &v): + Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} + +/* ************************************************************************* */ +Vector Cal3Unified::vector() const { + return (Vector(10) << Base::vector(), xi_); +} + +/* ************************************************************************* */ +void Cal3Unified::print(const std::string& s) const { + Base::print(s); + gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); +} + +/* ************************************************************************* */ +bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || + fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || + fabs(xi_ - K.xi_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Unified::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + // this part of code is modified from Cal3DS2, + // since the second part of this model (after project to normalized plane) + // is same as Cal3DS2 + + // parameters + const double xi = xi_; + + // Part1: project 3D space to NPlane + const double xs = p.x(), ys = p.y(); // normalized points in 3D space + const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); + const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx); + const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; + const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane + + // Part2: project NPlane point to pixel plane: use Cal3DS2 + Point2 m(x,y); + Matrix H1base, H2base; // jacobians from Base class + Point2 puncalib = Base::uncalibrate(m, H1base, H2base); + + // Inlined derivative for calibration + if (H1) { + // part1 + Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, + -ys * sqrt_nx * xi_sqrt_nx2); + Matrix DDS2U = H2base * DU; + + *H1 = collect(2, &H1base, &DDS2U); + } + // Inlined derivative for points + if (H2) { + // part1 + const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; + const double mid = -(xi * xs*ys) * denom; + Matrix DU = (Matrix(2, 2) << + (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); + + *H2 = H2base * DU; + } + + return puncalib; +} + +/* ************************************************************************* */ +Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { + + // calibrate point to Nplane use base class::calibrate() + Point2 pnplane = Base::calibrate(pi, tol); + + // call nplane to space + return this->nPlaneToSpace(pnplane); +} +/* ************************************************************************* */ +Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double xy2 = x * x + y * y; + const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); + + return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); +} + +/* ************************************************************************* */ +Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); + + return Point2((x / sq_xy), (y / sq_xy)); +} + +/* ************************************************************************* */ +Cal3Unified Cal3Unified::retract(const Vector& d) const { + return Cal3Unified(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h new file mode 100644 index 000000000..6961757bd --- /dev/null +++ b/gtsam/geometry/Cal3Unified.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unify.h + * @brief Unified Calibration Model, see Mei07icra for details + * @date Mar 8, 2014 + * @author Jing Dong + */ + +/** + * @addtogroup geometry + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { + + typedef Cal3Unified This; + typedef Cal3DS2 Base; + +private: + + double xi_; // mirror parameter + + // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] + // rr = Pn.x^2 + Pn.y^2 + // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + // pi = K*pn + +public: + //Matrix K() const ; + //Eigen::Vector4d k() const { return Base::k(); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Unified() : Base(), xi_(0) {} + + Cal3Unified(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : + Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Unified(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Unified& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// mirror parameter + inline double xi() const { return xi_;} + + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Convert a 3D point to normalized unit plane + Point2 spaceToNPlane(const Point2& p) const; + + /// Convert a normalized unit plane point to 3D space + Point2 nPlaneToSpace(const Point2& p) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Unified retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Unified& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 10 ; } //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 10; } //TODO: make a final dimension variable + +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("Cal3Unified", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(xi_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp new file mode 100644 index 000000000..b260415f1 --- /dev/null +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3Unify.cpp + * @brief Unit tests for transform derivatives + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified) + +/* +ground truth from matlab, code : +X = [0.5 0.7 1]'; +V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +[P, J] = spaceToImgPlane(X, V); +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox +*/ + +static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +static Point2 p(0.5, 0.7); + +/* ************************************************************************* */ +TEST( Cal3Unified, uncalibrate) +{ + Point2 p_i(364.7791831734982, 305.6677211952602) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,p_i)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, spaceNplane) +{ + Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); + CHECK(assert_equal(p, K.nPlaneToSpace(q))); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, calibrate) +{ + Point2 pi = K.uncalibrate(p); + Point2 pn_hat = K.calibrate(pi); + CHECK( p.equals(pn_hat, 1e-8)); +} + +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3Unified, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-6)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, Duncalibrate2) +{ + Matrix computed; + K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-6)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, assert_equal) +{ + CHECK(assert_equal(K,K,1e-9)); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, retract) +{ + Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); + Vector d(10); + d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; + Cal3Unified actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-9)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */