From 43df7375c09aebd99e97c337c3cf641da008b025 Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Fri, 5 Mar 2010 00:55:36 +0000
Subject: [PATCH] add helper functions

---
 cpp/CalibratedCamera.cpp |  5 +++--
 cpp/CalibratedCamera.h   | 16 ++++++++++++++--
 cpp/Pose3.h              |  2 +-
 3 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/cpp/CalibratedCamera.cpp b/cpp/CalibratedCamera.cpp
index ab46ceba3..edff4321e 100644
--- a/cpp/CalibratedCamera.cpp
+++ b/cpp/CalibratedCamera.cpp
@@ -31,8 +31,9 @@ namespace gtsam {
 		pose_(pose) {
 	}
 
-	CalibratedCamera::~CalibratedCamera() {
-	}
+	CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(expmap<Pose3>(v)) {}
+
+	CalibratedCamera::~CalibratedCamera() {}
 
 	CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
 		double st = sin(pose2.theta()), ct = cos(pose2.theta());
diff --git a/cpp/CalibratedCamera.h b/cpp/CalibratedCamera.h
index 57f0052b0..13dd39ccb 100644
--- a/cpp/CalibratedCamera.h
+++ b/cpp/CalibratedCamera.h
@@ -37,12 +37,24 @@ namespace gtsam {
 
 	public:
 		CalibratedCamera(const Pose3& pose);
+		CalibratedCamera(const Vector &v) ;
 		virtual ~CalibratedCamera();
 
-		const Pose3& pose() const {
-			return pose_;
+		inline const Pose3& pose() const {	return pose_; }
+		bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
+			return pose_.equals(camera.pose(), tol) ;
 		}
 
+		inline const CalibratedCamera compose(const CalibratedCamera &c) const {
+			return CalibratedCamera( pose_ * c.pose() ) ;
+		}
+
+		inline const CalibratedCamera inverse() const {
+			return CalibratedCamera( pose_.inverse() ) ;
+		}
+
+		inline static size_t dim() { return 6 ; }
+
 		/**
 		 * Create a level camera at the given 2D pose and height
 		 * @param pose2 specifies the location and viewing direction
diff --git a/cpp/Pose3.h b/cpp/Pose3.h
index ae9bf518c..83e14aff0 100644
--- a/cpp/Pose3.h
+++ b/cpp/Pose3.h
@@ -73,7 +73,7 @@ namespace gtsam {
     Pose3 transform_to(const Pose3& pose) const;
 
     /** get the dimension by the type */
-    static inline size_t dim() { return 6; };
+    inline static size_t dim() { return 6; }
 
   private:
     /** Serialization function */