add logmap

gtsam/geometry/Cal3_S2.h

@@ -114,6 +114,7 @@ namespace gtsam {
 	     * return DOF, dimensionality of tangent space
 	     */
 	    inline size_t dim() const { return 5; }
+	  	static size_t Dim() { return 5; }
 
 	    /**
 	     * Given 5-dim tangent vector, create new calibration
@@ -123,6 +124,13 @@ namespace gtsam {
 	            s_ + d(2), u0_ + d(3), v0_ + d(4));
 	    }
 
+	    /**
+	     * logmap for the calibration
+	     */
+	    Vector logmap(const Cal3_S2& T2) const {
+	    	return vector() - T2.vector();
+	    }
+
 	private:
 		/** Serialization function */
 		friend class boost::serialization::access;

gtsam/geometry/GeneralCameraT.h

@@ -32,9 +32,8 @@
 #include <gtsam/geometry/Cal3_S2.h>
 
 
-#include <gtsam/sfm/Cal3Dummy.h>
-#include <gtsam/sfm/Cal3Bundler.h>
-#include <gtsam/sfm/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
 
 namespace gtsam {