Stereo calibration can now be optimized

gtsam/geometry/Cal3_S2Stereo.h

@@ -103,6 +103,38 @@ namespace gtsam {
     /// return baseline
     inline double baseline() const { return b_; }
 
+    /// vectorized form (column-wise)
+    Vector6 vector() const {
+      Vector6 v;
+      v << K_.vector(), b_;
+      return v;
+    }
+
+    /// @}
+    /// @name Manifold
+    /// @{
+
+    /// return DOF, dimensionality of tangent space
+    inline size_t dim() const {
+      return 6;
+    }
+
+    /// return DOF, dimensionality of tangent space
+    static size_t Dim() {
+      return 6;
+    }
+
+    /// 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));
+    }
+
+    /// Unretraction for the calibration
+    Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
+      return T2.vector() - vector();
+    }
+
+
     /// @}
     /// @name Advanced Interface
     /// @{
@@ -119,4 +151,23 @@ namespace gtsam {
     /// @}
 
   };
+
+  // Define GTSAM traits
+  namespace traits {
+
+  template<>
+  struct GTSAM_EXPORT is_manifold<Cal3_S2Stereo> : public boost::true_type{
+  };
+
+  template<>
+  struct GTSAM_EXPORT dimension<Cal3_S2Stereo> : public boost::integral_constant<int, 6>{
+  };
+
+  template<>
+  struct GTSAM_EXPORT zero<Cal3_S2Stereo> {
+    static Cal3_S2Stereo value() { return Cal3_S2Stereo();}
+  };
+
+  }
+
 } // \ namespace gtsam