Merge pull request #626 from borglab/feature/cal3

Cal3 Base Class

gtsam/geometry/Cal3.cpp

@@ -0,0 +1,75 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Cal3.cpp
+ * @brief  Common code for all calibration models.
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Cal3.h>
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Cal3::Cal3(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));
+  fy_ = fx_;
+}
+
+/* ************************************************************************* */
+Cal3::Cal3(const std::string& path) {
+  const auto buffer = path + std::string("/calibration_info.txt");
+  std::ifstream infile(buffer, std::ios::in);
+
+  if (infile && !infile.eof()) {
+    infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
+  } else {
+    throw std::runtime_error("Cal3: Unable to load the calibration");
+  }
+
+  infile.close();
+}
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
+}
+
+/* ************************************************************************* */
+void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
+
+/* ************************************************************************* */
+bool Cal3::equals(const Cal3& K, double tol) const {
+  return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
+          std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
+          std::fabs(v0_ - K.v0_) < tol);
+}
+
+Matrix3 Cal3::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;
+}
+
+/* ************************************************************************* */
+
+}  // \ namespace gtsam

gtsam/geometry/Cal3.h

@@ -61,6 +61,143 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
   }
 }
 
-//TODO(Varun) Make common base class for all calibration models.
+/**
+ * @brief Common base class for all calibration models.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT Cal3 {
+ protected:
+  double fx_ = 1.0f, fy_ = 1.0f;  ///< focal length
+  double s_ = 0.0f;               ///< skew
+  double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
+
+ public:
+  enum { dimension = 5 };
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3>;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// Create a default calibration that leaves coordinates unchanged
+  Cal3() = default;
+
+  /// constructor from doubles
+  Cal3(double fx, double fy, double s, double u0, double v0)
+      : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
+
+  /// constructor from vector
+  Cal3(const Vector5& 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
+   * @param fov field of view in degrees
+   * @param w image width
+   * @param h image height
+   */
+  Cal3(double fov, int w, int h);
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /**
+   * Load calibration parameters from `calibration_info.txt` file located in
+   * `path` directory.
+   *
+   * The contents of calibration file should be the 5 parameters in order:
+   * `fx, fy, s, u0, v0`
+   *
+   * @param path path to directory containing `calibration_info.txt`.
+   */
+  Cal3(const std::string& path);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3& cal);
+
+  /// print with optional string
+  virtual void print(const std::string& s = "") const;
+
+  /// Check if equal up to specified tolerance
+  bool equals(const Cal3& 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
+  virtual Matrix3 K() const {
+    Matrix3 K;
+    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
+  }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
+  /** @deprecated The following function has been deprecated, use K above */
+  Matrix3 matrix() const { return K(); }
+#endif
+
+  /// Return inverted calibration matrix inv(K)
+  Matrix3 inverse() const;
+
+  /// return DOF, dimensionality of tangent space
+  inline virtual size_t dim() const { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+ 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_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(v0_);
+  }
+
+  /// @}
+};
 
 }  // \ namespace gtsam

gtsam/geometry/Cal3Bundler.cpp

@@ -15,28 +15,19 @@
  * @author ydjian
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3Bundler.h>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler() :
-    f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
-}
-
-/* ************************************************************************* */
-Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
-                         double tol)
-    : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
-
 /* ************************************************************************* */
 Matrix3 Cal3Bundler::K() const {
+  // This function is needed to ensure skew = 0;
   Matrix3 K;
-  K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
+  K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0;
   return K;
 }
 
@@ -48,35 +39,42 @@ Vector4 Cal3Bundler::k() const {
 }
 
 /* ************************************************************************* */
-Vector3 Cal3Bundler::vector() const {
+Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
-  return Vector3(f_, k1_, k2_);
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+  return os;
 }
 
 /* ************************************************************************* */
 void Cal3Bundler::print(const std::string& s) const {
-  gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
+  gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(),
+               s + ".K");
 }
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
+  return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      || std::abs(v0_ - K.v0_) > tol)
+          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
-    return false;
+          std::fabs(v0_ - K.v0_) < tol);
-  return true;
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
+Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
-    OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
+                                OptionalJacobian<2, 2> Dp) const {
-  //  r = x^2 + y^2;
+  //  r = x² + y²;
-  //  g = (1 + k(1)*r + k(2)*r^2);
+  //  g = (1 + k(1)*r + k(2)*r²);
   //  pi(:,i) = g * pn(:,i)
   const double x = p.x(), y = p.y();
   const double r = x * x + y * y;
   const double g = 1. + (k1_ + k2_ * r) * r;
   const double u = g * x, v = g * y;
 
+  const double f_ = fx_;
+
   // Derivatives make use of intermediate variables above
   if (Dcal) {
     double rx = r * x, ry = r * y;
@@ -94,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 }
 
 /* ************************************************************************* */
-Point2 Cal3Bundler::calibrate(const Point2& pi,
+Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
-                              OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
-  // Copied from Cal3DS2 :-(
+  // Copied from Cal3DS2
-  // but specialized with k1,k2 non-zero only and fx=fy and s=0
+  // but specialized with k1, k2 non-zero only and fx=fy and s=0
-  double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
+  double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
   const Point2 invKPi(x, y);
 
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  // initialize by ignoring the distortion at all, might be problematic for
+  // pixels around boundary
   Point2 pn(x, y);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
   for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol_)
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-      break;
     const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
     const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
@@ -119,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
-        "Cal3Bundler::calibrate fails to converge. need a better initialization");
+        "Cal3Bundler::calibrate fails to converge. need a better "
+        "initialization");
 
   calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
 
@@ -150,14 +148,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-/* ************************************************************************* */
+}  // \ namespace gtsam
-Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
-  return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
-}
-
-/* ************************************************************************* */
-Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
-  return T2.vector() - vector();
-}
-
-}

gtsam/geometry/Cal3Bundler.h

@@ -14,6 +14,7 @@
  * @brief Calibration used by Bundler
  * @date Sep 25, 2010
  * @author Yong Dian Jian
+ * @author Varun Agrawal
  */
 
 #pragma once
@@ -28,23 +29,23 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler {
+class GTSAM_EXPORT Cal3Bundler : public Cal3 {
+ private:
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
-private:
+  // NOTE: We use the base class fx to represent the common focal length.
-  double f_; ///< focal length
+  // Also, image center parameters (u0, v0) are not optimized
-  double k1_, k2_; ///< radial distortion
+  // but are treated as constants.
-  double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
-  double tol_; ///< tolerance value when calibrating
-
-public:
 
+ public:
   enum { dimension = 3 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default constructor
-  Cal3Bundler();
+  Cal3Bundler() = default;
 
   /**
    *  Constructor
@@ -56,7 +57,8 @@ public:
    *  @param tol optional calibration tolerance value
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
-              double tol = 1e-5);
+              double tol = 1e-5)
+      : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
 
   virtual ~Cal3Bundler() {}
 
@@ -64,8 +66,12 @@ public:
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Bundler& cal);
+
   /// print with optional string
-  void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
@@ -74,59 +80,36 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  /// distorsion parameter k1
+  inline double k1() const { return k1_; }
+
+  /// distorsion parameter k2
+  inline double k2() const { return k2_; }
+
+  /// image center in x
+  inline double px() const { return u0_; }
+
+  /// image center in y
+  inline double py() const { return v0_; }
+
+  Matrix3 K() const override;  ///< Standard 3*3 calibration matrix
   Vector4 k() const;  ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
 
-  /// focal length x
-  inline double fx() const {
-    return f_;
-  }
-
-  /// focal length y
-  inline double fy() const {
-    return f_;
-  }
-
-  /// distorsion parameter k1
-  inline double k1() const {
-    return k1_;
-  }
-
-  /// distorsion parameter k2
-  inline double k2() const {
-    return k2_;
-  }
-
-  /// image center in x
-  inline double px() const {
-    return u0_;
-  }
-
-  /// image center in y
-  inline double py() const {
-    return v0_;
-  }
-
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
-  inline double u0() const {
+  inline double u0() const { return u0_; }
-    return u0_;
-  }
 
   /// get parameter v0
-  inline double v0() const {
+  inline double v0() const { return v0_; }
-    return v0_;
-  }
 #endif
 
-
   /**
    * @brief: convert intrinsic coordinates xy to image coordinates uv
    * Version of uncalibrate with derivatives
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
@@ -141,8 +124,7 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& pi,
+  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
-                   OptionalJacobian<2, 3> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
@@ -158,48 +140,45 @@ public:
   /// @name Manifold
   /// @{
 
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
   /// Update calibration with tangent space delta
-  Cal3Bundler retract(const Vector& d) const;
+  inline Cal3Bundler retract(const Vector& d) const {
+    return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
+  }
 
   /// Calculate local coordinates to another calibration
-  Vector3 localCoordinates(const Cal3Bundler& T2) const;
+  Vector3 localCoordinates(const Cal3Bundler& T2) const {
-
+    return T2.vector() - vector();
-  /// dimensionality
-  virtual size_t dim() const {
-    return 3;
   }
 
-  /// dimensionality
+ private:
-  static size_t Dim() {
-    return 3;
-  }
-
-private:
-
   /// @}
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(f_);
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(k1_);
+        "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(k2_);
+    ar& BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
-    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
-template<>
+template <>
 struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
 }  // namespace gtsam

gtsam/geometry/Cal3DS2.cpp

@@ -13,28 +13,30 @@
  * @file Cal3DS2.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2.h>
 
 namespace gtsam {
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s_) const {
+std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
-  Base::print(s_);
+  os << (Cal3DS2_Base&)cal;
+  return os;
 }
 
+/* ************************************************************************* */
+void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
+
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3DS2_Base::equals(*base, tol);
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
@@ -46,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
 Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
   return T2.vector() - vector();
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2.h

@@ -11,9 +11,11 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
+ * @brief Calibration of a camera with radial distortion, calculations in base
+ * class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
+ * @autho Varun Agrawal
  */
 
 #pragma once
@@ -30,22 +32,20 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
+  using Base = Cal3DS2_Base;
 
-  typedef Cal3DS2_Base Base;
+ public:
-
-public:
-
   enum { dimension = 9 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : Base() {}
+  Cal3DS2() = default;
 
-  Cal3DS2(double fx, double fy, double s, double u0, double v0,
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) :
+          double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
 
   virtual ~Cal3DS2() {}
 
@@ -53,12 +53,16 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2(const Vector &v) : Base(v) {}
+  Cal3DS2(const Vector9& v) : Base(v) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
@@ -70,16 +74,16 @@ public:
   /// @{
 
   /// Given delta vector, update calibration
-  Cal3DS2 retract(const Vector& d) const ;
+  Cal3DS2 retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3DS2& T2) const ;
+  Vector localCoordinates(const Cal3DS2& T2) const;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
@@ -92,30 +96,24 @@ public:
 
   /// @}
 
-
+ private:
-private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3DS2",
+        "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
   }
 
   /// @}
-
 };
 
-template<>
+template <>
 struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
-template<>
+template <>
 struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
-
 }
-

gtsam/geometry/Cal3DS2_Base.cpp

@@ -16,33 +16,14 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Cal3DS2_Base.h>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
-    : fx_(v(0)),
-      fy_(v(1)),
-      s_(v(2)),
-      u0_(v(3)),
-      v0_(v(4)),
-      k1_(v(5)),
-      k2_(v(6)),
-      p1_(v(7)),
-      p2_(v(8)) {}
-
-/* ************************************************************************* */
-Matrix3 Cal3DS2_Base::K() const {
-    Matrix3 K;
-    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-    return K;
-}
-
 /* ************************************************************************* */
 Vector9 Cal3DS2_Base::vector() const {
   Vector9 v;
@@ -50,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
+     << ", p2: " << cal.p2();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3DS2_Base::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
@@ -58,17 +47,16 @@ void Cal3DS2_Base::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(p1_ - K.p1_) < tol &&
-    return false;
+         std::fabs(p2_ - K.p2_) < tol;
-  return true;
 }
 
 /* ************************************************************************* */
-static Matrix29 D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
-    double yy, double xy, double rr, double r4, double pnx, double pny,
+                               double xy, double rr, double r4, double pnx,
-    const Matrix2& DK) {
+                               double pny, const Matrix2& DK) {
   Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
   Matrix24 DR2;
@@ -80,8 +68,8 @@ static Matrix29 D2dcalibration(double x, double y, double xx,
 }
 
 /* ************************************************************************* */
-static Matrix2 D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
-    double g, double k1, double k2, double p1, double p2,
+                            double k2, double p1, double p2,
                             const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
@@ -105,9 +93,9 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
 /* ************************************************************************* */
 Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
                                  OptionalJacobian<2, 2> Dp) const {
-  //  rr = x^2 + y^2;
+  //  r² = x² + y²;
-  //  g = (1 + k(1)*rr + k(2)*rr^2);
+  //  g = (1 + k(1)*r² + k(2)*r⁴);
-  //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
+  //  dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
   //  pi(:,i) = g * pn(:,i) + dp;
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
@@ -202,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }
-
 }
 /* ************************************************************************* */
-
-

gtsam/geometry/Cal3DS2_Base.h

@@ -33,46 +33,29 @@ namespace gtsam {
  * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
  * but using only k1,k2,p1, and p2 coefficients.
  * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- * rr = Pn.x^2 + Pn.y^2
+ * r² = P.x² + P.y²
- * \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ;
+ * P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²)
- *                      p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y  ]
+ *                            p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
- * pi = K*Pn
+ * pi = K*P̂
  */
-class GTSAM_EXPORT Cal3DS2_Base {
+class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
-
+ protected:
-protected:
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< radial 2nd-order and 4th-order
- double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
+  double p1_ = 0.0f, p2_ = 0.0f;  ///< tangential distortion
- double k1_, k2_;                // radial 2nd-order and 4th-order
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
- double p1_, p2_;                // tangential distortion
- double tol_ = 1e-5;             // tolerance value when calibrating
-
-public:
 
+ public:
   enum { dimension = 9 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
- Cal3DS2_Base()
+  Cal3DS2_Base() = default;
-     : fx_(1),
-       fy_(1),
-       s_(0),
-       u0_(0),
-       v0_(0),
-       k1_(0),
-       k2_(0),
-       p1_(0),
-       p2_(0),
-       tol_(1e-5) {}
 
   Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
                double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
-     : fx_(fx),
+      : Cal3(fx, fy, s, u0, v0),
-       fy_(fy),
-       s_(s),
-       u0_(u0),
-       v0_(v0),
         k1_(k1),
         k2_(k2),
         p1_(p1),
@@ -85,14 +68,23 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2_Base(const Vector &v) ;
+  Cal3DS2_Base(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        p1_(v(7)),
+        p2_(v(8)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3DS2_Base& cal);
+
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
@@ -101,35 +93,17 @@ public:
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_;}
-
-  /// focal length x
-  inline double fy() const { return 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_;}
-
   /// First distortion coefficient
-  inline double k1() const { return k1_;}
+  inline double k1() const { return k1_; }
 
   /// Second distortion coefficient
-  inline double k2() const { return k2_;}
+  inline double k2() const { return k2_; }
 
   /// First tangential distortion coefficient
-  inline double p1() const { return p1_;}
+  inline double p1() const { return p1_; }
 
   /// Second tangential distortion coefficient
-  inline double p2() const { return p2_;}
+  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_); }
@@ -152,10 +126,16 @@ public:
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
-  Matrix2 D2d_intrinsic(const Point2& p) const ;
+  Matrix2 D2d_intrinsic(const Point2& p) const;
 
   /// Derivative of uncalibrate wrpt the calibration parameters
-  Matrix29 D2d_calibration(const Point2& p) const ;
+  Matrix29 D2d_calibration(const Point2& p) const;
+
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const override { return Dim(); }
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone
@@ -168,31 +148,23 @@ public:
 
   /// @}
 
-private:
+ private:
-
   /// @name Advanced Interface
   /// @{
 
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+        "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
-    ar & BOOST_SERIALIZATION_NVP(fy_);
+    ar& BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(s_);
+    ar& BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
+    ar& BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar& BOOST_SERIALIZATION_NVP(p2_);
-    ar & BOOST_SERIALIZATION_NVP(k1_);
+    ar& BOOST_SERIALIZATION_NVP(tol_);
-    ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(p2_);
-    ar & BOOST_SERIALIZATION_NVP(tol_);
   }
 
   /// @}
-
 };
-
 }
-

gtsam/geometry/Cal3Fisheye.cpp

@@ -13,6 +13,7 @@
  * @file Cal3Fisheye.cpp
  * @date Apr 8, 2020
  * @author ghaggin
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Matrix.h>
@@ -23,18 +24,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3Fisheye::Cal3Fisheye(const Vector9& v)
-    : fx_(v[0]),
-      fy_(v[1]),
-      s_(v[2]),
-      u0_(v[3]),
-      v0_(v[4]),
-      k1_(v[5]),
-      k2_(v[6]),
-      k3_(v[7]),
-      k4_(v[8]) {}
-
 /* ************************************************************************* */
 Vector9 Cal3Fisheye::vector() const {
   Vector9 v;
@@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const {
   return v;
 }
 
-/* ************************************************************************* */
-Matrix3 Cal3Fisheye::K() const {
-  Matrix3 K;
-  K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
-  return K;
-}
-
 /* ************************************************************************* */
 double Cal3Fisheye::Scaling(double r) {
   static constexpr double threshold = 1e-8;
@@ -157,6 +139,14 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
   return pi;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
+  os << (Cal3&)cal;
+  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
+     << ", k4: " << cal.k4();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Fisheye::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
@@ -165,24 +155,12 @@ void Cal3Fisheye::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
-      std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
+  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
-      std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+         std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
-      std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
+         std::fabs(k4_ - K.k4_) < tol;
-      std::abs(k4_ - K.k4_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
-Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
-  return Cal3Fisheye(vector() + d);
-}
 
-/* ************************************************************************* */
+}  // \ namespace gtsam
-Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
-  return T2.vector() - vector();
-}
-
-}  // namespace gtsam
-/* ************************************************************************* */

gtsam/geometry/Cal3Fisheye.h

@@ -38,40 +38,35 @@ namespace gtsam {
  * Intrinsic coordinates:
  *   [x_i;y_i] = [x/z; y/z]
  * Distorted coordinates:
- *   r^2 = (x_i)^2 + (y_i)^2
+ *   r² = (x_i)² + (y_i)²
  *   th = atan(r)
- *   th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
+ *   th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
  *   [x_d; y_d] = (th_d / r)*[x_i; y_i]
  * Pixel coordinates:
  *   K = [fx s u0; 0 fy v0 ;0 0 1]
  *   [u; v; 1] = K*[x_d; y_d; 1]
  */
-class GTSAM_EXPORT Cal3Fisheye {
+class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
  private:
-  double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
+  double k1_ = 0.0f, k2_ = 0.0f;  ///< fisheye distortion coefficients
-  double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
+  double k3_ = 0.0f, k4_ = 0.0f;  ///< fisheye distortion coefficients
-  double tol_ = 1e-5;             // tolerance value when calibrating
+  double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
-  typedef boost::shared_ptr<Cal3Fisheye>
+  ///< shared pointer to fisheye calibration object
-      shared_ptr;  ///< shared pointer to fisheye calibration object
+  using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Fisheye()
+  Cal3Fisheye() = default;
-      : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
 
   Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
               const double v0, const double k1, const double k2,
               const double k3, const double k4, double tol = 1e-5)
-      : fx_(fx),
+      : Cal3(fx, fy, s, u0, v0),
-        fy_(fy),
-        s_(s),
-        u0_(u0),
-        v0_(v0),
         k1_(k1),
         k2_(k2),
         k3_(k3),
@@ -84,27 +79,17 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Advanced Constructors
   /// @{
 
-  explicit Cal3Fisheye(const Vector9& v);
+  explicit Cal3Fisheye(const Vector9& v)
+      : Cal3(v(0), v(1), v(2), v(3), v(4)),
+        k1_(v(5)),
+        k2_(v(6)),
+        k3_(v(7)),
+        k4_(v(8)) {}
 
   /// @}
   /// @name Standard Interface
   /// @{
 
-  /// focal length x
-  inline double fx() const { return fx_; }
-
-  /// focal length x
-  inline double fy() const { return 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_; }
-
   /// First distortion coefficient
   inline double k1() const { return k1_; }
 
@@ -117,9 +102,6 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// Second tangential distortion coefficient
   inline double k4() const { return k4_; }
 
-  /// return calibration matrix
-  Matrix3 K() const;
-
   /// return distortion parameter vector
   Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
 
@@ -133,16 +115,21 @@ class GTSAM_EXPORT Cal3Fisheye {
    * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
    * coordinates [u; v]
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
-   * k4)
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
    * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
                      OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
+  /**
-  /// y_i]
+   * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
+   * y_i]
+   * @param p point in image coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
+   * @return point in intrinsic coordinates
+   */
   Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
                    OptionalJacobian<2, 2> Dp = boost::none) const;
 
@@ -150,8 +137,12 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Fisheye& cal);
+
   /// print with optional string
-  virtual void print(const std::string& s = "") const;
+  virtual void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
@@ -160,17 +151,21 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Manifold
   /// @{
 
+  /// Return dimensions of calibration manifold object
+  virtual size_t dim() const override { return Dim(); }
+
+  /// Return dimensions of calibration manifold object
+  inline static size_t Dim() { return dimension; }
+
   /// Given delta vector, update calibration
-  Cal3Fisheye retract(const Vector& d) const;
+  inline Cal3Fisheye retract(const Vector& d) const {
+    return Cal3Fisheye(vector() + d);
+  }
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Fisheye& T2) const;
+  Vector localCoordinates(const Cal3Fisheye& T2) const {
-
+    return T2.vector() - vector();
-  /// Return dimensions of calibration manifold object
+  }
-  virtual size_t dim() const { return 9; }
-
-  /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 9; }
 
   /// @}
   /// @name Clone
@@ -191,11 +186,8 @@ class GTSAM_EXPORT Cal3Fisheye {
   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_);
+        "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
-    ar& BOOST_SERIALIZATION_NVP(s_);
-    ar& BOOST_SERIALIZATION_NVP(u0_);
-    ar& BOOST_SERIALIZATION_NVP(v0_);
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
     ar& BOOST_SERIALIZATION_NVP(k3_);

gtsam/geometry/Cal3Unified.cpp

@@ -16,19 +16,15 @@
  * @author Varun Agrawal
  */
 
-#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Cal3Unified.h>
+#include <gtsam/geometry/Point2.h>
 
 #include <cmath>
 
 namespace gtsam {
 
-/* ************************************************************************* */
-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]) {}
-
 /* ************************************************************************* */
 Vector10 Cal3Unified::vector() const {
   Vector10 v;
@@ -36,6 +32,13 @@ Vector10 Cal3Unified::vector() const {
   return v;
 }
 
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
+  os << (Cal3DS2_Base&)cal;
+  os << ", xi: " << cal.xi();
+  return os;
+}
+
 /* ************************************************************************* */
 void Cal3Unified::print(const std::string& s) const {
   Base::print(s);
@@ -44,20 +47,14 @@ void Cal3Unified::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
-  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
+  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
-      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
+  return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
-      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
-      std::abs(xi_ - K.xi_) > tol)
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
-Point2 Cal3Unified::uncalibrate(const Point2& p,
+Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
-       OptionalJacobian<2,10> Dcal,
+                                OptionalJacobian<2, 2> Dp) const {
-       OptionalJacobian<2,2> Dp) const {
-
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
   // is same as Cal3DS2
@@ -73,7 +70,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx;  // points on NPlane
 
   // Part2: project NPlane point to pixel plane: use Cal3DS2
-  Point2 m(x,y);
+  Point2 m(x, y);
   Matrix29 H1base;
   Matrix2 H2base;  // jacobians from Base class
   Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
@@ -91,10 +88,10 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
   if (Dp) {
     // part1
     const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
-    const double mid = -(xi * xs*ys) * denom;
+    const double mid = -(xi * xs * ys) * denom;
     Matrix2 DU;
-    DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
+    DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid,  //
-        mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
+        mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
 
     *Dp << H2base * DU;
   }
@@ -117,7 +114,6 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
 }
 /* ************************************************************************* */
 Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
-
   const double x = p.x(), y = p.y();
   const double xy2 = x * x + y * y;
   const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@@ -127,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(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);
 
@@ -140,11 +135,10 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
+Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 
-}
 /* ************************************************************************* */
 
-
+}  // \ namespace gtsam

gtsam/geometry/Cal3Unified.h

@@ -28,40 +28,39 @@
 namespace gtsam {
 
 /**
- * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
+ * @brief Calibration of a omni-directional camera with mirror + lens radial
+ * distortion
  * @addtogroup geometry
  * \nosubgrouping
  *
  * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
  * 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})]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² +
- * rr = Pn.x^2 + Pn.y^2
+ * P.y² + 1})]
- * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ * r² = Pn.x² + Pn.y²
- *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ;
+ *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
+  using This = Cal3Unified;
+  using Base = Cal3DS2_Base;
 
-  typedef Cal3Unified This;
+ private:
-  typedef Cal3DS2_Base Base;
+  double xi_ = 0.0f;  ///< mirror parameter
-
-private:
-
-  double xi_;  // mirror parameter
-
-public:
 
+ public:
   enum { dimension = 10 };
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Unified() : Base(), xi_(0) {}
+  Cal3Unified() = default;
 
-  Cal3Unified(double fx, double fy, double s, double u0, double v0,
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
+              double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
-        Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
+      : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
 
   virtual ~Cal3Unified() {}
 
@@ -69,12 +68,17 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3Unified(const Vector &v) ;
+  Cal3Unified(const Vector10& v)
+      : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
 
   /// @}
   /// @name Testable
   /// @{
 
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3Unified& cal);
+
   /// print with optional string
   void print(const std::string& s = "") const override;
 
@@ -86,7 +90,10 @@ public:
   /// @{
 
   /// mirror parameter
-  inline double xi() const { return xi_;}
+  inline double xi() const { return xi_; }
+
+  /// Return all parameters as a vector
+  Vector10 vector() const;
 
   /**
    * convert intrinsic coordinates xy to image coordinates uv
@@ -96,8 +103,8 @@ public:
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p,
-      OptionalJacobian<2,10> Dcal = boost::none,
+                     OptionalJacobian<2, 10> Dcal = boost::none,
-      OptionalJacobian<2,2> Dp = boost::none) const ;
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
   /// Conver a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
@@ -114,41 +121,33 @@ public:
   /// @{
 
   /// Given delta vector, update calibration
-  Cal3Unified retract(const Vector& d) const ;
+  Cal3Unified retract(const Vector& d) const;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector10 localCoordinates(const Cal3Unified& T2) const ;
+  Vector localCoordinates(const Cal3Unified& T2) const;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return dimension ; }
+  virtual size_t dim() const override { return Dim(); }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
-
-  /// Return all parameters as a vector
-  Vector10 vector() const ;
 
   /// @}
 
-private:
+ private:
-
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-  {
+    ar& boost::serialization::make_nvp(
-    ar & boost::serialization::make_nvp("Cal3Unified",
+        "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
-        boost::serialization::base_object<Cal3DS2_Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(xi_);
-    ar & BOOST_SERIALIZATION_NVP(xi_);
   }
-
 };
 
-template<>
+template <>
 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
-template<>
+template <>
 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
-
 }
-

gtsam/geometry/Cal3_S2.cpp

@@ -22,94 +22,53 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
 
 /* ************************************************************************* */
-Cal3_S2::Cal3_S2(double fov, int w, int h) :
+std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
-    s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
+  // Use the base class version since it is identical.
-  double a = fov * M_PI / 360.0; // fov/2 in radians
+  os << (Cal3&)cal;
-  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()
-     << ", 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_);
 }
 
 /* ************************************************************************* */
-Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
+Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
-                           OptionalJacobian<2,2> Dp) const {
+                          OptionalJacobian<2, 2> Dp) const {
   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_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),
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
-                  inv_fy_delta_v);
+
-    if(Dcal)
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
-       *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,  -inv_fx * point.y(),
+  if (Dcal)
-            -inv_fx,  inv_fx_s_inv_fy,
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
-            0, -inv_fy * point.y(), 0,  0, -inv_fy;
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
-    if(Dp)
+        0, 0, -inv_fy;
-        *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  if (Dp) *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 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,25 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3_S2 {
+class GTSAM_EXPORT Cal3_S2 : public Cal3 {
-private:
+ public:
-  double fx_, fy_, s_, u0_, v0_;
-
-public:
   enum { dimension = 5 };
-  typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
+
+  ///< shared pointer to calibration object
+  using shared_ptr = boost::shared_ptr<Cal3_S2>;
 
   /// @name Standard Constructors
   /// @{
 
   /// Create a default calibration that leaves coordinates unchanged
-  Cal3_S2() :
+  Cal3_S2() = default;
-      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 Vector5& 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,141 +57,65 @@ 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
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
-  /// @{
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
 
-  /// load calibration from location (default name is calibration_info.txt)
+  /**
-  Cal3_S2(const std::string &path);
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * 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);
+  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;
+  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;
 
-  /// @}
-  /// @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
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-      OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * 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 Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in intrinsic coordinates
-   */
-  Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-                   OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * convert homogeneous image coordinates to intrinsic coordinates
-   * @param p point in image coordinates
-   * @return point in intrinsic coordinates
-   */
-  Vector3 calibrate(const Vector3& p) 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> H1 = boost::none,
-      OptionalJacobian<5,5> H2=boost::none) const {
+                         OptionalJacobian<5, 5> H2 = boost::none) const {
-    if(H1) *H1 = -I_5x5;
+    if (H1) *H1 = -I_5x5;
-    if(H2) *H2 =  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
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  inline size_t dim() const { return dimension; }
+  inline static size_t Dim() { return dimension; }
-
-  /// return DOF, dimensionality of tangent space
-  static size_t Dim() { return dimension; }
 
   /// Given 5-dim tangent vector, create new calibration
   inline Cal3_S2 retract(const Vector& d) const {
@@ -212,27 +131,22 @@ public:
   /// @name Advanced Interface
   /// @{
 
-private:
+ private:
-
   /// Serialization function
   friend class boost::serialization::access;
-  template<class Archive>
+  template <class Archive>
-  void serialize(Archive & ar, const unsigned int /*version*/) {
+  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<>
+template <>
 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
-template<>
+template <>
 struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
 }  // \ namespace gtsam

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -20,18 +20,26 @@
 #include <iostream>
 
 namespace gtsam {
-using namespace std;
+
+/* ************************************************************************* */
+std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
+  os << (Cal3_S2&)cal;
+  os << ", b: " << cal.baseline();
+  return os;
+}
 
 /* ************************************************************************* */
 void Cal3_S2Stereo::print(const std::string& s) const {
-     K_.print(s+"K: ");
+  std::cout << s << (s != "" ? " " : "");
-     std::cout << s << "Baseline: " << b_ << std::endl;
+  std::cout << "K: " << (Matrix)K() << std::endl;
-   }
+  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::fabs(b_ - other.baseline()) < tol);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3_S2Stereo.h

@@ -22,48 +22,49 @@
 
 namespace gtsam {
 
-  /**
+/**
  * @brief The most common 5DOF 3D->2D calibration, stereo version
  * @addtogroup geometry
  * \nosubgrouping
  */
-  class GTSAM_EXPORT Cal3_S2Stereo {
+class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
  private:
-
+  double b_ = 1.0f;  ///< Stereo baseline.
-    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
+  using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
 
   /// @name Standard Constructors
   /// @
 
   /// default calibration leaves coordinates unchanged
-    Cal3_S2Stereo() :
+  Cal3_S2Stereo() = default;
-      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, double b)
-      K_(fx, fy, s, u0, v0), b_(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 Vector6& 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;
+  /// Output stream operator
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const Cal3_S2Stereo& cal);
+
+  /// print with optional string
+  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 +74,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 override { return Cal3_S2::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 +85,7 @@ namespace gtsam {
   /// vectorized form (column-wise)
   Vector6 vector() const {
     Vector6 v;
-      v << K_.vector(), b_;
+    v << Cal3_S2::vector(), b_;
     return v;
   }
 
@@ -111,14 +94,15 @@ namespace gtsam {
   /// @{
 
   /// return DOF, dimensionality of tangent space
-    inline size_t dim() const { return dimension; }
+  inline size_t dim() const override { return Dim(); }
 
   /// return DOF, dimensionality of tangent space
-    static size_t Dim() { return dimension; }
+  inline static size_t Dim() { return dimension; }
 
   /// 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
@@ -126,7 +110,6 @@ namespace gtsam {
     return T2.vector() - vector();
   }
 
-
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -134,23 +117,21 @@ namespace gtsam {
  private:
   /** Serialization function */
   friend class boost::serialization::access;
-    template<class Archive>
+  template <class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/)
+  void serialize(Archive& ar, const unsigned int /*version*/) {
-    {
+    ar& boost::serialization::make_nvp(
-      ar & BOOST_SERIALIZATION_NVP(K_);
+        "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
-      ar & BOOST_SERIALIZATION_NVP(b_);
+    ar& BOOST_SERIALIZATION_NVP(b_);
   }
   /// @}
+};
 
-  };
+// Define GTSAM traits
+template <>
+struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
 
-  // Define GTSAM traits
+template <>
-  template<>
+struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
-  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+};
-  };
-
-  template<>
-  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
-  };
 
 }  // \ namespace gtsam

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -11,11 +11,12 @@
 
 /**
  * @file  testCal3Bundler.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Bundler calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 
@@ -25,30 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
 
 static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
-static Point2 p(2,3);
+static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, vector)
+TEST(Cal3Bundler, vector) {
-{
   Cal3Bundler K;
   Vector expected(3);
   expected << 1, 0, 0;
-  CHECK(assert_equal(expected,K.vector()));
+  CHECK(assert_equal(expected, K.vector()));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, uncalibrate)
+TEST(Cal3Bundler, uncalibrate) {
-{
+  Vector v = K.vector();
-  Vector v = K.vector() ;
+  double r = p.x() * p.x() + p.y() * p.y();
-  double r = p.x()*p.x() + p.y()*p.y() ;
+  double g = v[0] * (1 + v[1] * r + v[2] * r * r);
-  double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
+  Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
-  Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
   Point2 actual = K.uncalibrate(p);
-  CHECK(assert_equal(expected,actual));
+  CHECK(assert_equal(expected, actual));
 }
 
-TEST( Cal3Bundler, calibrate )
+TEST(Cal3Bundler, calibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
@@ -56,26 +54,28 @@ TEST( Cal3Bundler, calibrate )
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
-Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
+Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
-
+  return k.calibrate(pt);
-/* ************************************************************************* */
-TEST( Cal3Bundler, Duncalibrate)
-{
-  Matrix Dcal, Dp;
-  Point2 actual = K.uncalibrate(p, Dcal, Dp);
-  Point2 expected(2182, 3773);
-  CHECK(assert_equal(expected,actual,1e-7));
-  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
-  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
-  CHECK(assert_equal(numerical1,Dcal,1e-7));
-  CHECK(assert_equal(numerical2,Dp,1e-7));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, Dcalibrate)
+TEST(Cal3Bundler, Duncalibrate) {
-{
+  Matrix Dcal, Dp;
+  Point2 actual = K.uncalibrate(p, Dcal, Dp);
+  Point2 expected(2182, 3773);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, Dcalibrate) {
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -83,27 +83,41 @@ TEST( Cal3Bundler, Dcalibrate)
   CHECK(assert_equal(pn, actual, 1e-7));
   Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
   Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
-  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
-  CHECK(assert_equal(numerical2,Dp,1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, assert_equal)
+TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
-{
-  CHECK(assert_equal(K,K,1e-7));
-}
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, retract)
+TEST(Cal3Bundler, retract) {
-{
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
-  Vector d(3);
+  EXPECT_LONGS_EQUAL(3, expected.dim());
+
+  EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
+  EXPECT_LONGS_EQUAL(expected.dim(), 3);
+
+  Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(Cal3_S2, Print) {
+  Cal3Bundler cal(1, 2, 3, 4, 5);
+  std::stringstream os;
+  os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
+     << ", px: " << cal.px() << ", py: " << cal.py();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3DFisheye.cpp

@@ -10,12 +10,13 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3DFisheye.cpp
+ * @file  testCal3Fisheye.cpp
  * @brief Unit tests for fisheye calibration class
  * @author ghaggin
  */
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Point3.h>
@@ -41,7 +42,11 @@ TEST(Cal3Fisheye, retract) {
   Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
                        K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
                        K.k4() + 9);
-  Vector d(9);
+
+  EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9);
+  EXPECT_LONGS_EQUAL(expected.dim(), 9);
+
+  Vector9 d;
   d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
   Cal3Fisheye actual = K.retract(d);
   CHECK(assert_equal(expected, actual, 1e-7));
@@ -186,16 +191,26 @@ Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3Fisheye, Dcalibrate)
+TEST(Cal3Fisheye, Dcalibrate) {
-{
   Point2 p(0.5, 0.5);
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;
   K.calibrate(pi, Dcal, Dp);
   Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
-  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
   Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
-  CHECK(assert_equal(numerical2,Dp,1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, Print) {
+  Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3DS2.cpp

@@ -11,12 +11,12 @@
 
 /**
  * @file  testCal3DS2.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Cal3DS2 calibration model.
  */
 
-
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3DS2.h>
 
@@ -25,53 +25,53 @@ using namespace gtsam;
 GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
 
-static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
+static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
-static Point2 p(2,3);
+                 4.0 * 1e-3);
+static Point2 p(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3DS2, uncalibrate)
+TEST(Cal3DS2, Uncalibrate) {
-{
+  Vector k = K.k();
-  Vector k = K.k() ;
+  double r = p.x() * p.x() + p.y() * p.y();
-  double r = p.x()*p.x() + p.y()*p.y() ;
+  double g = 1 + k[0] * r + k[1] * r * r;
-  double g = 1+k[0]*r+k[1]*r*r ;
+  double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x());
-  double tx = 2*k[2]*p.x()*p.y()       +   k[3]*(r+2*p.x()*p.x()) ;
+  double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y();
-  double ty =   k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
+  Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished();
-  Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished();
+  Vector v_i = K.K() * v_hat;
-  Vector v_i = K.K() * v_hat ;
+  Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2));
-  Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
   Point2 q = K.uncalibrate(p);
-  CHECK(assert_equal(q,p_i));
+  CHECK(assert_equal(q, p_i));
 }
 
-TEST( Cal3DS2, calibrate )
+TEST(Cal3DS2, Calibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Point2 pn_hat = K.calibrate(pi);
-  CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
+  CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
 }
 
-Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Duncalibrate1)
+TEST(Cal3DS2, 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-5));
   Matrix separate = K.D2d_calibration(p);
-  CHECK(assert_equal(numerical,separate,1e-5));
+  CHECK(assert_equal(numerical, separate, 1e-5));
 }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Duncalibrate2)
+TEST(Cal3DS2, Duncalibrate2) {
-{
+  Matrix computed;
-  Matrix computed; K.uncalibrate(p, boost::none, 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-5));
   Matrix separate = K.D2d_intrinsic(p);
-  CHECK(assert_equal(numerical,separate,1e-5));
+  CHECK(assert_equal(numerical, separate, 1e-5));
 }
 
 Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
@@ -79,8 +79,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, Dcalibrate)
+TEST(Cal3DS2, Dcalibrate) {
-{
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
   Matrix Dcal, Dp;
@@ -92,20 +91,37 @@ TEST( Cal3DS2, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
 
 /* ************************************************************************* */
-TEST( Cal3DS2, retract)
+TEST(Cal3DS2, Retract) {
-{
   Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
                    2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
-  Vector d(9);
+
-  d << 1,2,3,4,5,6,7,8,9;
+  EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
+  EXPECT_LONGS_EQUAL(expected.dim(), 9);
+
+  Vector9 d;
+  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
   Cal3DS2 actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(Cal3DS2, Print) {
+  Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3Unified.cpp

@@ -10,17 +10,18 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3Unify.cpp
+ * @file  testCal3Unified.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for Cal3Unified calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
 
@@ -35,51 +36,49 @@ 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 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 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( Cal3Unified, uncalibrate)
+TEST(Cal3Unified, Uncalibrate) {
-{
+  Point2 p_i(364.7791831734982, 305.6677211952602);
-  Point2 p_i(364.7791831734982, 305.6677211952602) ;
   Point2 q = K.uncalibrate(p);
-  CHECK(assert_equal(q,p_i));
+  CHECK(assert_equal(q, p_i));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, spaceNplane)
+TEST(Cal3Unified, SpaceNplane) {
-{
   Point2 q = K.spaceToNPlane(p);
   CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
   CHECK(assert_equal(p, K.nPlaneToSpace(q)));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, calibrate)
+TEST(Cal3Unified, Calibrate) {
-{
   Point2 pi = K.uncalibrate(p);
   Point2 pn_hat = K.calibrate(pi);
-  CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8));
+  CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
 }
 
-Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Duncalibrate1)
+TEST(Cal3Unified, Duncalibrate1) {
-{
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-6));
+  CHECK(assert_equal(numerical, computed, 1e-6));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Duncalibrate2)
+TEST(Cal3Unified, Duncalibrate2) {
-{
   Matrix computed;
   K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-6));
+  CHECK(assert_equal(numerical, computed, 1e-6));
 }
 
 Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
@@ -87,38 +86,37 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, Dcalibrate)
+TEST(Cal3Unified, Dcalibrate) {
-{
   Point2 pi = K.uncalibrate(p);
   Matrix Dcal, Dp;
   K.calibrate(pi, Dcal, Dp);
   Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
-  CHECK(assert_equal(numerical1,Dcal,1e-5));
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
   Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
-  CHECK(assert_equal(numerical2,Dp,1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, assert_equal)
+TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
-{
-  CHECK(assert_equal(K,K,1e-9));
-}
 
 /* ************************************************************************* */
-TEST( Cal3Unified, retract)
+TEST(Cal3Unified, Retract) {
-{
+  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7,
-  Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+                       2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
-      1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
+                       0.1 + 1);
-  Vector d(10);
+
+  EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
+  EXPECT_LONGS_EQUAL(expected.dim(), 10);
+
+  Vector10 d;
   d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
   Cal3Unified actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-9));
+  CHECK(assert_equal(expected, actual, 1e-9));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9));
 }
 
 /* ************************************************************************* */
-TEST( Cal3Unified, DerivedValue)
+TEST(Cal3Unified, DerivedValue) {
-{
   Values values;
   Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
   Key key = 1;
@@ -126,9 +124,24 @@ TEST( Cal3Unified, DerivedValue)
 
   Cal3Unified calafter = values.at<Cal3Unified>(key);
 
-  CHECK(assert_equal(cal,calafter,1e-9));
+  CHECK(assert_equal(cal, calafter, 1e-9));
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(Cal3Unified, Print) {
+  Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
+     << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2()
+     << ", xi: " << cal.xi();
+
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3_S2.cpp

@@ -11,7 +11,7 @@
 
 /**
  * @file  testCal3_S2.cpp
- * @brief Unit tests for transform derivatives
+ * @brief Unit tests for basic Cal3_S2 calibration model.
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -31,55 +31,58 @@ static Point2 p_uv(1320.3, 1740);
 static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
-TEST( Cal3_S2, easy_constructor)
+TEST(Cal3_S2, Constructor) {
-{
   Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
 
   double fov = 60;  // degrees
-  size_t w=640,h=480;
+  size_t w = 640, h = 480;
-  Cal3_S2 actual(fov,w,h);
+  Cal3_S2 actual(fov, w, h);
 
-  CHECK(assert_equal(expected,actual,1e-3));
+  CHECK(assert_equal(expected, actual, 1e-3));
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, calibrate)
+TEST(Cal3_S2, Calibrate) {
-{
+  Point2 intrinsic(2, 3);
-  Point2 intrinsic(2,3);
   Point2 expectedimage(1320.3, 1740);
   Point2 imagecoordinates = K.uncalibrate(intrinsic);
-  CHECK(assert_equal(expectedimage,imagecoordinates));
+  CHECK(assert_equal(expectedimage, imagecoordinates));
-  CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
+  CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, calibrate_homogeneous) {
+TEST(Cal3_S2, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
-  CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
+  CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
 }
 
 /* ************************************************************************* */
-Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
+Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
-TEST( Cal3_S2, Duncalibrate1)
+  return k.uncalibrate(pt);
-{
+}
-  Matrix25 computed; K.uncalibrate(p, computed, boost::none);
+
+TEST(Cal3_S2, Duncalibrate1) {
+  Matrix25 computed;
+  K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
-  CHECK(assert_equal(numerical,computed,1e-8));
+  CHECK(assert_equal(numerical, computed, 1e-8));
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, Duncalibrate2)
+TEST(Cal3_S2, Duncalibrate2) {
-{
+  Matrix computed;
-  Matrix computed; K.uncalibrate(p, boost::none, computed);
+  K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
-  CHECK(assert_equal(numerical,computed,1e-9));
+  CHECK(assert_equal(numerical, computed, 1e-9));
+}
+
+Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
+  return k.calibrate(pt);
 }
 
-Point2 calibrate_(const Cal3_S2& k,  const Point2& pt) {return k.calibrate(pt); }
 /* ************************************************************************* */
-TEST(Cal3_S2, Dcalibrate1)
+TEST(Cal3_S2, Dcalibrate1) {
-{
   Matrix computed;
   Point2 expected = K.calibrate(p_uv, computed, boost::none);
   Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
@@ -88,8 +91,7 @@ TEST(Cal3_S2, Dcalibrate1)
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, Dcalibrate2)
+TEST(Cal3_S2, Dcalibrate2) {
-{
   Matrix computed;
   Point2 expected = K.calibrate(p_uv, boost::none, computed);
   Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
@@ -98,23 +100,25 @@ TEST(Cal3_S2, Dcalibrate2)
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, assert_equal)
+TEST(Cal3_S2, Equal) {
-{
+  CHECK(assert_equal(K, K, 1e-9));
-  CHECK(assert_equal(K,K,1e-9));
 
   Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
-  CHECK(assert_equal(K,K1,1e-9));
+  CHECK(assert_equal(K, K1, 1e-9));
 }
 
 /* ************************************************************************* */
-TEST( Cal3_S2, retract)
+TEST(Cal3_S2, Retract) {
-{
+  Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
-  Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
+
-  Vector d(5);
+  EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
-  d << 1,2,3,4,5;
+  EXPECT_LONGS_EQUAL(expected.dim(), 5);
+
+  Vector5 d;
+  d << 1, 2, 3, 4, 5;
   Cal3_S2 actual = K.retract(d);
-  CHECK(assert_equal(expected,actual,1e-7));
+  CHECK(assert_equal(expected, actual, 1e-7));
-  CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
 }
 
 /* ************************************************************************* */
@@ -122,18 +126,17 @@ TEST(Cal3_S2, between) {
   Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
   Matrix H1, H2;
 
-  EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
+  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));
-
 }
 
 /* ************************************************************************* */
 TEST(Cal3_S2, Print) {
   Cal3_S2 cal(5, 5, 5, 5, 5);
   std::stringstream os;
-  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();
 
   EXPECT(assert_stdout_equal(os.str(), cal));
 }
@@ -144,4 +147,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/geometry/tests/testCal3_S2Stereo.cpp

@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  testCal3_S2Stereo.cpp
+ * @brief Unit tests for stereo-rig calibration model.
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo)
+GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo)
+
+static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1);
+static Point2 p(1, -2);
+static Point2 p_uv(1320.3, 1740);
+static Point2 p_xy(2, 3);
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Constructor) {
+  Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
+
+  double fov = 60;  // degrees
+  size_t w = 640, h = 480;
+  Cal3_S2Stereo actual(fov, w, h, 3);
+
+  CHECK(assert_equal(expected, actual, 1e-3));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Calibrate) {
+  Point2 intrinsic(2, 3);
+  Point2 expectedimage(1320.3, 1740);
+  Point2 imagecoordinates = K.uncalibrate(intrinsic);
+  CHECK(assert_equal(expectedimage, imagecoordinates));
+  CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
+  Vector3 intrinsic(2, 3, 1);
+  Vector3 image(1320.3, 1740, 1);
+  CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Equal) {
+  CHECK(assert_equal(K, K, 1e-9));
+
+  Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
+  CHECK(assert_equal(K, K1, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Retract) {
+  Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
+                         7);
+  EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);
+  EXPECT_LONGS_EQUAL(expected.dim(), 6);
+
+  Vector6 d;
+  d << 1, 2, 3, 4, 5, 6;
+  Cal3_S2Stereo actual = K.retract(d);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Print) {
+  Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2);
+  std::stringstream os;
+  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
+     << ", px: " << cal.px() << ", py: " << cal.py()
+     << ", b: " << cal.baseline();
+  EXPECT(assert_stdout_equal(os.str(), cal));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/gtsam.i

@@ -852,8 +852,7 @@ class Cal3_S2 {
   gtsam::Point2 principalPoint() const;
   Vector vector() const;
   Matrix K() const;
-  Matrix matrix() const;
+  Matrix inverse() const;
-  Matrix matrix_inverse() const;
 
   // enabling serialization functionality
   void serialize() const;