diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 1f18048c6..a83552e93 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/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; diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index 32aab0534..c00d58312 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -32,9 +32,8 @@ #include -#include -#include -#include +#include +#include namespace gtsam {