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

@@ -25,40 +25,41 @@ using namespace std;
 
 namespace gtsam {
 
-  /** Explicit instantiation of base class to export members */
+/** Explicit instantiation of base class to export members */
-  INSTANTIATE_LIE(Rot3M);
+INSTANTIATE_LIE(Rot3M);
 
-	static const Matrix I3 = eye(3);
+static const Matrix I3 = eye(3);
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-	// static member functions to construct rotations
+Rot3M Rot3M::Rx(double t) {
-
-  Rot3M Rot3M::Rx(double t) {
 	double st = sin(t), ct = cos(t);
 	return Rot3M(
 			1,  0,  0,
 			0, ct,-st,
 			0, st, ct);
-  }
+}
 
-  Rot3M Rot3M::Ry(double t) {
+/* ************************************************************************* */
+Rot3M Rot3M::Ry(double t) {
 	double st = sin(t), ct = cos(t);
 	return Rot3M(
 			ct, 0, st,
 			0, 1,  0,
 			-st, 0, ct);
-  }
+}
 
-  Rot3M Rot3M::Rz(double t) {
+/* ************************************************************************* */
+Rot3M Rot3M::Rz(double t) {
 	double st = sin(t), ct = cos(t);
 	return Rot3M(
 			ct,-st, 0,
 			st, ct, 0,
 			0,  0, 1);
-  }
+}
 
-  // Considerably faster than composing matrices above !
+/* ************************************************************************* */
-  Rot3M Rot3M::RzRyRx(double x, double y, double z) {
+// Considerably faster than composing matrices above !
+Rot3M Rot3M::RzRyRx(double x, double y, double z) {
 	double cx=cos(x),sx=sin(x);
 	double cy=cos(y),sy=sin(y);
 	double cz=cos(z),sz=sin(z);
@@ -78,10 +79,10 @@ namespace gtsam {
 			_cs,  c_c + sss, -s_c + css,
 			-sy,        sc_,        cc_
 	);
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
+Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
 	// get components of axis \omega
 	double wx = w(0), wy=w(1), wz=w(2);
 	double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
@@ -97,70 +98,78 @@ 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);
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Rot3M Rot3M::rodriguez(const Vector& w) {
+Rot3M Rot3M::rodriguez(const Vector& w) {
 	double t = w.norm();
 	if (t < 1e-10) return Rot3M();
 	return rodriguez(w/t, t);
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  bool Rot3M::equals(const Rot3M & R, double tol) const {
+bool Rot3M::equals(const Rot3M & R, double tol) const {
 	return equal_with_abs_tol(matrix(), R.matrix(), tol);
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Matrix Rot3M::matrix() const {
+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 {
+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;
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Point3 Rot3M::column(int index) const{
+Point3 Rot3M::column(int index) const{
 	if(index == 3)
 		return r3_;
 	else if (index == 2)
 		return r2_;
 	else
 		return r1_; // default returns r1
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Vector Rot3M::xyz() const {
+Vector Rot3M::xyz() const {
 	Matrix I;Vector q;
 	boost::tie(I,q)=RQ(matrix());
 	return q;
-  }
+}
 
-  Vector Rot3M::ypr() const {
+/* ************************************************************************* */
+Vector Rot3M::ypr() const {
 	Vector q = xyz();
 	return Vector_(3,q(2),q(1),q(0));
-  }
+}
 
-  Vector Rot3M::rpy() const {
+/* ************************************************************************* */
+Vector Rot3M::rpy() const {
 	Vector q = xyz();
 	return Vector_(3,q(0),q(1),q(2));
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  // Log map at identity - return the canonical coordinates of this rotation
+// Log map at identity - return the canonical coordinates of this rotation
-  Vector Rot3M::Logmap(const Rot3M& R) {
+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())) *
@@ -187,45 +197,45 @@ namespace gtsam {
 				R.r3().x()-R.r1().z(),
 				R.r1().y()-R.r2().x());
 	}
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Point3 Rot3M::rotate(const Point3& p,
+Point3 Rot3M::rotate(const Point3& p,
 		boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
 	if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
 	if (H2) *H2 = matrix();
 	return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  // see doc/math.lyx, SO(3) section
+// see doc/math.lyx, SO(3) section
-  Point3 Rot3M::unrotate(const Point3& p,
+Point3 Rot3M::unrotate(const Point3& p,
 		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	const Matrix Rt(transpose());
 	Point3 q(Rt*p.vector()); // q = Rt*p
 	if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
 	if (H2) *H2 = Rt;
 	return q;
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Rot3M Rot3M::compose (const Rot3M& R2,
+Rot3M Rot3M::compose (const Rot3M& R2,
 		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	if (H1) *H1 = R2.transpose();
 	if (H2) *H2 = I3;
 	return *this * R2;
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Rot3M Rot3M::between (const Rot3M& R2,
+Rot3M Rot3M::between (const Rot3M& R2,
 		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	if (H1) *H1 = -(R2.transpose()*matrix());
 	if (H2) *H2 = I3;
 	return between_default(*this, R2);
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  pair<Matrix, Vector> RQ(const Matrix& A) {
+pair<Matrix, Vector> RQ(const Matrix& A) {
 
 	double x = -atan2(-A(2, 1), A(2, 2));
 	Rot3M Qx = Rot3M::Rx(-x);
@@ -241,8 +251,8 @@ namespace gtsam {
 
 	Vector xyz = Vector_(3, x, y, z);
 	return make_pair(R, xyz);
-	}
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
 
 } // namespace gtsam