diff --git a/geometry/CalibratedCamera.cpp b/geometry/CalibratedCamera.cpp index 7a2684504d..468e6df0e8 100644 --- a/geometry/CalibratedCamera.cpp +++ b/geometry/CalibratedCamera.cpp @@ -65,5 +65,22 @@ namespace gtsam { } return project_to_camera(q); } + + + CalibratedCamera CalibratedCamera::expmap(const Vector& d) const { + return CalibratedCamera(pose().expmap(d)) ; + } + Vector CalibratedCamera::logmap(const CalibratedCamera& T2) const { + return pose().logmap(T2.pose()) ; + } + + CalibratedCamera CalibratedCamera::Expmap(const Vector& v) { + return CalibratedCamera(Pose3::Expmap(v)) ; + } + + Vector CalibratedCamera::Logmap(const CalibratedCamera& p) { + return Pose3::Logmap(p.pose()) ; + } + /* ************************************************************************* */ } diff --git a/geometry/CalibratedCamera.h b/geometry/CalibratedCamera.h index 22fd43a729..36cbb5d9d0 100644 --- a/geometry/CalibratedCamera.h +++ b/geometry/CalibratedCamera.h @@ -43,7 +43,14 @@ namespace gtsam { return CalibratedCamera( pose_.inverse() ) ; } - inline static size_t dim() { return 6 ; } + CalibratedCamera expmap(const Vector& d) const; + Vector logmap(const CalibratedCamera& T2) const; + + static CalibratedCamera Expmap(const Vector& v); + static Vector Logmap(const CalibratedCamera& p); + + inline size_t dim() const { return 6 ; } + inline static size_t Dim() { return 6 ; } /** * Create a level camera at the given 2D pose and height