Override dim(), cleanup, and add unicode

gtsam/geometry/Cal3.cpp

@@ -27,7 +27,6 @@ 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_;
 }
@@ -63,6 +62,14 @@ bool Cal3::equals(const Cal3& K, double tol) const {
           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

@@ -161,7 +161,7 @@ class GTSAM_EXPORT Cal3 {
   }
 
   /// return calibration matrix K
-  Matrix3 K() const {
+  virtual Matrix3 K() const {
     Matrix3 K;
     K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
     return K;
@@ -173,13 +173,13 @@ class GTSAM_EXPORT Cal3 {
 #endif
 
   /// Return inverted calibration matrix inv(K)
-  Matrix3 inverse() const {
+  Matrix3 inverse() const;
-    const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
+
-    Matrix3 K_inverse;
+  /// return DOF, dimensionality of tangent space
-    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
+  inline virtual size_t dim() const { return Dim(); }
-        -v0_ / fy_, 0.0, 0.0, 1.0;
+
-    return K_inverse;
+  /// return DOF, dimensionality of tangent space
-  }
+  inline static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Advanced Interface

gtsam/geometry/Cal3Bundler.cpp

@@ -25,8 +25,9 @@ namespace gtsam {
 
 /* ************************************************************************* */
 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;
 }
 
@@ -39,7 +40,7 @@ Vector4 Cal3Bundler::k() const {
 
 /* ************************************************************************* */
 Vector3 Cal3Bundler::vector() const {
-  return Vector3(f_, k1_, k2_);
+  return Vector3(fx_, k1_, k2_);
 }
 
 /* ************************************************************************* */
@@ -51,12 +52,13 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
 
 /* ************************************************************************* */
 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 {
-  return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
+  const Cal3* base = dynamic_cast<const Cal3*>(&K);
+  return (Cal3::equals(*base, 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);
 }
@@ -64,14 +66,16 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
 /* ************************************************************************* */
 Point2 Cal3Bundler::uncalibrate(const Point2& p, //
     OptionalJacobian<2, 3> Dcal, 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;
@@ -92,9 +96,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
 Point2 Cal3Bundler::calibrate(const Point2& pi,
                               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

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
@@ -31,11 +32,11 @@ namespace gtsam {
 class GTSAM_EXPORT Cal3Bundler : public Cal3 {
 
  private:
-  double f_ = 1.0f;               ///< focal length
   double k1_ = 0.0f, k2_ = 0.0f;  ///< radial distortion
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
-  // NOTE: image center parameters (u0, v0) are not optimized
+  // NOTE: We use the base class fx to represent the common focal length.
+  // Also, image center parameters (u0, v0) are not optimized
   // but are treated as constants.
 
  public:
@@ -59,7 +60,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
    */
   Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
               double tol = 1e-5)
-      : Cal3(f, f, 0, u0, v0), f_(f), k1_(k1), k2_(k2), tol_(tol) {}
+      : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
 
   virtual ~Cal3Bundler() {}
 
@@ -81,16 +82,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @name Standard Interface
   /// @{
 
-  /// 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_;
@@ -111,7 +102,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
     return v0_;
   }
 
-  Matrix3 K() const; ///< Standard 3*3 calibration matrix
+  Matrix3 K() const override; ///< Standard 3*3 calibration matrix
   Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
@@ -165,14 +156,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  virtual size_t dim() const { return dimension; }
+  virtual 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; }
 
   /// Update calibration with tangent space delta
   inline Cal3Bundler retract(const Vector& d) const {
-    return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
+    return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
   }
 
   /// Calculate local coordinates to another calibration
@@ -192,7 +183,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
   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(tol_);

gtsam/geometry/Cal3DS2.h

@@ -81,10 +81,10 @@ public:
   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

gtsam/geometry/Cal3DS2_Base.cpp

@@ -93,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;

gtsam/geometry/Cal3DS2_Base.h

@@ -33,11 +33,10 @@ 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 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
- *                      p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y  ]
+ * pi = K*P̂
- * pi = K*Pn
  */
 class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
  protected:
@@ -132,6 +131,12 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// Derivative of uncalibrate wrpt the calibration parameters
   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
   /// @{

gtsam/geometry/Cal3Fisheye.h

@@ -38,9 +38,9 @@ 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]
@@ -152,10 +152,10 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   /// @{
 
   /// 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; }
 
   /// Given delta vector, update calibration
   inline Cal3Fisheye retract(const Vector& d) const {

gtsam/geometry/Cal3Unified.h

@@ -34,10 +34,10 @@ namespace gtsam {
  *
  * 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² + P.y² + 1})]
- * rr = Pn.x^2 + Pn.y^2
+ * r² = Pn.x² + Pn.y²
- * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ * \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) + 2*k4 pn.x pn.y  ]
+ *                      k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
@@ -127,10 +127,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   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; }
 
   /// @}
 

gtsam/geometry/Cal3_S2.cpp

@@ -55,8 +55,9 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
   const double u = p.x(), v = p.y();
   double delta_u = u - u0_, delta_v = v - v0_;
   double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
-  double inv_fy_delta_v = inv_fy * delta_v,
+  double inv_fy_delta_v = inv_fy * delta_v;
-         inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+
   Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
   if (Dcal)
     *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,

gtsam/geometry/Cal3_S2.h

@@ -115,10 +115,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
   /// @{
 
   /// 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 {

gtsam/geometry/Cal3_S2Stereo.h

@@ -79,7 +79,7 @@ namespace gtsam {
     const Cal3_S2& calibration() const { return *this; }
 
     /// return calibration matrix K, same for left and right
-    Matrix3 K() const { return K(); }
+    Matrix3 K() const override { return Cal3_S2::K(); }
 
     /// return baseline
     inline double baseline() const { return b_; }
@@ -96,10 +96,10 @@ 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 {

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -29,8 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
 static Point2 p(2,3);
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, vector)
+TEST(Cal3Bundler, vector) {
-{
   Cal3Bundler K;
   Vector expected(3);
   expected << 1, 0, 0;
@@ -38,8 +37,7 @@ TEST(Cal3Bundler, vector)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, uncalibrate)
+TEST(Cal3Bundler, uncalibrate) {
-{
   Vector v = K.vector() ;
   double r = p.x()*p.x() + p.y()*p.y() ;
   double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
@@ -48,8 +46,7 @@ TEST(Cal3Bundler, uncalibrate)
   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);
@@ -62,8 +59,7 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
 Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, Duncalibrate)
+TEST(Cal3Bundler, Duncalibrate) {
-{
   Matrix Dcal, Dp;
   Point2 actual = K.uncalibrate(p, Dcal, Dp);
   Point2 expected(2182, 3773);
@@ -75,8 +71,7 @@ TEST(Cal3Bundler, Duncalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, Dcalibrate)
+TEST(Cal3Bundler, Dcalibrate) {
-{
   Matrix Dcal, Dp;
   Point2 pn(0.5, 0.5);
   Point2 pi = K.uncalibrate(pn);
@@ -89,15 +84,15 @@ TEST(Cal3Bundler, Dcalibrate)
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, assert_equal)
+TEST(Cal3Bundler, assert_equal) {
-{
   CHECK(assert_equal(K,K,1e-7));
 }
 
 /* ************************************************************************* */
-TEST(Cal3Bundler, retract)
+TEST(Cal3Bundler, retract) {
-{
   Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
+  EXPECT_LONGS_EQUAL(3, expected.dim());
+
   Vector3 d;
   d << 10, 1e-3, 1e-3;
   Cal3Bundler actual = K.retract(d);