Added more functions for geometric objects to gtsam.h

gtsam.h

@@ -4,6 +4,10 @@ class Point2 {
 	void print(string s) const;
 	double x();
 	double y();
+	Point2* compose_(const Point2& p2);
+	Point2* between_(const Point2& p2);
+	Vector localCoordinates(const Point2& p);
+	Point2* retract_(Vector v);
 };
 
 class Point3 {
@@ -16,6 +20,10 @@ class Point3 {
 	double x();
 	double y();
 	double z();
+	Point3* compose_(const Point3& p2);
+	Point3* between_(const Point3& p2);
+	Vector localCoordinates(const Point3& p);
+	Point3* retract_(Vector v);
 };
 
 class Rot2 {
@@ -25,6 +33,10 @@ class Rot2 {
 	bool equals(const Rot2& rot, double tol) const;
 	double c() const;
 	double s() const;
+	Rot2* compose_(const Rot2& p2);
+	Rot2* between_(const Rot2& p2);
+	Vector localCoordinates(const Rot2& p);
+	Rot2* retract_(Vector v);
 };
 
 class Rot3 {
@@ -32,6 +44,10 @@ class Rot3 {
 	Rot3(Matrix R);
 	void print(string s) const;
 	bool equals(const Rot3& rot, double tol) const;
+	Rot3* compose_(const Rot3& p2);
+	Rot3* between_(const Rot3& p2);
+	Vector localCoordinates(const Rot3& p);
+	Rot3* retract_(Vector v);
 };
 
 class Pose2 {
@@ -65,6 +81,7 @@ class Pose3 {
 	Pose3* compose_(const Pose3& p2);
 	Pose3* between_(const Pose3& p2);
 	Vector localCoordinates(const Pose3& p);
+	Pose3* retract_(Vector v);
 };
 
 class SharedGaussian {

gtsam/geometry/Point2.h

@@ -72,6 +72,11 @@ public:
 		return *this + p2;
 	}
 
+	/// MATLAB version returns shared pointer
+	boost::shared_ptr<Point2> compose_(const Point2& p2) {
+		return boost::shared_ptr<Point2>(new Point2(compose(p2)));
+	}
+
 	/** operators */
 	inline Point2 operator- () const {return Point2(-x_,-y_);}
 	inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
@@ -92,6 +97,11 @@ public:
 	/// Updates a with tangent space delta
 	inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
 
+  /// MATLAB version returns shared pointer
+  boost::shared_ptr<Point2> retract_(const Vector& v) {
+    return boost::shared_ptr<Point2>(new Point2(retract(v)));
+  }
+
 	/// Local coordinates of manifold neighborhood around current value
 	inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
 
@@ -136,6 +146,11 @@ public:
 		return p2 - (*this);
 	}
 
+	/// MATLAB version returns shared pointer
+	boost::shared_ptr<Point2> between_(const Point2& p2) {
+		return boost::shared_ptr<Point2>(new Point2(between(p2)));
+	}
+
 	/** get functions for x, y */
 	double x() const {return x_;}
 	double y() const {return y_;}

gtsam/geometry/Point3.h

@@ -76,6 +76,11 @@ namespace gtsam {
   	  return *this + p2;
     }
 
+  	/// MATLAB version returns shared pointer
+  	boost::shared_ptr<Point3> compose_(const Point3& p2) {
+  		return boost::shared_ptr<Point3>(new Point3(compose(p2)));
+  	}
+
     /// @}
     /// @name Manifold
     /// @{
@@ -89,6 +94,11 @@ namespace gtsam {
   	/// Updates a with tangent space delta
   	inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
 
+    /// MATLAB version returns shared pointer
+    boost::shared_ptr<Point3> retract_(const Vector& v) {
+      return boost::shared_ptr<Point3>(new Point3(retract(v)));
+    }
+
   	/// Returns inverse retraction
   	inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
 
@@ -138,6 +148,11 @@ namespace gtsam {
       return p2 - *this;
     }
 
+  	/// MATLAB version returns shared pointer
+  	boost::shared_ptr<Point3> between_(const Point3& p2) {
+  		return boost::shared_ptr<Point3>(new Point3(between(p2)));
+  	}
+
     /** return vectorized form (column-wise)*/
     Vector vector() const {
       Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_;

gtsam/geometry/Pose3.h

@@ -110,10 +110,15 @@ namespace gtsam {
     size_t dim() const { return dimension; }
 
 
-    /** Exponential map around another pose */    /// Retraction from R^6 to Pose3 manifold neighborhood around current pose
+    /// Retraction from R^6 to Pose3 manifold neighborhood around current pose
     Pose3 retract(const Vector& d) const;
 
-    /// Logarithm map around another pose T1			/// Local 6D coordinates of Pose3 manifold neighborhood around current pose
+    /// MATLAB version returns shared pointer
+    boost::shared_ptr<Pose3> retract_(const Vector& v) {
+      return boost::shared_ptr<Pose3>(new Pose3(retract(v)));
+    }
+
+    /// Local 6D coordinates of Pose3 manifold neighborhood around current pose
     Vector localCoordinates(const Pose3& T2) const;
 
     /// @}

gtsam/geometry/Rot2.h

@@ -138,6 +138,11 @@ namespace gtsam {
 			return *this * R1;
 		}
 
+		/// MATLAB version returns shared pointer
+		boost::shared_ptr<Rot2> compose_(const Rot2& p2) {
+			return boost::shared_ptr<Rot2>(new Rot2(compose(p2)));
+		}
+
 		/** Compose - make a new rotation by adding angles */
 		Rot2 operator*(const Rot2& R) const {
 			return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
@@ -165,6 +170,11 @@ namespace gtsam {
   	/// Updates a with tangent space delta
   	inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
 
+    /// MATLAB version returns shared pointer
+    boost::shared_ptr<Rot2> retract_(const Vector& v) {
+      return boost::shared_ptr<Rot2>(new Rot2(retract(v)));
+    }
+
   	/// Returns inverse retraction
   	inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
 
@@ -206,6 +216,11 @@ namespace gtsam {
 			return between_default(*this, p2);
 		}
 
+		/// MATLAB version returns shared pointer
+		boost::shared_ptr<Rot2> between_(const Rot2& p2) {
+			return boost::shared_ptr<Rot2>(new Rot2(between(p2)));
+		}
+
 		/** return 2*2 rotation matrix */
 		Matrix matrix() const;
 

gtsam/geometry/Rot3M.h

@@ -152,6 +152,11 @@ namespace gtsam {
     Rot3M compose(const Rot3M& R2,
   	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
+  	/// MATLAB version returns shared pointer
+  	boost::shared_ptr<Rot3M> compose_(const Rot3M& p2) {
+  		return boost::shared_ptr<Rot3M>(new Rot3M(compose(p2)));
+  	}
+
     /// rotate point from rotated coordinate frame to world = R*p
     inline Point3 operator*(const Point3& p) const { return rotate(p);}
 
@@ -177,6 +182,11 @@ namespace gtsam {
   	/// Updates a with tangent space delta
     Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
 
+    /// MATLAB version returns shared pointer
+    boost::shared_ptr<Rot3M> retract_(const Vector& v) {
+      return boost::shared_ptr<Rot3M>(new Rot3M(retract(v)));
+    }
+
     /// Returns inverse retraction
     Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
 
@@ -247,6 +257,11 @@ namespace gtsam {
     		boost::optional<Matrix&> H1=boost::none,
     		boost::optional<Matrix&> H2=boost::none) const;
 
+  	/// MATLAB version returns shared pointer
+  	boost::shared_ptr<Rot3M> between_(const Rot3M& p2) {
+  		return boost::shared_ptr<Rot3M>(new Rot3M(between(p2)));
+  	}
+
     /** compose two rotations */
     Rot3M operator*(const Rot3M& R2) const {
       return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));