add helper functions

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());

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

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 */