Wrapped and grouped lots of geometry methods

gtsam.h

@@ -68,72 +68,142 @@ namespace gtsam {
 //*************************************************************************
 
 class Point2 {
+  // Standard Constructors
 	Point2();
 	Point2(double x, double y);
-	static gtsam::Point2 Expmap(Vector v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Point2& pose, double tol) const;
+
+  // Group
+  static gtsam::Point2 identity();
+  gtsam::Point2 inverse() const;
+  gtsam::Point2 compose(const gtsam::Point2& p2) const;
+  gtsam::Point2 between(const gtsam::Point2& p2) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Point2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Point2& p) const;
+
+  // Lie Group
+  static gtsam::Point2 Expmap(Vector v);
 	static Vector Logmap(const gtsam::Point2& p);
-	void print(string s) const;
+
-	double x();
+  // Standard Interface
-	double y();
+  double x() const;
-	Vector localCoordinates(const gtsam::Point2& p);
+	double y() const;
-	gtsam::Point2 compose(const gtsam::Point2& p2);
+  Vector vector() const;
-	gtsam::Point2 between(const gtsam::Point2& p2);
-	gtsam::Point2 retract(Vector v);
 };
 
 class StereoPoint2 {
+  // Standard Constructors
   StereoPoint2();
   StereoPoint2(double uL, double uR, double v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::StereoPoint2& point, double tol) const;
+
+  // Group
+  static gtsam::StereoPoint2 identity();
+  gtsam::StereoPoint2 inverse() const;
+  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
+  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::StereoPoint2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::StereoPoint2& p) const;
+
+  // Lie Group
   static gtsam::StereoPoint2 Expmap(Vector v);
   static Vector Logmap(const gtsam::StereoPoint2& p);
-  void print(string s) const;
+
-  Vector localCoordinates(const gtsam::StereoPoint2& p);
+  // Standard Interface
-  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2);
+  Vector vector() const;
-  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2);
-  gtsam::StereoPoint2 retract(Vector v);
 };
 
 class Point3 {
+  // Standard Constructors
 	Point3();
 	Point3(double x, double y, double z);
 	Point3(Vector v);
-	static gtsam::Point3 Expmap(Vector v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Point3& p, double tol) const;
+
+  // Group
+  static gtsam::Point3 identity();
+  gtsam::Point3 inverse() const;
+  gtsam::Point3 compose(const gtsam::Point3& p2) const;
+  gtsam::Point3 between(const gtsam::Point3& p2) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Point3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Point3& p) const;
+
+  // Lie Group
+  static gtsam::Point3 Expmap(Vector v);
 	static Vector Logmap(const gtsam::Point3& p);
-	void print(string s) const;
+
-	bool equals(const gtsam::Point3& p, double tol);
+  // Standard Interface
 	Vector vector() const;
-	double x();
+	double x() const;
-	double y();
+	double y() const;
-	double z();
+	double z() const;
-	Vector localCoordinates(const gtsam::Point3& p);
-	gtsam::Point3 retract(Vector v);
-	gtsam::Point3 compose(const gtsam::Point3& p2);
-	gtsam::Point3 between(const gtsam::Point3& p2);
 };
 
 class Rot2 {
+  // Standard Constructors and Named Constructors
 	Rot2();
 	Rot2(double theta);
-	static gtsam::Rot2 Expmap(Vector v);
+  static gtsam::Rot2 fromAngle(double theta);
+  static gtsam::Rot2 fromDegrees(double theta);
+  static gtsam::Rot2 fromCosSin(double c, double s);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Rot2& rot, double tol) const;
+
+  // Group
+  static gtsam::Rot2 identity();
+  static gtsam::Rot2 inverse();
+  gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
+  gtsam::Rot2 between(const gtsam::Rot2& p2) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Rot2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Rot2& p) const;
+
+  // Lie Group
+  static gtsam::Rot2 Expmap(Vector v);
 	static Vector Logmap(const gtsam::Rot2& p);
-	static gtsam::Rot2 fromAngle(double theta);
+
-	static gtsam::Rot2 fromDegrees(double theta);
+  // Group Action on Point2
-	static gtsam::Rot2 fromCosSin(double c, double s);
+  gtsam::Point2 rotate(const gtsam::Point2& point) const;
+  gtsam::Point2 unrotate(const gtsam::Point2& point) const;
+
+  // Standard Interface
 	static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative
 	static gtsam::Rot2 atan2(double y, double x);
-	void print(string s) const;
-	bool equals(const gtsam::Rot2& rot, double tol) const;
 	double theta() const;
 	double degrees() const;
 	double c() const;
 	double s() const;
-	Vector localCoordinates(const gtsam::Rot2& p);
+  Matrix matrix() const;
-	gtsam::Rot2 retract(Vector v);
-	gtsam::Rot2 compose(const gtsam::Rot2& p2);
-	gtsam::Rot2 between(const gtsam::Rot2& p2);
 };
 
 class Rot3 {
+  // Standard Constructors and Named Constructors
 	Rot3();
 	Rot3(Matrix R);
 	static gtsam::Rot3 Rx(double t);
@@ -147,17 +217,29 @@ class Rot3 {
 	static gtsam::Rot3 ypr(double y, double p, double r);
 	static gtsam::Rot3 quaternion(double w, double x, double y, double z);
 	static gtsam::Rot3 rodriguez(Vector v);
+
+  // Testable
 	void print(string s) const;
 	bool equals(const gtsam::Rot3& rot, double tol) const;
+
+  // Group
 	static gtsam::Rot3 identity();
+  gtsam::Rot3 inverse() const;
 	gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
-	gtsam::Rot3 inverse() const;
 	gtsam::Rot3 between(const gtsam::Rot3& p2) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Rot3 retractCayley(Vector v) const;
+  gtsam::Rot3 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Rot3& p) const;
+
+  // Group Action on Point3
 	gtsam::Point3 rotate(const gtsam::Point3& p) const;
 	gtsam::Point3 unrotate(const gtsam::Point3& p) const;
-	gtsam::Rot3 retractCayley(Vector v) const;
+
-	gtsam::Rot3 retract(Vector v) const;
+  // Standard Interface
-	Vector localCoordinates(const gtsam::Rot3& p) const;
 	static gtsam::Rot3 Expmap(Vector v);
 	static Vector Logmap(const gtsam::Rot3& p);
 	Matrix matrix() const;
@@ -170,30 +252,53 @@ class Rot3 {
 	double pitch() const;
 	double yaw() const;
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
+  Matrix matrix() const;
 };
 
 class Pose2 {
+  // Standard Constructor
 	Pose2();
 	Pose2(double x, double y, double theta);
 	Pose2(double theta, const gtsam::Point2& t);
 	Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
 	Pose2(Vector v);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Pose2& pose, double tol) const;
+
+  // Group
+  static gtsam::Pose2 identity();
+  gtsam::Pose2 inverse() const;
+  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
+  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Pose2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Pose2& p) const;
+
+  // Lie Group
 	static gtsam::Pose2 Expmap(Vector v);
 	static Vector Logmap(const gtsam::Pose2& p);
-	void print(string s) const;
+  Matrix adjointMap() const;
-	bool equals(const gtsam::Pose2& pose, double tol) const;
+  Vector adjoint() const;
+  static Matrix wedge();
+
+  // Group Actions on Point2
+  gtsam::Point2 transform_from(const gtsam::Point2& p) const;
+  gtsam::Point2 transform_to(const gtsam::Point2& p) const;
+
+  // Standard Interface
 	double x() const;
 	double y() const;
 	double theta() const;
-	size_t dim() const;
+	gtsam::Rot2 bearing(const gtsam::Point2& point) const;
-	Vector localCoordinates(const gtsam::Pose2& p);
+	double range(const gtsam::Point2& point) const;
-	gtsam::Pose2 retract(Vector v);
-	gtsam::Pose2 compose(const gtsam::Pose2& p2);
-	gtsam::Pose2 between(const gtsam::Pose2& p2);
-	gtsam::Rot2 bearing(const gtsam::Point2& point);
-	double range(const gtsam::Point2& point);
 	gtsam::Point2 translation() const;
 	gtsam::Rot2 rotation() const;
+  Matrix matrix() const;
 };
 
 class Pose3 {
@@ -210,15 +315,15 @@ class Pose3 {
 
 	// Group
 	static gtsam::Pose3 identity();
-	gtsam::Pose3 inverse();
+	gtsam::Pose3 inverse() const;
-	gtsam::Pose3 compose(const gtsam::Pose3& p2);
+	gtsam::Pose3 compose(const gtsam::Pose3& p2) const;
-	gtsam::Pose3 between(const gtsam::Pose3& p2);
+	gtsam::Pose3 between(const gtsam::Pose3& p2) const;
 
 	// Manifold
 	static size_t Dim();
 	size_t dim() const;
-	gtsam::Pose3 retract(Vector v);
+	gtsam::Pose3 retract(Vector v) const;
-	gtsam::Pose3 retractFirstOrder(Vector v);
+	gtsam::Pose3 retractFirstOrder(Vector v) const;
 	Vector localCoordinates(const gtsam::Pose3& T2) const;
 
 	// Lie Group
@@ -245,41 +350,98 @@ class Pose3 {
 };
 
 class Cal3_S2 {
+  // Standard Constructors
   Cal3_S2();
   Cal3_S2(double fx, double fy, double s, double u0, double v0);
 
+  // Testable
   void print(string s) const;
+  bool equals(const gtsam::Cal3_S2& pose, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3_S2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3_S2& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  Vector vector() const;
+  Matrix matrix() const;
+  Matrix matrix_inverse() const;
 };
 
 class Cal3_S2Stereo {
+  // Standard Constructors
   Cal3_S2Stereo();
   Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
 
+  // Testable
   void print(string s) const;
+  bool equals(const gtsam::Cal3_S2Stereo& pose, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3_S2Stereo retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double skew() const;
+  double px() const;
+  double py() const;
+  gtsam::Point2 principalPoint() const;
+  Vector vector() const;
+  Matrix matrix() const;
+  Matrix matrix_inverse() const;
+  double baseline() const;
 };
 
 class CalibratedCamera {
-
+  // Standard Constructors and Named Constructors
 	CalibratedCamera();
 	CalibratedCamera(const gtsam::Pose3& pose);
 	CalibratedCamera(const Vector& v);
+  gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height);
 
+  // Testable
 	void print(string s) const;
 	bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
 
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::CalibratedCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
+
+  // Group
+  gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
+  gtsam::CalibratedCamera inverse() const;
+
+  // Action on Point3
+  gtsam::Point2 project(const gtsam::Point3& point) const;
+  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+
+  // Standard Interface
 	gtsam::Pose3 pose() const;
-
+  double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
-	gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
-	gtsam::CalibratedCamera inverse() const;
-	gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height);
-	gtsam::CalibratedCamera retract(const Vector& d) const;
-	Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
-
-	gtsam::Point2 project(const gtsam::Point3& point) const;
-	static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
 };
 
-
 //*************************************************************************
 // inference
 //*************************************************************************

gtsam/geometry/Pose2.cpp

@@ -108,7 +108,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
 /* ************************************************************************* */
 // Calculate Adjoint map
 // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
-Matrix Pose2::AdjointMap() const {
+Matrix Pose2::adjointMap() const {
 	double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
 	return Matrix_(3,3,
 			c,  -s,   y,
@@ -119,7 +119,7 @@ Matrix Pose2::AdjointMap() const {
 
 /* ************************************************************************* */
 Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
-	if (H1) *H1 = -AdjointMap();
+	if (H1) *H1 = -adjointMap();
 	return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
 }
 
@@ -142,7 +142,7 @@ Point2 Pose2::transform_to(const Point2& point,
 Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
 		boost::optional<Matrix&> H2) const {
 	// TODO: inline and reuse?
-	if(H1) *H1 = p2.inverse().AdjointMap();
+	if(H1) *H1 = p2.inverse().adjointMap();
 	if(H2) *H2 = I3;
 	return (*this)*p2;
 }

gtsam/geometry/Pose2.h

@@ -158,10 +158,10 @@ public:
 	 * Calculate Adjoint map
 	 * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
 	 */
-	Matrix AdjointMap() const;
+	Matrix adjointMap() const;
-	inline Vector Adjoint(const Vector& xi) const {
+	inline Vector adjoint(const Vector& xi) const {
 		assert(xi.size() == 3);
-		return AdjointMap()*xi;
+		return adjointMap()*xi;
 	}
 
 	/**