From 3a11ec9b6cd5b6f53538bb775066886205dd75da Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Sun, 19 Sep 2010 18:40:04 +0000
Subject: [PATCH] add interface for vSLAM

---
 geometry/CalibratedCamera.cpp | 17 +++++++++++++++++
 geometry/CalibratedCamera.h   |  9 ++++++++-
 2 files changed, 25 insertions(+), 1 deletion(-)

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