Refactor Bundler and Fisheye models

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;
@@ -59,11 +49,9 @@ void Cal3Bundler::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
+  return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
-      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
+          std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
-      || std::abs(v0_ - K.v0_) > tol)
+          std::fabs(v0_ - K.v0_) < tol);
-    return false;
-  return true;
 }
 
 /* ************************************************************************* */
@@ -150,14 +138,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

@@ -28,14 +28,16 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler {
+class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
  private:
   double f_; ///< focal length
   double k1_, k2_; ///< radial distortion
-  double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
   double tol_; ///< tolerance value when calibrating
 
+  // NOTE: image center parameters (u0, v0) are not optimized
+  // but are constants.
+
  public:
 
   enum { dimension = 3 };
@@ -44,7 +46,7 @@ public:
   /// @{
 
   /// Default constructor
-  Cal3Bundler();
+  Cal3Bundler() : Cal3(), f_(1), k1_(0), k2_(0), tol_(1e-5) {}
 
   /**
    *  Constructor
@@ -56,7 +58,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), f_(f), k1_(k1), k2_(k2), tol_(tol) {}
 
   virtual ~Cal3Bundler() {}
 
@@ -65,7 +68,7 @@ public:
   /// @{
 
   /// 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,11 +77,6 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix3 K() const; ///< 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_;
@@ -109,6 +107,11 @@ public:
     return v0_;
   }
 
+  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
+
+  Vector3 vector() const;
+
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /// get parameter u0
   inline double u0() const {
@@ -121,12 +124,11 @@ public:
   }
 #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
    */
@@ -158,20 +160,20 @@ public:
   /// @name Manifold
   /// @{
 
+  /// return DOF, dimensionality of tangent space
+  virtual size_t dim() const { return dimension; }
+
+  /// return DOF, dimensionality of tangent space
+  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(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
-  /// Calculate local coordinates to another calibration
-  Vector3 localCoordinates(const Cal3Bundler& T2) const;
-
-  /// dimensionality
-  virtual size_t dim() const {
-    return 3;
   }
 
-  /// dimensionality
+  /// Calculate local coordinates to another calibration
-  static size_t Dim() {
+  Vector3 localCoordinates(const Cal3Bundler& T2) const {
-    return 3;
+    return T2.vector() - vector();
   }
 
  private:
@@ -184,11 +186,11 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
     ar& BOOST_SERIALIZATION_NVP(f_);
     ar& BOOST_SERIALIZATION_NVP(k1_);
     ar& BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(u0_);
-    ar & BOOST_SERIALIZATION_NVP(v0_);
     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;
@@ -165,24 +147,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

@@ -46,11 +46,10 @@ namespace gtsam {
  *   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_, k2_, k3_, k4_;  ///< fisheye distortion coefficients
-  double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
+  double tol_ = 1e-5;         ///< tolerance value when calibrating
-  double tol_ = 1e-5;             // tolerance value when calibrating
 
  public:
   enum { dimension = 9 };
@@ -61,17 +60,12 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3Fisheye()
+  Cal3Fisheye() : Cal3(), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
-      : 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 +78,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 +101,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 +114,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;
 
@@ -151,7 +137,7 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @{
 
   /// 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 +146,21 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Manifold
   /// @{
 
+  /// Return dimensions of calibration manifold object
+  virtual size_t dim() const { return dimension; }
+
+  /// Return dimensions of calibration manifold object
+  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 +181,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_);