Formatting and comments, adding Rot3 and Pose3 to matlab interface

gtsam.h

@@ -11,6 +11,7 @@ class Point3 {
 	Point3(double x, double y, double z);
 	Point3(Vector v);
 	void print(string s) const;
+	bool equals(const Point3& p, double tol);
 	Vector vector() const;
 	double x();
 	double y();
@@ -21,11 +22,18 @@ class Rot2 {
 	Rot2();
 	Rot2(double theta);
 	void print(string s) const;
-	bool equals(const Rot2& pose, double tol) const;
+	bool equals(const Rot2& rot, double tol) const;
 	double c() const;
 	double s() const;
 };
 
+class Rot3 {
+	Rot3();
+	Rot3(Matrix R);
+	void print(string s) const;
+	bool equals(const Rot3& rot, double tol) const;
+};
+
 class Pose2 {
 	Pose2();
 	Pose2(double x, double y, double theta);
@@ -44,6 +52,21 @@ class Pose2 {
 	Pose2* retract_(Vector v);
 };
 
+class Pose3 {
+	Pose3();
+	Pose3(const Rot3& r, const Point3& t);
+	Pose3(Vector v);
+	void print(string s) const;
+	bool equals(const Pose3& pose, double tol) const;
+	double x() const;
+	double y() const;
+	double z() const;
+	int dim() const;
+	Pose3* compose_(const Pose3& p2);
+	Pose3* between_(const Pose3& p2);
+	Vector localCoordinates(const Pose3& p);
+};
+
 class SharedGaussian {
 	SharedGaussian(Matrix covariance);
 	void print(string s) const;

gtsam/geometry/Pose3.cpp

@@ -216,7 +216,8 @@ namespace gtsam {
   Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
   	if (H1)
 #ifdef CORRECT_POSE3_EXMAP
-		{ *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ?
+  		// FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ?
+  	{ *H1 = - AdjointMap(p); }
 #else
   	{
   		Matrix Rt = R_.transpose();

gtsam/geometry/Pose3.h

@@ -89,6 +89,11 @@ 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_);
@@ -144,12 +149,17 @@ 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)
      */
-    Matrix AdjointMap() const;
+    Matrix AdjointMap() const; /// FIXME Not tested - marked as incorrect
-    Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
+    Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } /// FIXME Not tested - marked as incorrect
 
     /**
      * wedge for Pose3:

gtsam/geometry/Rot3M.cpp

@@ -31,8 +31,6 @@ namespace gtsam {
 static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */
-	// static member functions to construct rotations
-
 Rot3M Rot3M::Rx(double t) {
 	double st = sin(t), ct = cos(t);
 	return Rot3M(
@@ -41,6 +39,7 @@ namespace gtsam {
 			0, st, ct);
 }
 
+/* ************************************************************************* */
 Rot3M Rot3M::Ry(double t) {
 	double st = sin(t), ct = cos(t);
 	return Rot3M(
@@ -49,6 +48,7 @@ namespace gtsam {
 			-st, 0, ct);
 }
 
+/* ************************************************************************* */
 Rot3M Rot3M::Rz(double t) {
 	double st = sin(t), ct = cos(t);
 	return Rot3M(
@@ -57,6 +57,7 @@ namespace gtsam {
 			0,  0, 1);
 }
 
+/* ************************************************************************* */
 // Considerably faster than composing matrices above !
 Rot3M Rot3M::RzRyRx(double x, double y, double z) {
 	double cx=cos(x),sx=sin(x);
@@ -97,7 +98,8 @@ namespace gtsam {
 	double                  C11 = c_1*wwTyy, C12 = c_1*wy*wz;
 	double                                   C22 = c_1*wwTzz;
 
-    return Rot3M(   c + C00, -swz + C01,  swy + C02,
+	return Rot3M(
+			  c + C00, -swz + C01,  swy + C02,
 			swz + C01,    c + C11, -swx + C12,
 		 -swy + C02,  swx + C12,    c + C22);
 }
@@ -116,18 +118,22 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Matrix Rot3M::matrix() const {
-    double r[] = { r1_.x(), r2_.x(), r3_.x(),
+	Matrix R(3,3);
+	R <<
+			r1_.x(), r2_.x(), r3_.x(),
 			r1_.y(), r2_.y(), r3_.y(),
-        r1_.z(), r2_.z(), r3_.z() };
+			r1_.z(), r2_.z(), r3_.z();
-    return Matrix_(3,3, r);
+	return R;
 }
 
 /* ************************************************************************* */
 Matrix Rot3M::transpose() const {
-    double r[] = { r1_.x(), r1_.y(), r1_.z(),
+	Matrix Rt(3,3);
+	Rt <<
+			r1_.x(), r1_.y(), r1_.z(),
 			r2_.x(), r2_.y(), r2_.z(),
-        r3_.x(), r3_.y(), r3_.z()};
+			r3_.x(), r3_.y(), r3_.z();
-    return Matrix_(3,3, r);
+	return Rt;
 }
 
 /* ************************************************************************* */
@@ -147,11 +153,13 @@ namespace gtsam {
 	return q;
 }
 
+/* ************************************************************************* */
 Vector Rot3M::ypr() const {
 	Vector q = xyz();
 	return Vector_(3,q(2),q(1),q(0));
 }
 
+/* ************************************************************************* */
 Vector Rot3M::rpy() const {
 	Vector q = xyz();
 	return Vector_(3,q(0),q(1),q(2));
@@ -161,6 +169,7 @@ namespace gtsam {
 // Log map at identity - return the canonical coordinates of this rotation
 Vector Rot3M::Logmap(const Rot3M& R) {
 	double tr = R.r1().x()+R.r2().y()+R.r3().z();
+	// FIXME should tr in statement below be absolute value?
 	if (tr > 3.0 - 1e-17) {   // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
 		return zero(3);
 	} else if (tr > 3.0 - 1e-10)  {   // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
@@ -170,6 +179,7 @@ namespace gtsam {
 				R.r2().z()-R.r3().y(),
 				R.r3().x()-R.r1().z(),
 				R.r1().y()-R.r2().x());
+		// FIXME: in statement below, is this the right comparision?
 	} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
 		if(fabs(R.r3().z() - -1.0) > 1e-10)
 			return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *