explicit retract versions for calling in MATLAB

gtsam.h

@@ -139,6 +139,7 @@ class Rot3 {
 	bool equals(const Rot3& rot, double tol) const;
 	Vector localCoordinates(const Rot3& p);
 	Rot3 retract(Vector v);
+	Rot3 retractCayley(Vector v);
 	Rot3 compose(const Rot3& p2);
 	Rot3 between(const Rot3& p2);
 };
@@ -185,6 +186,7 @@ class Pose3 {
 	Pose3 compose(const Pose3& p2);
 	Pose3 between(const Pose3& p2);
 	Pose3 retract(Vector v);
+	Pose3 retractFirstOrder(Vector v);
 	Point3 translation() const;
 	Rot3 rotation() const;
 };

gtsam/geometry/Pose3.cpp

@@ -102,6 +102,15 @@ namespace gtsam {
 		}
   }
 
+  /* ************************************************************************* */
+  Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
+      Vector omega(sub(xi, 0, 3));
+      Point3 v(sub(xi, 3, 6));
+      Rot3 R = R_.retract(omega);  // R is done exactly
+      Point3 t = t_ + R_ * v; // First order t approximation
+      return Pose3(R, t);
+	}
+
   /* ************************************************************************* */
 	// Different versions of retract
   Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
@@ -109,24 +118,11 @@ namespace gtsam {
     	// Lie group exponential map, traces out geodesic
       return compose(Expmap(xi));
     } else if(mode == Pose3::FIRST_ORDER) {
-      Vector omega(sub(xi, 0, 3));
+    	// First order
-      Point3 v(sub(xi, 3, 6));
+      return retractFirstOrder(xi);
-
-      // R is always done exactly in all three retract versions below
-      Rot3 R = R_.retract(omega);
-
-      // Incorrect version
-      // Retracts R and t independently
-      // Point3 t = t_.retract(v.vector());
-
-      // First order t approximation
-      Point3 t = t_ + R_ * v;
-
-      // Second order t approximation
-      // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
-
-      return Pose3(R, t);
     } else {
+      // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
+      // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
       assert(false);
       exit(1);
     }

gtsam/geometry/Pose3.h

@@ -121,6 +121,8 @@ namespace gtsam {
     /// Dimensionality of the tangent space = 6 DOF
     size_t dim() const { return dimension; }
 
+    /// Retraction with fast first-order approximation to the exponential map
+    Pose3 retractFirstOrder(const Vector& d) const;
 
     /// Retraction from R^6 to Pose3 manifold neighborhood around current pose
     Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;

gtsam/geometry/Rot3.h

@@ -251,6 +251,11 @@ namespace gtsam {
 #endif
       };
 
+#ifndef GTSAM_DEFAULT_QUATERNIONS
+    /// Retraction from R^3 to Rot3 manifold using the Cayley transform
+    Rot3 retractCayley(const Vector& omega) const;
+#endif
+
     /// Retraction from R^3 to Rot3 manifold neighborhood around current rotation
     Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 

gtsam/geometry/Rot3M.cpp

@@ -246,19 +246,23 @@ Vector Rot3::Logmap(const Rot3& R) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
+Rot3 Rot3::retractCayley(const Vector& omega) const {
-  if(mode == Rot3::EXPMAP) {
-    return (*this)*Expmap(omega);
-  } else if(mode == Rot3::CAYLEY) {
 	const double x = omega(0), y = omega(1), z = omega(2);
 	const double x2 = x * x, y2 = y * y, z2 = z * z;
 	const double xy = x * y, xz = x * z, yz = y * z;
 	const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
-    return (*this)* Rot3(
+	return (*this)
-        (4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f,
+			* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
 					(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
-        (xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f
+					(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
-    );
+}
+
+/* ************************************************************************* */
+Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
+  if(mode == Rot3::EXPMAP) {
+    return (*this)*Expmap(omega);
+  } else if(mode == Rot3::CAYLEY) {
+    return retractCayley(omega);
   } else if(mode == Rot3::SLOW_CAYLEY) {
     Matrix Omega = skewSymmetric(omega);
     return (*this)*Cayley<3>(-Omega/2);