Removed wrap function duplicates due to shared_ptr, removed the wrapping functions inside geometry classes, added named constructors to the wrappable functions

gtsam.h

@@ -26,8 +26,10 @@
 
 /**
  * Status:
- *  - Depreciated versions of functions that return a shared_ptr unnecessarily are still present
  *  - TODO: global functions
+ *  - TODO: default values for arguments
+ *  - TODO: overloaded functions
+ *  - TODO: Handle Rot3M conversions to quaternions
  *  - TODO: namespace detection to handle nested namespaces
  */
 
@@ -43,12 +45,6 @@ class Point2 {
 	Point2 compose(const Point2& p2);
 	Point2 between(const Point2& p2);
 	Point2 retract(Vector v);
-
-	// Depreciated interface
-	Point2* compose_(const Point2& p2);
-	Point2* between_(const Point2& p2);
-	Point2* retract_(Vector v);
-	static Point2* Expmap_(Vector v);
 };
 
 class Point3 {
@@ -67,12 +63,6 @@ class Point3 {
 	Point3 retract(Vector v);
 	Point3 compose(const Point3& p2);
 	Point3 between(const Point3& p2);
-
-	// Depreciated interface
-	static Point3* Expmap_(Vector v);
-	Point3* compose_(const Point3& p2);
-	Point3* between_(const Point3& p2);
-	Point3* retract_(Vector v);
 };
 
 class Rot2 {
@@ -80,20 +70,21 @@ class Rot2 {
 	Rot2(double theta);
 	static Rot2 Expmap(Vector v);
 	static Vector Logmap(const Rot2& p);
+	static Rot2 fromAngle(double theta);
+	static Rot2 fromDegrees(double theta);
+	static Rot2 fromCosSin(double c, double s);
+	static Rot2 relativeBearing(const Point2& d); // Ignoring derivative
+	static Rot2 atan2(double y, double x);
 	void print(string s) const;
 	bool equals(const Rot2& rot, double tol) const;
+	double theta() const;
+	double degrees() const;
 	double c() const;
 	double s() const;
 	Vector localCoordinates(const Rot2& p);
 	Rot2 retract(Vector v);
 	Rot2 compose(const Rot2& p2);
 	Rot2 between(const Rot2& p2);
-
-	// Depreciated interface
-	Rot2* compose_(const Rot2& p2);
-	Rot2* between_(const Rot2& p2);
-	Rot2* retract_(Vector v);
-	static Rot2* Expmap_(Vector v);
 };
 
 class Rot3 {
@@ -102,22 +93,27 @@ class Rot3 {
 	static Rot3 Expmap(Vector v);
 	static Vector Logmap(const Rot3& p);
   static Rot3 ypr(double y, double p, double r);
+  static Rot3 Rx(double t);
+  static Rot3 Ry(double t);
+  static Rot3 Rz(double t);
+  static Rot3 RzRyRx(double x, double y, double z);
+  static Rot3 RzRyRx(const Vector& xyz);
+  static Rot3 yaw  (double t); // positive yaw is to right (as in aircraft heading)
+  static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
+  static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft)
+  static Rot3 quaternion(double w, double x, double y, double z);
+  static Rot3 rodriguez(const Vector& v);
 	Matrix matrix() const;
 	Matrix transpose() const;
 	Vector xyz() const;
 	Vector ypr() const;
+//  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
 	void print(string s) const;
 	bool equals(const Rot3& rot, double tol) const;
 	Vector localCoordinates(const Rot3& p);
 	Rot3 retract(Vector v);
 	Rot3 compose(const Rot3& p2);
 	Rot3 between(const Rot3& p2);
-
-	// Depreciated interface
-	Rot3* compose_(const Rot3& p2);
-	Rot3* between_(const Rot3& p2);
-	Rot3* retract_(Vector v);
-	static Rot3* Expmap_(Vector v);
 };
 
 class Pose2 {
@@ -138,12 +134,6 @@ class Pose2 {
 	Pose2 retract(Vector v);
 	Pose2 compose(const Pose2& p2);
 	Pose2 between(const Pose2& p2);
-
-	// Depreciated interface
-	Pose2* compose_(const Pose2& p2);
-	Pose2* between_(const Pose2& p2);
-	Pose2* retract_(Vector v);
-	static Pose2* Expmap_(Vector v);
 };
 
 class Pose3 {
@@ -165,14 +155,6 @@ class Pose3 {
 	Pose3 retract(Vector v);
 	Point3 translation() const;
 	Rot3 rotation() const;
-
-	// Depreciated interface
-	static Pose3* Expmap_(Vector v);
-	Pose3* compose_(const Pose3& p2);
-	Pose3* between_(const Pose3& p2);
-	Pose3* retract_(Vector v);
-	Point3* translation_() const;
-	Rot3* rotation_() const;
 };
 
 class SharedGaussian {
@@ -314,7 +296,7 @@ class PlanarSLAMOdometry {
 class GaussianSequentialSolver {
   GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
   GaussianBayesNet* eliminate() const;
-  VectorValues* optimize() const; // FAIL: parse error here
+  VectorValues* optimize() const;
   GaussianFactor* marginalFactor(int j) const;
   Matrix marginalCovariance(int j) const;
 };

gtsam/geometry/Point2.h

@@ -72,11 +72,6 @@ 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_);}
@@ -97,11 +92,6 @@ 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)); }
 
@@ -111,9 +101,6 @@ public:
 
 	/// Exponential map around identity - just create a Point2 from a vector
 	static inline Point2 Expmap(const Vector& v) { return Point2(v); }
-	static inline boost::shared_ptr<Point2> Expmap_(const Vector& v) {
-		return boost::shared_ptr<Point2>(new Point2(Expmap(v)));
-	}
 
 	/// Log map around identity - just return the Point2 as a vector
 	static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
@@ -149,11 +136,6 @@ 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,11 +76,6 @@ 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
     /// @{
@@ -94,11 +89,6 @@ 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); }
 
@@ -108,9 +98,6 @@ namespace gtsam {
 
     /** Exponential map at identity - just create a Point3 from x,y,z */
     static inline Point3 Expmap(const Vector& v) { return Point3(v); }
-  	static inline boost::shared_ptr<Point3> Expmap_(const Vector& v) {
-  		return boost::shared_ptr<Point3>(new Point3(Expmap(v)));
-  	}
 
     /** Log map at identity - return the x,y,z of this point */
     static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
@@ -151,11 +138,6 @@ 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/Pose2.h

@@ -103,11 +103,6 @@ public:
 			boost::optional<Matrix&> H1 = boost::none,
 			boost::optional<Matrix&> H2 = boost::none) const;
 
-	/// MATLAB version returns shared pointer
-	boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
-		return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
-	}
-
   /// compose syntactic sugar
 	inline Pose2 operator*(const Pose2& p2) const {
 		return Pose2(r_*p2.r(), t_ + r_*p2.t());
@@ -126,11 +121,6 @@ public:
   /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
 	Pose2 retract(const Vector& v) const;
 
-  /// MATLAB version returns shared pointer
-  boost::shared_ptr<Pose2> retract_(const Vector& v) {
-    return boost::shared_ptr<Pose2>(new Pose2(retract(v)));
-  }
-
   /// Local 3D coordinates of Pose2 manifold neighborhood around current pose
 	Vector localCoordinates(const Pose2& p2) const;
 
@@ -140,9 +130,6 @@ public:
 
   /// Exponential map from Lie algebra se(2) to SE(2)
 	static Pose2 Expmap(const Vector& xi);
-	static inline boost::shared_ptr<Pose2> Expmap_(const Vector& v) {
-		return boost::shared_ptr<Pose2>(new Pose2(Expmap(v)));
-	}
 
   /// Exponential map from SE(2) to Lie algebra se(2)
 	static Vector Logmap(const Pose2& p);
@@ -159,11 +146,6 @@ public:
 			boost::optional<Matrix&> H1=boost::none,
 			boost::optional<Matrix&> H2=boost::none) const;
 
-	/// MATLAB version returns shared pointer
-	boost::shared_ptr<Pose2> between_(const Pose2& p2) {
-		return boost::shared_ptr<Pose2>(new Pose2(between(p2)));
-	}
-
 	/**
 	 * Return point coordinates in pose coordinate frame
 	 */

gtsam/geometry/Pose3.h

@@ -96,11 +96,6 @@ namespace gtsam {
         boost::optional<Matrix&> H1=boost::none,
         boost::optional<Matrix&> H2=boost::none) const;
 
-  	/// MATLAB version returns shared pointer
-  	boost::shared_ptr<Pose3> compose_(const Pose3& p2) {
-  		return boost::shared_ptr<Pose3>(new Pose3(compose(p2)));
-  	}
-
     /// compose syntactic sugar
     Pose3 operator*(const Pose3& T) const {
       return Pose3(R_*T.R_, t_ + R_*T.t_);
@@ -120,11 +115,6 @@ namespace gtsam {
     /// Retraction from R^6 to Pose3 manifold neighborhood around current pose
     Pose3 retract(const Vector& d) const;
 
-    /// 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;
 
@@ -134,9 +124,6 @@ namespace gtsam {
 
     /// Exponential map from Lie algebra se(3) to SE(3)
     static Pose3 Expmap(const Vector& xi);
-  	static inline boost::shared_ptr<Pose3> Expmap_(const Vector& v) {
-  		return boost::shared_ptr<Pose3>(new Pose3(Expmap(v)));
-  	}
 
     /// Exponential map from SE(3) to Lie algebra se(3)
     static Vector Logmap(const Pose3& p);
@@ -164,11 +151,6 @@ namespace gtsam {
         boost::optional<Matrix&> H1=boost::none,
         boost::optional<Matrix&> H2=boost::none) const;
 
-  	/// MATLAB version returns shared pointer
-  	boost::shared_ptr<Pose3> between_(const Pose3& p2) {
-  		return boost::shared_ptr<Pose3>(new Pose3(between(p2)));
-  	}
-
     /**
      * Calculate Adjoint map
      * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)

gtsam/geometry/Rot2.h

@@ -138,11 +138,6 @@ 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_);
@@ -170,11 +165,6 @@ 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)); }
 
@@ -189,9 +179,6 @@ namespace gtsam {
 			else
 				return Rot2::fromAngle(v(0));
 		}
-		static inline boost::shared_ptr<Rot2> Expmap_(const Vector& v) {
-			return boost::shared_ptr<Rot2>(new Rot2(Expmap(v)));
-		}
 
 		/// Logmap around identity - return the angle of the rotation
 		static inline Vector Logmap(const Rot2& r) {
@@ -219,11 +206,6 @@ 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,11 +152,6 @@ 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);}
 
@@ -182,11 +177,6 @@ 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)); }
 
@@ -202,9 +192,6 @@ namespace gtsam {
     	if(zero(v)) return Rot3M();
     	else return rodriguez(v);
     }
-		static inline boost::shared_ptr<Rot3M> Expmap_(const Vector& v) {
-			return boost::shared_ptr<Rot3M>(new Rot3M(Expmap(v)));
-		}
 
     /**
      * Log map at identity - return the canonical coordinates of this rotation
@@ -260,11 +247,6 @@ 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_));