From d0579dff50221c9fa79eed0456137ad31af3f253 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 15 Jan 2015 16:17:53 +0100 Subject: [PATCH] Wrapping Cal3Unified as a derived class of Cal3DS2_Base --- gtsam.h | 79 +++++++++++++++++++++++++---------- gtsam/geometry/Cal3DS2.h | 14 ++++++- gtsam/geometry/Cal3DS2_Base.h | 28 ++++++++++--- gtsam/geometry/Cal3Unified.h | 10 +++-- 4 files changed, 100 insertions(+), 31 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1fbc0f907..c5528309e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -629,28 +629,13 @@ class Cal3_S2 { void serialize() const; }; -#include -class Cal3DS2 { +#include +virtual class Cal3DS2_Base { // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); - Cal3DS2(Vector v); + Cal3DS2_Base(); // Testable void print(string s) const; - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // TODO: D2d functions that start with an uppercase letter // Standard Interface double fx() const; @@ -658,14 +643,66 @@ class Cal3DS2 { double skew() const; double px() const; double py() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; // enabling serialization functionality void serialize() const; }; +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include class Cal3_S2Stereo { // Standard Constructors Cal3_S2Stereo(); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0c77eebbc..2fb0c8653 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -68,7 +68,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; @@ -89,10 +89,20 @@ public: /// 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 Cal3DS2(*this)); + } + + /// @} + private: - /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 37c156743..d4f4cabe5 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,9 +45,6 @@ protected: double p1_, p2_ ; // tangential distortion public: - Matrix3 K() const ; - Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } - Vector9 vector() const ; /// @name Standard Constructors /// @{ @@ -59,6 +56,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + virtual ~Cal3DS2_Base() {} + /// @} /// @name Advanced Constructors /// @{ @@ -70,7 +69,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; @@ -106,6 +105,15 @@ public: /// Second tangential distortion coefficient inline double p2() const { return p2_;} + /// return calibration matrix -- not really applicable + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + + /// Return all parameters as a vector + Vector9 vector() const; + /** * convert intrinsic coordinates xy to (distorted) image coordinates uv * @param p point in intrinsic coordinates @@ -126,9 +134,19 @@ public: /// Derivative of uncalibrate wrpt the calibration parameters Matrix29 D2d_calibration(const Point2& p) const ; -private: + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3DS2_Base(*this)); + } /// @} + +private: + /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d0e0f3891..8e4394c0d 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,9 +50,8 @@ private: double xi_; // mirror parameter public: - enum { dimension = 10 }; - Vector10 vector() const ; + enum { dimension = 10 }; /// @name Standard Constructors /// @{ @@ -77,7 +76,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const ; + virtual void print(const std::string& s = "") const ; /// assert equality up to a tolerance bool equals(const Cal3Unified& K, double tol = 10e-9) const; @@ -125,6 +124,11 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 10; } //TODO: make a final dimension variable + /// Return all parameters as a vector + Vector10 vector() const ; + + /// @} + private: /** Serialization function */