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/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 8e200ff2a..86a82ff4c 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -26,22 +26,21 @@ namespace gtsam { /* ************************************************************************* */ Cal3Unify::Cal3Unify(const Vector &v): - xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} + Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ Matrix Cal3Unify::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); + return Base::K(); } /* ************************************************************************* */ Vector Cal3Unify::vector() const { - return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); + return (Vector(10) << Base::vector(), xi_); } /* ************************************************************************* */ void Cal3Unify::print(const std::string& s) const { - gtsam::print(K(), s + ".K"); - gtsam::print(Vector(k()), s + ".k"); + Base::print(s); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } @@ -119,7 +118,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); - *H1 = collect(2, &DDS2U, &DDS2V); + *H1 = collect(2, &DDS2V, &DDS2U); } // Inlined derivative for points if (H2) { diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h index 4d70db051..37b291994 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unify.h @@ -22,7 +22,7 @@ #pragma once -#include +#include #include namespace gtsam { @@ -32,14 +32,14 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unify : public DerivedValue { +class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 { + + typedef Cal3Unify This; + typedef Cal3DS2 Base; private: double xi_; // mirror parameter - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double k3_, k4_ ; // tangential distortion // 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})] @@ -50,18 +50,18 @@ private: public: Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Eigen::Vector4d k() const { return Base::k(); } Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + Cal3Unify() : Cal3DS2(), xi_(0) {} - Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0) : - xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + Cal3Unify(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) : + Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} /// @} /// @name Advanced Constructors @@ -150,7 +150,6 @@ private: { ar & boost::serialization::make_nvp("Cal3Unify", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(xi_); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); @@ -160,6 +159,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k3_); ar & BOOST_SERIALIZATION_NVP(k4_); + ar & BOOST_SERIALIZATION_NVP(xi_); } diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 370877ef3..015cff237 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -32,7 +32,7 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Cal3Unify 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); /* ************************************************************************* */ @@ -67,7 +67,7 @@ TEST( Cal3Unify, 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-6)); } /* ************************************************************************* */ @@ -76,7 +76,7 @@ TEST( Cal3Unify, 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-6)); } /* ************************************************************************* */ @@ -88,10 +88,10 @@ TEST( Cal3Unify, assert_equal) /* ************************************************************************* */ TEST( Cal3Unify, retract) { - Cal3Unify expected(0.1 + 1, 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); + Cal3Unify 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 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; + d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unify actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));