Refactor Cal3_S2 and Cal3_S2Stereo classes

gtsam/geometry/Cal3_S2.cpp

@@ -24,67 +24,29 @@
 namespace gtsam {
 using namespace std;
 
-/* ************************************************************************* */
-Cal3_S2::Cal3_S2(double fov, int w, int h) :
-    s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
-  double a = fov * M_PI / 360.0; // fov/2 in radians
-  fx_ = (double) w / (2.0 * tan(a)); //    old formula: fx_ = (double) w * tan(a);
-  fy_ = fx_;
-}
-
-/* ************************************************************************* */
-Cal3_S2::Cal3_S2(const std::string &path) :
-    fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
-
-  char buffer[200];
-  buffer[0] = 0;
-  sprintf(buffer, "%s/calibration_info.txt", path.c_str());
-  std::ifstream infile(buffer, std::ios::in);
-
-  if (infile)
-    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
-  else {
-    throw std::runtime_error("Cal3_S2: Unable to load the calibration");
-  }
-
-  infile.close();
-}
-
 /* ************************************************************************* */
 ostream& operator<<(ostream& os, const Cal3_S2& cal) {
-  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
+  os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew()
-     << ", py:" << cal.py() << "}";
+     << ", px:" << cal.px() << ", py:" << cal.py() << "}";
   return os;
 }
 
 /* ************************************************************************* */
 void Cal3_S2::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
+  gtsam::print((Matrix)K(), s);
 }
 
 /* ************************************************************************* */
 bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol)
+  return Cal3::equals(K, tol);
-    return false;
-  if (std::abs(fy_ - K.fy_) > tol)
-    return false;
-  if (std::abs(s_ - K.s_) > tol)
-    return false;
-  if (std::abs(u0_ - K.u0_) > tol)
-    return false;
-  if (std::abs(v0_ - K.v0_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
 Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
                             OptionalJacobian<2, 2> Dp) const {
   const double x = p.x(), y = p.y();
-  if (Dcal)
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-    *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
-  if (Dp)
-    *Dp << fx_, s_, 0.0, fy_;
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
@@ -94,22 +56,19 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
   const double u = p.x(), v = p.y();
   double delta_u = u - u0_, delta_v = v - v0_;
   double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
-    double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+  double inv_fy_delta_v = inv_fy * delta_v,
-    Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
+         inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
-                  inv_fy_delta_v);
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
   if (Dcal)
-       *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,  -inv_fx * point.y(),
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
-            -inv_fx,  inv_fx_s_inv_fy,
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
-            0, -inv_fy * point.y(), 0,  0, -inv_fy;
+        0, 0, -inv_fy;
-    if(Dp)
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
-        *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
   return point;
 }
 
 /* ************************************************************************* */
-Vector3 Cal3_S2::calibrate(const Vector3& p) const {
+Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; }
-  return matrix_inverse() * p;
-}
 
 /* ************************************************************************* */
 

gtsam/geometry/Cal3_S2.h

@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -30,31 +31,24 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3_S2 {
+class GTSAM_EXPORT Cal3_S2 : public Cal3 {
-private:
-  double fx_, fy_, s_, u0_, v0_;
-
  public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
+  typedef boost::shared_ptr<Cal3_S2>
+      shared_ptr;  ///< shared pointer to calibration object
 
   /// @name Standard Constructors
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3_S2() :
+  Cal3_S2() : Cal3() {}
-      fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
-  }
 
   /// constructor from doubles
-  Cal3_S2(double fx, double fy, double s, double u0, double v0) :
+  Cal3_S2(double fx, double fy, double s, double u0, double v0)
-      fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
+      : Cal3(fx, fy, s, u0, v0) {}
-  }
 
   /// constructor from vector
-  Cal3_S2(const Vector &d) :
+  Cal3_S2(const Vector& d) : Cal3(d) {}
-      fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
-  }
 
   /**
    * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@@ -62,99 +56,12 @@ public:
    * @param w image width
    * @param h image height
    */
-  Cal3_S2(double fov, int w, int h);
+  Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
-
-  /// @}
-  /// @name Advanced Constructors
-  /// @{
-
-  /// load calibration from location (default name is calibration_info.txt)
-  Cal3_S2(const std::string &path);
-
-  /// @}
-  /// @name Testable
-  /// @{
-
-  /// Output stream operator
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
-
-  /// print with optional string
-  void print(const std::string& s = "Cal3_S2") const;
-
-  /// Check if equal up to specified tolerance
-  bool equals(const Cal3_S2& K, double tol = 10e-9) const;
-
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /// focal length x
-  inline double fx() const {
-    return fx_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return fy_;
-  }
-
-  /// aspect ratio
-  inline double aspectRatio() const {
-    return fx_/fy_;
-  }
-
-  /// skew
-  inline double skew() const {
-    return s_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
-  /// return the principal point
-  Point2 principalPoint() const {
-    return Point2(u0_, v0_);
-  }
-
-  /// vectorized form (column-wise)
-  Vector5 vector() const {
-    Vector5 v;
-    v << fx_, fy_, s_, u0_, v0_;
-    return v;
-  }
-
-  /// return calibration matrix K
-  Matrix3 K() const {
-    Matrix3 K;
-    K <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-  }
-
-  /** @deprecated The following function has been deprecated, use K above */
-  Matrix3 matrix() const {
-    return K();
-  }
-
-  /// return inverted calibration matrix inv(K)
-  Matrix3 matrix_inverse() const {
-    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    Matrix3 K_inverse;
-    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
-        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
-    return K_inverse;
-  }
 
   /**
-   * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
@@ -162,9 +69,9 @@ public:
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /**
-   * convert image coordinates uv to intrinsic coordinates xy
+   * Convert image coordinates uv to intrinsic coordinates xy
    * @param p point in image coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
@@ -172,22 +79,36 @@ public:
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /**
-   * convert homogeneous image coordinates to intrinsic coordinates
+   * Convert homogeneous image coordinates to intrinsic coordinates
    * @param p point in image coordinates
    * @return point in intrinsic coordinates
    */
   Vector3 calibrate(const Vector3& p) const;
 
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2& cal);
+
+  /// print with optional string
+  void print(const std::string& s = "Cal3_S2") const override;
+
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3_S2& K, double tol = 10e-9) const;
+
   /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
   inline Cal3_S2 between(const Cal3_S2& q,
                          OptionalJacobian<5, 5> H1 = boost::none,
                          OptionalJacobian<5, 5> H2 = boost::none) const {
     if (H1) *H1 = -I_5x5;
     if (H2) *H2 = I_5x5;
-    return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
+    return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
+                   q.v0_ - v0_);
   }
 
-
   /// @}
   /// @name Manifold
   /// @{
@@ -213,20 +134,15 @@ public:
   /// @{
 
  private:
-
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(fy_);
+        "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(s_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
   }
 
   /// @}
-
 };
 
 template <>

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -24,14 +24,15 @@ using namespace std;
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
-     K_.print(s+"K: ");
+  std::cout << s << (s != "" ? " " : "");
-     std::cout << s << "Baseline: " << b_ << std::endl;
+  print("K: ");
+  std::cout << "Baseline: " << b_ << std::endl;
 }
 
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
-  if (std::abs(b_ - other.b_) > tol) return false;
+  const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
-  return K_.equals(other.K_,tol);
+  return Cal3_S2::equals(*base, tol) && (std::abs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3_S2Stereo.h

@@ -27,43 +27,40 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Cal3_S2Stereo {
+  class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   private:
-
-    Cal3_S2 K_;
     double b_;
 
   public:
 
     enum { dimension = 6 };
-    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;  ///< shared pointer to stereo calibration object
+    ///< shared pointer to stereo calibration object
+    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;
 
     /// @name Standard Constructors
     /// @
 
     /// default calibration leaves coordinates unchanged
-    Cal3_S2Stereo() :
+    Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {}
-      K_(1, 1, 0, 0, 0), b_(1.0) {
-    }
 
     /// constructor from doubles
-    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
+    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0,
-      K_(fx, fy, s, u0, v0), b_(b) {
+                  double b)
-    }
+        : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
 
     /// constructor from vector
-    Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
+    Cal3_S2Stereo(const Vector& d)
+        : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
 
     /// easy constructor; field-of-view in degrees, assumes zero skew
-    Cal3_S2Stereo(double fov, int w, int h, double b) :
+    Cal3_S2Stereo(double fov, int w, int h, double b)
-      K_(fov, w, h), b_(b) {
+        : Cal3_S2(fov, w, h), b_(b) {}
-    }
 
     /// @}
     /// @name Testable
     /// @{
 
-    void print(const std::string& s = "") const;
+    void print(const std::string& s = "") const override;
 
     /// Check if equal up to specified tolerance
     bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
@@ -73,28 +70,10 @@ namespace gtsam {
     /// @{
 
     /// return calibration, same for left and right
-    const Cal3_S2& calibration() const { return K_;}
+    const Cal3_S2& calibration() const { return *this; }
 
     /// return calibration matrix K, same for left and right
-    Matrix matrix() const { return K_.matrix();}
+    Matrix3 K() const { return K(); }
-
-    /// focal length x
-    inline double fx() const { return K_.fx();}
-
-    /// focal length x
-    inline double fy() const { return K_.fy();}
-
-    /// skew
-    inline double skew() const { return K_.skew();}
-
-    /// image center in x
-    inline double px() const { return K_.px();}
-
-    /// image center in y
-    inline double py() const { return K_.py();}
-
-    /// return the principal point
-    Point2 principalPoint() const { return K_.principalPoint();}
 
     /// return baseline
     inline double baseline() const { return b_; }
@@ -102,7 +81,7 @@ namespace gtsam {
     /// vectorized form (column-wise)
     Vector6 vector() const {
       Vector6 v;
-      v << K_.vector(), b_;
+      v << vector(), b_;
       return v;
     }
 
@@ -118,7 +97,8 @@ namespace gtsam {
 
     /// Given 6-dim tangent vector, create new calibration
     inline Cal3_S2Stereo retract(const Vector& d) const {
-      return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
+      return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
+                           py() + d(4), b_ + d(5));
     }
 
     /// Unretraction for the calibration
@@ -137,7 +117,8 @@ namespace gtsam {
     template<class Archive>
     void serialize(Archive & ar, const unsigned int /*version*/)
     {
-      ar & BOOST_SERIALIZATION_NVP(K_);
+      ar& boost::serialization::make_nvp(
+          "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
       ar& BOOST_SERIALIZATION_NVP(b_);
     }
     /// @}

gtsam/geometry/tests/testCal3_S2.cpp

@@ -125,7 +125,6 @@ TEST(Cal3_S2, between) {
   EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
   EXPECT(assert_equal(-I_5x5, H1));
   EXPECT(assert_equal(I_5x5, H2));
-
 }
 
 /* ************************************************************************* */