Wrapping Cal3Unified as a derived class of Cal3DS2_Base

gtsam.h

@@ -629,28 +629,13 @@ class Cal3_S2 {
   void serialize() const;
 };
 
-#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
-class Cal3DS2 {
+virtual class Cal3DS2_Base {
   // Standard Constructors
-  Cal3DS2();
+  Cal3DS2_Base();
-  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
-  Cal3DS2(Vector v);
 
   // 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;
+  double k1() const;
-  Vector k() const;
+  double k2() const;
-  //Matrix K() const; //FIXME: Uppercase
+
+  // 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 <gtsam/geometry/Cal3DS2.h>
+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 <gtsam/geometry/Cal3Unified.h>
+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 <gtsam/geometry/Cal3_S2Stereo.h>
 class Cal3_S2Stereo {
   // Standard Constructors
   Cal3_S2Stereo();

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<Base> clone() const {
+    return boost::shared_ptr<Base>(new Cal3DS2(*this));
+  }
+
+  /// @}
+
 
 private:
 
-  /// @}
   /// @name Advanced Interface
   /// @{
 

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<Cal3DS2_Base> clone() const {
+    return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
+  }
 
   /// @}
+
+private:
+
   /// @name Advanced Interface
   /// @{
 

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 */