diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 5b335f564..12f299637 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -42,18 +42,28 @@ public:
 	Point2(double x, double y): x_(x), y_(y) {}
 	Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); }
 
-	/** dimension of the variable - used to autodetect sizes */
-	inline static size_t Dim() { return dimension; }
+  /// @name Testable
+  /// @{
 
-	/** print with optional string */
+	/// print with optional string
 	void print(const std::string& s = "") const;
 
-	/** equals with an tolerance, prints out message if unequal*/
+	/// equals with an tolerance, prints out message if unequal
 	bool equals(const Point2& q, double tol = 1e-9) const;
 
-	// Group requirements
+  /// @}
+  /// @name Group
+  /// @{
 
-	/** "Compose", just adds the coordinates of two points. With optional derivatives */
+	/// identity
+	inline static Point2 identity() {
+		return Point2();
+	}
+
+	/// "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2()
+	inline Point2 inverse() const { return Point2(-x_, -y_); }
+
+	/// "Compose", just adds the coordinates of two points. With optional derivatives
 	inline Point2 compose(const Point2& p2,
 			boost::optional<Matrix&> H1=boost::none,
 			boost::optional<Matrix&> H2=boost::none) const {
@@ -62,33 +72,60 @@ public:
 		return *this + p2;
 	}
 
-	/** identity */
-	inline static Point2 identity() {
-		return Point2();
-	}
+	/** operators */
+	inline Point2 operator- () const {return Point2(-x_,-y_);}
+	inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
+	inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
+	inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
+	inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
 
-	/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
-	inline Point2 inverse() const { return Point2(-x_, -y_); }
+  /// @}
+  /// @name Manifold
+  /// @{
 
-	// Manifold requirements
+	/// dimension of the variable - used to autodetect sizes
+	inline static size_t Dim() { return dimension; }
 
-	/** Size of the tangent space */
+  /// Dimensionality of tangent space = 2 DOF
 	inline size_t dim() const { return dimension; }
 
-	/** Updates a with tangent space delta */
+	/// Updates a with tangent space delta
 	inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
 
 	/// Local coordinates of manifold neighborhood around current value
 	inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
 
-	/** Lie requirements */
+  /// @}
+  /// @name Lie Group
+  /// @{
 
-	/** Exponential map around identity - just create a Point2 from a vector */
+	/// Exponential map around identity - just create a Point2 from a vector
 	static inline Point2 Expmap(const Vector& v) { return Point2(v); }
 
-	/** Log map around identity - just return the Point2 as a vector */
+	/// 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()); }
 
+  /// @}
+  /// @name Vector Operators
+  /// @{
+
+	/** norm of point */
+	double norm() const;
+
+	/** creates a unit vector */
+	Point2 unit() const { return *this/norm(); }
+
+	/** distance between two points */
+	inline double dist(const Point2& p2) const {
+		return (p2 - *this).norm();
+	}
+
+	/** operators */
+	inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
+	inline void operator *= (double s) {x_*=s;y_*=s;}
+	inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
+
+  /// @}
 
 	/** "Between", subtracts point coordinates */
 	inline Point2 between(const Point2& p2,
@@ -106,27 +143,6 @@ public:
 	/** return vectorized form (column-wise) */
 	Vector vector() const { return Vector_(2, x_, y_); }
 
-	/** operators */
-	inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
-	inline void operator *= (double s) {x_*=s;y_*=s;}
-	inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
-	inline Point2 operator- () const {return Point2(-x_,-y_);}
-	inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
-	inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
-	inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
-	inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
-
-	/** norm of point */
-	double norm() const;
-
-	/** creates a unit vector */
-	Point2 unit() const { return *this/norm(); }
-
-	/** distance between two points */
-	inline double dist(const Point2& p2) const {
-		return (p2 - *this).norm();
-	}
-
 private:
 	/** Serialization function */
 	friend class boost::serialization::access;
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 2f8c9f8cc..a01f5009b 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -46,29 +46,28 @@ namespace gtsam {
     Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
     Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
 
+    /// @name Testable
+    /// @{
+
     /** print with optional string */
     void print(const std::string& s = "") const;
 
     /** equals with an tolerance */
     bool equals(const Point3& p, double tol = 1e-9) const;
 
-    /** dimension of the variable - used to autodetect sizes */
-    inline static size_t Dim() { return dimension; }
+    /// @}
+    /// @name Group
+    /// @{
 
-    /** Lie requirements */
-
-    /** return DOF, dimensionality of tangent space */
-    inline size_t dim() const { return dimension; }
-
-		/** identity */
+    /// identity for group operation
 		inline static Point3 identity() {
 			return Point3();
 		}
 
-    /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */
+    /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
     inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
 
-    /** "Compose" - just adds coordinates of two points */
+    /// "Compose" - just adds coordinates of two points
     inline Point3 compose(const Point3& p2,
     		boost::optional<Matrix&> H1=boost::none,
     		boost::optional<Matrix&> H2=boost::none) const {
@@ -77,20 +76,58 @@ namespace gtsam {
   	  return *this + p2;
     }
 
+    /// @}
+    /// @name Manifold
+    /// @{
+
+    /// dimension of the variable - used to autodetect sizes
+    inline static size_t Dim() { return dimension; }
+
+    /// return dimensionality of tangent space, DOF = 3
+    inline size_t dim() const { return dimension; }
+
+  	/// Updates a with tangent space delta
+  	inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
+
+  	/// Returns inverse retraction
+  	inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
+
+    /// @}
+    /// @name Lie Group
+    /// @{
+
     /** Exponential map at identity - just create a Point3 from x,y,z */
     static inline Point3 Expmap(const Vector& v) { return Point3(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()); }
 
-  	// Manifold requirements
+    /// @}
+    /// @name Vector Operators
+    /// @{
 
-  	inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
+    Point3 operator - () const { return Point3(-x_,-y_,-z_);}
+    bool   operator ==(const Point3& q) const;
+    Point3 operator + (const Point3& q) const;
+    Point3 operator - (const Point3& q) const;
+    Point3 operator * (double s) const;
+    Point3 operator / (double s) const;
 
-  	/**
-  	 * Returns inverse retraction
-  	 */
-  	inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
+    /** distance between two points */
+    double dist(const Point3& p2) const {
+      return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
+    }
+
+    /** dot product */
+    double norm() const;
+
+    /** cross product @return this x q */
+    Point3 cross(const Point3 &q) const;
+
+    /** dot product @return this * q*/
+    double dot(const Point3 &q) const;
+
+    /// @}
 
     /** Between using the default implementation */
     inline Point3 between(const Point3& p2,
@@ -112,19 +149,6 @@ namespace gtsam {
     inline double y() const {return y_;}
     inline double z() const {return z_;}
 
-    /** operators */
-    Point3 operator - () const { return Point3(-x_,-y_,-z_);}
-    bool   operator ==(const Point3& q) const;
-    Point3 operator + (const Point3& q) const;
-    Point3 operator - (const Point3& q) const;
-    Point3 operator * (double s) const;
-    Point3 operator / (double s) const;
-
-    /** distance between two points */
-    double dist(const Point3& p2) const {
-      return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
-    }
-
     /** add two points, add(this,q) is same as this + q */
     Point3 add (const Point3 &q,
   	      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
@@ -133,15 +157,6 @@ namespace gtsam {
     Point3 sub (const Point3 &q,
   	      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
-    /** cross product @return this x q */
-    Point3 cross(const Point3 &q) const;
-
-    /** dot product @return this * q*/
-    double dot(const Point3 &q) const;
-
-    /** dot product */
-    double norm() const;
-
   private:
     /** Serialization function */
     friend class boost::serialization::access;
@@ -154,7 +169,7 @@ namespace gtsam {
     }
   };
 
-  /** Syntactic sugar for multiplying coordinates by a scalar s*p */
+  /// Syntactic sugar for multiplying coordinates by a scalar s*p
   inline Point3 operator*(double s, const Point3& p) { return p*s;}
 
 }
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 0e4bf4824..de311249a 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -79,7 +79,6 @@ public:
 		*this = Expmap(v);
 	}
 
-
   /// @name Testable
   /// @{
 
@@ -94,9 +93,7 @@ public:
   /// @{
 
   /// identity for group operation
-	inline static Pose2 identity() {
-		return Pose2();
-	}
+	inline static Pose2 identity() { return Pose2(); }
 
   /// inverse transformation with derivatives
 	Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
@@ -123,7 +120,6 @@ public:
   /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
 	inline static size_t Dim() { return dimension; }
 
-
   /// Dimensionality of tangent space = 3 DOF
 	inline size_t dim() const { return dimension; }
 
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index b8951c3ee..f03dcde06 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -128,8 +128,6 @@ namespace gtsam {
 
     Pose3 transform_to(const Pose3& pose) const;
 
-    /** Lie requirements */
-
     /** receives the point in Pose coordinates and transforms it to world coordinates */
     Point3 transform_from(const Point3& p,
         boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 3a8fb86f0..1a6d91b20 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -109,27 +109,25 @@ namespace gtsam {
 			return s_;
 		}
 
+    /// @name Testable
+    /// @{
+
 		/** print */
 		void print(const std::string& s = "theta") const;
 
 		/** equals with an tolerance */
 		bool equals(const Rot2& R, double tol = 1e-9) const;
 
-		/** dimension of the variable - used to autodetect sizes */
-		inline static size_t Dim() {
-			return dimension;
-		}
-
-		/** Lie requirements */
-
-		/** Dimensionality of the tangent space */
-		inline size_t dim() const {
-			return dimension;
-		}
+	  /// @}
+	  /// @name Group
+	  /// @{
 
 		/** identity */
-		inline static Rot2 identity() {
-			return Rot2();
+		inline static Rot2 identity() {	return Rot2(); }
+
+		/** The inverse rotation - negative angle */
+		Rot2 inverse() const {
+			return Rot2(c_, -s_);
 		}
 
 		/** Compose - make a new rotation by adding angles */
@@ -140,7 +138,41 @@ namespace gtsam {
 			return *this * R1;
 		}
 
-		/** Expmap around identity - create a rotation from an angle */
+		/** 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_);
+		}
+
+		/** syntactic sugar for rotate */
+		inline Point2 operator*(const Point2& p) const {
+			return rotate(p);
+		}
+
+	  /// @}
+	  /// @name Manifold
+	  /// @{
+
+		/// dimension of the variable - used to autodetect sizes
+		inline static size_t Dim() {
+			return dimension;
+		}
+
+		/// Dimensionality of the tangent space, DOF = 1
+		inline size_t dim() const {
+			return dimension;
+		}
+
+  	/// Updates a with tangent space delta
+  	inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
+
+  	/// Returns inverse retraction
+  	inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
+
+	  /// @}
+	  /// @name Lie Group
+	  /// @{
+
+		/// Expmap around identity - create a rotation from an angle
 		static Rot2 Expmap(const Vector& v) {
 			if (zero(v))
 				return (Rot2());
@@ -148,19 +180,23 @@ namespace gtsam {
 				return Rot2::fromAngle(v(0));
 		}
 
-		/** Logmap around identity - return the angle of the rotation */
+		/// Logmap around identity - return the angle of the rotation
 		static inline Vector Logmap(const Rot2& r) {
 			return Vector_(1, r.theta());
 		}
 
-  	// Manifold requirements
+		/// @}
+	  /// @name Vector Operators
+		/// @{
 
-  	inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
+		/**
+		 * Creates a unit vector as a Point2
+		 */
+		inline Point2 unit() const {
+			return Point2(c_, s_);
+		}
 
-  	/**
-  	 * Returns inverse retraction
-  	 */
-  	inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
+		/// @}
 
 		/** Between using the default implementation */
 		inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
@@ -176,16 +212,6 @@ namespace gtsam {
 		/** return 2*2 transpose (inverse) rotation matrix   */
 		Matrix transpose() const;
 
-		/** The inverse rotation - negative angle */
-		Rot2 inverse() const {
-			return Rot2(c_, -s_);
-		}
-
-		/** 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_);
-		}
-
 		/**
 		 * rotate point from rotated coordinate frame to
 		 * world = R*p
@@ -193,11 +219,6 @@ namespace gtsam {
 		Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
 				boost::optional<Matrix&> H2 = boost::none) const;
 
-		/** syntactic sugar for rotate */
-		inline Point2 operator*(const Point2& p) const {
-			return rotate(p);
-		}
-
 		/**
 		 * rotate point from world to rotated
 		 * frame = R'*p
@@ -205,12 +226,6 @@ namespace gtsam {
 		Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
 				boost::optional<Matrix&> H2 = boost::none) const;
 
-		/**
-		 * Creates a unit vector as a Point2
-		 */
-		inline Point2 unit() const {
-			return Point2(c_, s_);
-		}
 
 	private:
 		/** Serialization function */
diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h
index 0c17060b3..e41e01ef3 100644
--- a/gtsam/geometry/Rot3M.h
+++ b/gtsam/geometry/Rot3M.h
@@ -130,12 +130,76 @@ namespace gtsam {
     static Rot3M rodriguez(double wx, double wy, double wz)
   		{ return rodriguez(Vector_(3,wx,wy,wz));}
 
+    /// @name Testable
+    /// @{
+
     /** print */
     void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
 
     /** equals with an tolerance */
     bool equals(const Rot3M& p, double tol = 1e-9) const;
 
+    /// @}
+    /// @name Group
+    /// @{
+
+    /// identity for group operation
+    inline static Rot3M identity() {
+      return Rot3M();
+    }
+
+    /// Compose two rotations i.e., R= (*this) * R2
+    Rot3M compose(const Rot3M& R2,
+  	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+
+    /// rotate point from rotated coordinate frame to world = R*p
+    inline Point3 operator*(const Point3& p) const { return rotate(p);}
+
+    /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M()
+    Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const {
+    	if (H1) *H1 = -matrix();
+    	return Rot3M(
+    			r1_.x(), r1_.y(), r1_.z(),
+    			r2_.x(), r2_.y(), r2_.z(),
+    			r3_.x(), r3_.y(), r3_.z());
+    }
+
+    /// @}
+    /// @name Manifold
+    /// @{
+
+    /// dimension of the variable - used to autodetect sizes
+    static size_t Dim() { return dimension; }
+
+    /// return dimensionality of tangent space, DOF = 3
+    size_t dim() const { return dimension; }
+
+  	/// Updates a with tangent space delta
+    Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
+
+    /// Returns inverse retraction
+    Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
+
+    /// @}
+    /// @name Lie Group
+    /// @{
+
+    /**
+     * Exponential map at identity - create a rotation from canonical coordinates
+     * using Rodriguez' formula
+     */
+		static Rot3M Expmap(const Vector& v)  {
+    	if(zero(v)) return Rot3M();
+    	else return rodriguez(v);
+    }
+
+    /**
+     * Log map at identity - return the canonical coordinates of this rotation
+     */
+		static Vector Logmap(const Rot3M& R);
+
+    /// @}
+
     /** return 3*3 rotation matrix */
     Matrix matrix() const;
 
@@ -176,54 +240,6 @@ namespace gtsam {
           r1_.z(), r2_.z(), r3_.z()).finished());
     }
 
-    /** dimension of the variable - used to autodetect sizes */
-    static size_t Dim() { return dimension; }
-
-    /** Lie requirements */
-
-    /** return DOF, dimensionality of tangent space */
-    size_t dim() const { return dimension; }
-
-    /** Compose two rotations i.e., R= (*this) * R2
-     */
-    Rot3M compose(const Rot3M& R2,
-  	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
-
-    /** Exponential map at identity - create a rotation from canonical coordinates
-     * using Rodriguez' formula
-     */
-		static Rot3M Expmap(const Vector& v)  {
-    	if(zero(v)) return Rot3M();
-    	else return rodriguez(v);
-    }
-
-  /** identity */
-  inline static Rot3M identity() {
-    return Rot3M();
-  }
-
-    // Log map at identity - return the canonical coordinates of this rotation
-  static Vector Logmap(const Rot3M& R);
-
-  // Manifold requirements
-
-  Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
-
-  /**
-   * Returns inverse retraction
-   */
-  Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
-
-
-    // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M()
-    Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const {
-    	if (H1) *H1 = -matrix();
-    	return Rot3M(
-    			r1_.x(), r1_.y(), r1_.z(),
-    			r2_.x(), r2_.y(), r2_.z(),
-    			r3_.x(), r3_.y(), r3_.z());
-    }
-
     /**
      * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
      */
@@ -236,12 +252,6 @@ namespace gtsam {
       return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
     }
 
-    /**
-     * rotate point from rotated coordinate frame to
-     * world = R*p
-     */
-    inline Point3 operator*(const Point3& p) const { return rotate(p);}
-
     /**
      * rotate point from rotated coordinate frame to
      * world = R*p
diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h
index d21f7e2fb..5986a8103 100644
--- a/gtsam/geometry/Rot3Q.h
+++ b/gtsam/geometry/Rot3Q.h
@@ -127,12 +127,76 @@ namespace gtsam {
     static Rot3Q rodriguez(double wx, double wy, double wz)
   		{ return rodriguez(Vector_(3,wx,wy,wz));}
 
+    /// @name Testable
+    /// @{
+
     /** print */
     void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
 
     /** equals with an tolerance */
     bool equals(const Rot3Q& p, double tol = 1e-9) const;
 
+    /// @}
+    /// @name Group
+    /// @{
+
+    /// identity for group operation
+    inline static Rot3Q identity() {
+      return Rot3Q();
+    }
+
+    /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q()
+    Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const {
+    	if (H1) *H1 = -matrix();
+    	return Rot3Q(quaternion_.inverse());
+    }
+
+    /// Compose two rotations i.e., R= (*this) * R2
+    Rot3Q compose(const Rot3Q& R2,
+  	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+
+    /// rotate point from rotated coordinate frame to world = R*p
+    inline Point3 operator*(const Point3& p) const {
+      Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
+      return Point3(r(0), r(1), r(2));
+    }
+
+    /// @}
+    /// @name Manifold
+    /// @{
+
+    /// dimension of the variable - used to autodetect sizes
+    static size_t Dim() { return dimension; }
+
+    /// return dimensionality of tangent space, DOF = 3
+    size_t dim() const { return dimension; }
+
+    /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
+    Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); }
+
+    /// Returns inverse retraction
+    Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); }
+
+    /// @}
+    /// @name Lie Group
+    /// @{
+
+    /**
+     * Exponential map at identity - create a rotation from canonical coordinates
+     * using Rodriguez' formula
+     */
+		static Rot3Q Expmap(const Vector& v)  {
+    	if(zero(v)) return Rot3Q();
+    	else return rodriguez(v);
+    }
+
+    /**
+     * Log map at identity - return the canonical coordinates of this rotation
+     */
+    static Vector Logmap(const Rot3Q& R);
+
+    /// @}
+
     /** return 3*3 rotation matrix */
     Matrix matrix() const;
 
@@ -167,51 +231,6 @@ namespace gtsam {
      */
     Quaternion toQuaternion() const { return quaternion_; }
 
-    /** dimension of the variable - used to autodetect sizes */
-    static size_t Dim() { return dimension; }
-
-    /** Lie requirements */
-
-    /** return DOF, dimensionality of tangent space */
-    size_t dim() const { return dimension; }
-
-    /** Compose two rotations i.e., R= (*this) * R2
-     */
-    Rot3Q compose(const Rot3Q& R2,
-  	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
-
-    /** Exponential map at identity - create a rotation from canonical coordinates
-     * using Rodriguez' formula
-     */
-		static Rot3Q Expmap(const Vector& v)  {
-    	if(zero(v)) return Rot3Q();
-    	else return rodriguez(v);
-    }
-
-  /** identity */
-  inline static Rot3Q identity() {
-    return Rot3Q();
-  }
-
-    // Log map at identity - return the canonical coordinates of this rotation
-    static Vector Logmap(const Rot3Q& R);
-  
-  // Manifold requirements
-
-  Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); }
-
-  /**
-   * Returns inverse retraction
-   */
-  Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); }
-
-
-    // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q()
-    Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const {
-    	if (H1) *H1 = -matrix();
-    	return Rot3Q(quaternion_.inverse());
-    }
-
     /**
      * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
      */
@@ -222,15 +241,6 @@ namespace gtsam {
     /** compose two rotations */
     Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); }
 
-    /**
-     * rotate point from rotated coordinate frame to
-     * world = R*p
-     */
-    inline Point3 operator*(const Point3& p) const {
-      Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
-      return Point3(r(0), r(1), r(2));
-    }
-
     /**
      * rotate point from rotated coordinate frame to
      * world = R*p
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index e493b0047..b0f2d738c 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -44,6 +44,9 @@ namespace gtsam {
 			uL_(uL), uR_(uR), v_(v) {
 		}
 
+    /// @name Testable
+    /// @{
+
 		/** print */
 		void print(const std::string& s="") const;
 
@@ -53,49 +56,53 @@ namespace gtsam {
 					- q.v_) < tol);
 		}
 
-	    /** dimension of the variable - used to autodetect sizes */
-	    inline static size_t Dim() { return dimension; }
+    /// @}
+    /// @name Group
+    /// @{
 
-		/** Lie requirements */
-		inline size_t dim() const { return dimension; }
+		/// identity
+		inline static StereoPoint2 identity() { return StereoPoint2(); }
 
-		/** convert to vector */
-		Vector vector() const {
-			return Vector_(3, uL_, uR_, v_);
+		/// inverse
+		inline StereoPoint2 inverse() const {
+			return StereoPoint2()- (*this);
 		}
 
-		/** add two stereo points */
-		StereoPoint2 operator+(const StereoPoint2& b) const {
-			return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
-		}
-
-		/** subtract two stereo points */
-		StereoPoint2 operator-(const StereoPoint2& b) const {
-			return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
-		}
-
-		/*
-		 * convenient function to get a Point2 from the left image
-		 */
-		inline Point2 point2(){
-			return Point2(uL_, v_);
-		}
-
-		/** "Compose", just adds the coordinates of two points. */
+		/// "Compose", just adds the coordinates of two points.
 		inline StereoPoint2 compose(const StereoPoint2& p1) const {
 			return *this + p1;
 		}
 
-		/** identity */
-		inline static StereoPoint2 identity() {
-			return StereoPoint2();
+		/// add two stereo points
+		StereoPoint2 operator+(const StereoPoint2& b) const {
+			return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
 		}
 
-		/** inverse */
-		inline StereoPoint2 inverse() const {
-			return StereoPoint2()- (*this);
+		/// subtract two stereo points
+		StereoPoint2 operator-(const StereoPoint2& b) const {
+			return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
 		}
 
+    /// @}
+    /// @name Manifold
+    /// @{
+
+    /// dimension of the variable - used to autodetect sizes */
+    inline static size_t Dim() { return dimension; }
+
+    /// return dimensionality of tangent space, DOF = 3
+    inline size_t dim() const { return dimension; }
+
+  	/// Updates a with tangent space delta
+		inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
+
+		/// Returns inverse retraction
+		inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
+
+    /// @}
+    /// @name Lie Group
+    /// @{
+
 		/** Exponential map around identity - just create a Point2 from a vector */
 		static inline StereoPoint2 Expmap(const Vector& d) {
 			return StereoPoint2(d(0), d(1), d(2));
@@ -106,14 +113,17 @@ namespace gtsam {
 			return p.vector();
 		}
 
-		// Manifold requirements
+		/// @}}
 
-		inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
+		/** convert to vector */
+		Vector vector() const {
+			return Vector_(3, uL_, uR_, v_);
+		}
 
-		/**
-		 * Returns inverse retraction
-		 */
-		inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
+		/** convenient function to get a Point2 from the left image */
+		inline Point2 point2(){
+			return Point2(uL_, v_);
+		}
 
 		inline StereoPoint2 between(const StereoPoint2& p2) const {
 			return gtsam::between_default(*this, p2);