diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unified.cpp similarity index 84% rename from gtsam/geometry/Cal3Unify.cpp rename to gtsam/geometry/Cal3Unified.cpp index e837eff5d..d2a2be287 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -18,34 +18,34 @@ #include #include #include -#include +#include #include namespace gtsam { /* ************************************************************************* */ -Cal3Unify::Cal3Unify(const Vector &v): - Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} +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]) {} /* ************************************************************************* */ -Matrix Cal3Unify::K() const { +Matrix Cal3Unified::K() const { return Base::K(); } /* ************************************************************************* */ -Vector Cal3Unify::vector() const { +Vector Cal3Unified::vector() const { return (Vector(10) << Base::vector(), xi_); } /* ************************************************************************* */ -void Cal3Unify::print(const std::string& s) const { +void Cal3Unified::print(const std::string& s) const { Base::print(s); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } /* ************************************************************************* */ -bool Cal3Unify::equals(const Cal3Unify& K, double tol) const { +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 || @@ -55,7 +55,7 @@ bool Cal3Unify::equals(const Cal3Unify& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3Unify::uncalibrate(const Point2& p, +Point2 Cal3Unified::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -103,7 +103,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, } /* ************************************************************************* */ -Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { // calibrate point to Nplane use base class::calibrate() Point2 pnplane = Base::calibrate(pi, tol); @@ -112,7 +112,7 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { return this->nPlaneToSpace(pnplane); } /* ************************************************************************* */ -Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { +Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { const double x = p.x(), y = p.y(); const double xy2 = x * x + y * y; @@ -122,7 +122,7 @@ Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { } /* ************************************************************************* */ -Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { +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); @@ -131,12 +131,12 @@ Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { } /* ************************************************************************* */ -Cal3Unify Cal3Unify::retract(const Vector& d) const { - return Cal3Unify(vector() + d); +Cal3Unified Cal3Unified::retract(const Vector& d) const { + return Cal3Unified(vector() + d); } /* ************************************************************************* */ -Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const { +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unified.h similarity index 89% rename from gtsam/geometry/Cal3Unify.h rename to gtsam/geometry/Cal3Unified.h index 37b291994..d8c0251f0 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unified.h @@ -32,9 +32,9 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : protected Cal3DS2 { - typedef Cal3Unify This; + typedef Cal3Unified This; typedef Cal3DS2 Base; private: @@ -57,17 +57,17 @@ public: /// @{ /// Default Constructor with only unit focal length - Cal3Unify() : Cal3DS2(), xi_(0) {} + Cal3Unified() : Base(), xi_(0) {} - Cal3Unify(double fx, double fy, double s, double u0, double v0, + 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) : - Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} /// @} /// @name Advanced Constructors /// @{ - Cal3Unify(const Vector &v) ; + Cal3Unified(const Vector &v) ; /// @} /// @name Testable @@ -77,7 +77,7 @@ public: void print(const std::string& s = "") const ; /// assert equality up to a tolerance - bool equals(const Cal3Unify& K, double tol = 10e-9) const; + bool equals(const Cal3Unified& K, double tol = 10e-9) const; /// @} /// @name Standard Interface @@ -126,10 +126,10 @@ public: /// @{ /// Given delta vector, update calibration - Cal3Unify retract(const Vector& d) const ; + Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unify& T2) const ; + 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) @@ -148,7 +148,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unify", + ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index c36b756e3..b260415f1 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -17,12 +17,12 @@ #include #include #include -#include +#include using namespace gtsam; -GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) -GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified) /* ground truth from matlab, code : @@ -32,11 +32,11 @@ 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(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +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( Cal3Unify, uncalibrate) +TEST( Cal3Unified, uncalibrate) { Point2 p_i(364.7791831734982, 305.6677211952602) ; Point2 q = K.uncalibrate(p); @@ -44,7 +44,7 @@ TEST( Cal3Unify, uncalibrate) } /* ************************************************************************* */ -TEST( Cal3Unify, spaceNplane) +TEST( Cal3Unified, spaceNplane) { Point2 q = K.spaceToNPlane(p); CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); @@ -52,17 +52,17 @@ TEST( Cal3Unify, spaceNplane) } /* ************************************************************************* */ -TEST( Cal3Unify, calibrate) +TEST( Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); CHECK( p.equals(pn_hat, 1e-8)); } -Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Unify, Duncalibrate1) +TEST( Cal3Unified, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); @@ -71,7 +71,7 @@ TEST( Cal3Unify, Duncalibrate1) } /* ************************************************************************* */ -TEST( Cal3Unify, Duncalibrate2) +TEST( Cal3Unified, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); @@ -80,19 +80,19 @@ TEST( Cal3Unify, Duncalibrate2) } /* ************************************************************************* */ -TEST( Cal3Unify, assert_equal) +TEST( Cal3Unified, assert_equal) { CHECK(assert_equal(K,K,1e-9)); } /* ************************************************************************* */ -TEST( Cal3Unify, retract) +TEST( Cal3Unified, retract) { - Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 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; - Cal3Unify actual = K.retract(d); + Cal3Unified actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-9)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); }