Cal3 Base Class

gtsam/geometry/Cal3.cpp

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+  // old formula: fx_ = (double) w * tan(a);
+  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);
+}
+
+/* ************************************************************************* */
+
+}  // \ namespace gtsam

gtsam/geometry/Cal3.h

@@ -30,7 +30,7 @@ namespace gtsam {
  * Jacobians of `calibrate` using `uncalibrate`.
  * This is useful when there are iterative operations in the `calibrate`
  * function which make computing jacobians difficult.
- * 
+ *
  * Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
  * easily compute the Jacobians:
  * df/pi = -I (pn and pi are independent args)
@@ -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
+  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 {
+    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;
+  }
+
+  /// @}
+  /// @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

@@ -23,16 +23,6 @@
 
 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 {
   Matrix3 K;
@@ -52,18 +42,23 @@ Vector3 Cal3Bundler::vector() const {
   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");
 }
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
-      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
-      || std::abs(v0_ - K.v0_) > tol)
-    return false;
-  return true;
+  return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
+          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
+          std::fabs(v0_ - K.v0_) < tol);
 }
 
 /* ************************************************************************* */
@@ -150,14 +145,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-/* ************************************************************************* */
-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();
-}
-
-}
+}  // \ namespace gtsam