From 458cc52fffc83f3736a1f81bea2da8250ead15a4 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sun, 22 Aug 2010 21:45:53 +0000
Subject: [PATCH] Switched geometry to use primarily member functions for
 calculations with optional derivatives, so there are no more optional
 derivative functions.  Also split Vector specializations for Lie into
 gtsam/base/LieVector.h which will later change into a real wrapper function. 
 Specialized numericalDerivative to allow for functions to return doubles. 
 Projects ISAM2, MastSLAM updated.

---
 base/Lie.h                              |  27 ----
 base/LieVector.h                        |  27 ++++
 base/Makefile.am                        |   2 +-
 base/numericalDerivative.h              | 161 ++++++++++++++++++++++-
 geometry/Cal3_S2.cpp                    |  23 +---
 geometry/Cal3_S2.h                      |  15 +--
 geometry/CalibratedCamera.cpp           | 104 +++++----------
 geometry/CalibratedCamera.h             |  72 ++++-------
 geometry/Point3.cpp                     |  43 +++----
 geometry/Point3.h                       |  31 ++---
 geometry/Pose2.cpp                      | 159 +++++++++++------------
 geometry/Pose2.h                        | 103 +++++++--------
 geometry/Pose3.cpp                      |  51 ++++----
 geometry/Pose3.h                        |  62 +++++----
 geometry/Rot2.cpp                       | 150 ++++++++++------------
 geometry/Rot2.h                         |  31 ++---
 geometry/Rot3.cpp                       |  26 ++--
 geometry/Rot3.h                         |  16 +--
 geometry/SimpleCamera.cpp               |  77 ++++-------
 geometry/SimpleCamera.h                 |  48 +------
 geometry/StereoCamera.cpp               | 163 ++++++++++++++++--------
 geometry/StereoCamera.h                 |  22 ++--
 geometry/tests/testCal3_S2.cpp          |   9 +-
 geometry/tests/testCalibratedCamera.cpp |  27 +---
 geometry/tests/testPoint3.cpp           |   5 +-
 geometry/tests/testPose2.cpp            |  65 ++++++----
 geometry/tests/testPose3.cpp            |  52 ++++----
 geometry/tests/testRot2.cpp             |  11 +-
 geometry/tests/testRot3.cpp             |  20 +--
 geometry/tests/testSimpleCamera.cpp     |  21 +--
 geometry/tests/testStereoCamera.cpp     |   9 +-
 geometry/timeCalibratedCamera.cpp       | 101 ++++++++-------
 nonlinear/LieConfig-inl.h               |   1 +
 slam/BearingFactor.h                    |   2 +-
 slam/BearingRangeFactor.h               |   4 +-
 slam/RangeFactor.h                      |   2 +-
 slam/Simulated3D.h                      |   1 +
 slam/TransformConstraint.h              |   6 +-
 slam/pose2SLAM.h                        |   1 +
 slam/visualSLAM.h                       |   4 +-
 tests/testTransformConstraint.cpp       |  10 +-
 41 files changed, 881 insertions(+), 883 deletions(-)
 create mode 100644 base/LieVector.h

diff --git a/base/Lie.h b/base/Lie.h
index 487f97460..aacd0a531 100644
--- a/base/Lie.h
+++ b/base/Lie.h
@@ -132,33 +132,6 @@ namespace gtsam {
     return obj1.equals(obj2);
   }
 
-  // The rest of the file makes double and Vector behave as a Lie type (with + as compose)
-
-  // double,+ group operations
-  inline double compose(double p1,double p2) { return p1+p2;}
-  inline double inverse(double p) { return -p;}
-  inline double between(double p1,double p2) { return p2-p1;}
-
-  // double,+ is a trivial Lie group
-  template<> inline double expmap(const Vector& d) { return d(0);}
-  template<> inline double expmap(const double& p,const Vector& d) { return p+d(0);}
-  inline Vector logmap(const double& p) { return repeat(1,p);}
-  inline Vector logmap(const double& p1,const double& p2) { return Vector_(1,p2-p1);}
-
-  // Global functions needed for double
-  inline size_t dim(const double& v) { return 1; }
-
-  // Vector group operations
-  inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
-  inline Vector inverse(const Vector& p) { return -p;}
-  inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
-
-  // Vector is a trivial Lie group
-  template<> inline Vector expmap(const Vector& d) { return d;}
-  template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
-  inline Vector logmap(const Vector& p) { return p;}
-  inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
-
   /**
    *  Three term approximation of the Baker�Campbell�Hausdorff formula
    *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
diff --git a/base/LieVector.h b/base/LieVector.h
new file mode 100644
index 000000000..f9eb95427
--- /dev/null
+++ b/base/LieVector.h
@@ -0,0 +1,27 @@
+/**
+ * @file LieVector.h
+ * @brief A wrapper around vector providing Lie compatibility
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>
+
+namespace gtsam {
+/** temporarily using the old system */
+
+// The rest of the file makes double and Vector behave as a Lie type (with + as compose)
+
+// Vector group operations
+inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
+inline Vector inverse(const Vector& p) { return -p;}
+inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
+
+// Vector is a trivial Lie group
+template<> inline Vector expmap(const Vector& d) { return d;}
+template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
+template<> inline Vector logmap(const Vector& p) { return p;}
+template<> inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
+
+} // \namespace gtsam
diff --git a/base/Makefile.am b/base/Makefile.am
index 0c2551f78..f4e56ff6f 100644
--- a/base/Makefile.am
+++ b/base/Makefile.am
@@ -26,7 +26,7 @@ endif
 headers += Testable.h TestableAssertions.h numericalDerivative.h 
 
 # Lie Groups
-headers += Lie.h Lie-inl.h
+headers += Lie.h Lie-inl.h LieVector.h
 
 # Data structures
 headers += BTree.h DSF.h
diff --git a/base/numericalDerivative.h b/base/numericalDerivative.h
index 4529e6023..53fb41d8b 100644
--- a/base/numericalDerivative.h
+++ b/base/numericalDerivative.h
@@ -94,6 +94,28 @@ namespace gtsam {
 		return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
 	}
 
+	/** pseudo-template specialization for double Y values */
+	template<class X>
+	Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
+		double hx = h(x);
+		double factor = 1.0/(2.0*delta);
+		const size_t m = 1, n = dim(x);
+		Vector d(n,0.0);
+		Matrix H = zeros(m,n);
+		for (size_t j=0;j<n;j++) {
+			d(j) +=   delta; Vector hxplus = Vector_(1, h(expmap(x,d))-hx);
+			d(j) -= 2*delta; Vector hxmin  = Vector_(1, h(expmap(x,d))-hx);
+			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+		}
+		return H;
+	}
+
+	template<class X>
+	Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
+		return numericalDerivative11<X>(boost::bind(h, _1), x, delta);
+	}
+
 	/**
 	 * Compute numerical derivative in argument 1 of binary function
 	 * @param h binary function yielding m-vector
@@ -121,11 +143,35 @@ namespace gtsam {
 	}
 
 	template<class Y, class X1, class X2>
-	Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
+	inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
 			const X1& x1, const X2& x2, double delta=1e-5) {
 		return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
 	}
 
+	/** pseudo-partial template specialization for double return values */
+	template<class X1, class X2>
+	Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		double hx = h(x1,x2);
+		double factor = 1.0/(2.0*delta);
+		const size_t m = 1, n = dim(x1);
+		Vector d(n,0.0);
+		Matrix H = zeros(m,n);
+		for (size_t j=0;j<n;j++) {
+			d(j) +=   delta; Vector hxplus = Vector_(1, h(expmap(x1,d),x2)-hx);
+			d(j) -= 2*delta; Vector hxmin  = Vector_(1, h(expmap(x1,d),x2)-hx);
+			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+		}
+		return H;
+	}
+
+	template<class X1, class X2>
+	inline Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		return numericalDerivative21<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
+	}
+
 	/**
 	 * Compute numerical derivative in argument 2 of binary function
 	 * @param h binary function yielding m-vector
@@ -154,12 +200,34 @@ namespace gtsam {
 		return H;
 	}
 	template<class Y, class X1, class X2>
-	Matrix numericalDerivative22
-	(Y (*h)(const X1&, const X2&),
-			const X1& x1, const X2& x2, double delta=1e-5) {
+	inline Matrix numericalDerivative22
+	(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
 		return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
 	}
 
+	/** pseudo-specialization for double Y values */
+	template<class X1, class X2>
+	Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		double hx = h(x1,x2);
+		double factor = 1.0/(2.0*delta);
+		const size_t m = 1, n = dim(x2);
+		Vector d(n,0.0);
+		Matrix H = zeros(m,n);
+		for (size_t j=0;j<n;j++) {
+			d(j) +=   delta; Vector hxplus = Vector_(1,h(x1,expmap(x2,d))-hx);
+			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(x1,expmap(x2,d))-hx);
+			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+		}
+		return H;
+	}
+	template<class X1, class X2>
+	inline Matrix numericalDerivative22	(double (*h)(const X1&, const X2&),
+			const X1& x1, const X2& x2, double delta=1e-5) {
+		return numericalDerivative22<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
+	}
+
 	/**
 	 * Compute numerical derivative in argument 1 of ternary function
 	 * @param h ternary function yielding m-vector
@@ -189,7 +257,7 @@ namespace gtsam {
 		return H;
 	}
 	template<class Y, class X1, class X2, class X3>
-	Matrix numericalDerivative31
+	inline Matrix numericalDerivative31
 	(Y (*h)(const X1&, const X2&, const X3&),
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
 		return numericalDerivative31<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
@@ -215,7 +283,7 @@ namespace gtsam {
 		return H;
 	}
 	template<class Y, class X1, class X2, class X3>
-	Matrix numericalDerivative32
+	inline Matrix numericalDerivative32
 	(Y (*h)(const X1&, const X2&, const X3&),
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
 		return numericalDerivative32<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
@@ -241,9 +309,88 @@ namespace gtsam {
 		return H;
 	}
 	template<class Y, class X1, class X2, class X3>
-	Matrix numericalDerivative33
+	inline Matrix numericalDerivative33
 	(Y (*h)(const X1&, const X2&, const X3&),
 			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
 		return numericalDerivative33<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
 	}
+
+
+	/**
+	 * specializations for double outputs
+	 */
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative31
+	(boost::function<double(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		double hx = h(x1,x2,x3);
+		double factor = 1.0/(2.0*delta);
+		const size_t m = 1, n = dim(x1);
+		Vector d(n,0.0);
+		Matrix H = zeros(m,n);
+		for (size_t j=0;j<n;j++) {
+			d(j) +=   delta; Vector hxplus = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
+			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
+			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+		}
+		return H;
+	}
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative31
+	(double (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative31<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
+	}
+
+	// arg 2
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative32
+	(boost::function<double(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		double hx = h(x1,x2,x3);
+		double factor = 1.0/(2.0*delta);
+		const size_t m = 1, n = dim(x2);
+		Vector d(n,0.0);
+		Matrix H = zeros(m,n);
+		for (size_t j=0;j<n;j++) {
+			d(j) +=   delta; Vector hxplus = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
+			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
+			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+		}
+		return H;
+	}
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative32
+	(double (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative32<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
+	}
+
+	// arg 3
+	template<class X1, class X2, class X3>
+	Matrix numericalDerivative33
+	(boost::function<double(const X1&, const X2&, const X3&)> h,
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		double hx = h(x1,x2,x3);
+		double factor = 1.0/(2.0*delta);
+		const size_t m = 1, n = dim(x3);
+		Vector d(n,0.0);
+		Matrix H = zeros(m,n);
+		for (size_t j=0;j<n;j++) {
+			d(j) +=   delta; Vector hxplus = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
+			d(j) -= 2*delta; Vector hxmin  = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
+			d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
+			for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+		}
+		return H;
+	}
+	template<class X1, class X2, class X3>
+	inline Matrix numericalDerivative33
+	(double (*h)(const X1&, const X2&, const X3&),
+			const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
+		return numericalDerivative33<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
+	}
+
 }
diff --git a/geometry/Cal3_S2.cpp b/geometry/Cal3_S2.cpp
index 3a321c607..406ae27df 100644
--- a/geometry/Cal3_S2.cpp
+++ b/geometry/Cal3_S2.cpp
@@ -42,23 +42,12 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
 }
 
 /* ************************************************************************* */
-
-Point2 uncalibrate(const Cal3_S2& K, const Point2& p) {
-	return K.uncalibrate(p);
-}
-
-/* ************************************************************************* */
-
-Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p) {
-	return Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,
-			1.0);
-}
-
-/* ************************************************************************* */
-
-Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p) {
-
-	return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
+Point2 Cal3_S2::uncalibrate(const Point2& p,
+		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	if (H1) *H1 = Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,1.0);
+	if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
+	const double x = p.x(), y = p.y();
+	return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
 /* ************************************************************************* */
diff --git a/geometry/Cal3_S2.h b/geometry/Cal3_S2.h
index 3760f9134..a0f1a14bc 100644
--- a/geometry/Cal3_S2.h
+++ b/geometry/Cal3_S2.h
@@ -85,11 +85,11 @@ namespace gtsam {
 
 		/**
 		 * convert intrinsic coordinates xy to image coordinates uv
+		 * with optional derivatives
 		 */
-		Point2 uncalibrate(const Point2& p) const {
-			const double x = p.x(), y = p.y();
-			return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-		}
+		Point2 uncalibrate(const Point2& p,
+				boost::optional<Matrix&> H1 = boost::none,
+				boost::optional<Matrix&> H2 = boost::none) const;
 
 		/**
 		 * convert image coordinates uv to intrinsic coordinates xy
@@ -100,7 +100,6 @@ namespace gtsam {
 		}
 
 		/** friends */
-		friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
 		friend Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d);
 
 	private:
@@ -132,10 +131,4 @@ namespace gtsam {
             cal.s_ + d(2), cal.u0_ + d(3), cal.v0_ + d(4));
     }
 
-	/**
-	 * convert intrinsic coordinates xy to image coordinates uv
-	 */
-	Point2 uncalibrate(const Cal3_S2& K, const Point2& p);
-	Matrix Duncalibrate1(const Cal3_S2& K, const Point2& p);
-	Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
 }
diff --git a/geometry/CalibratedCamera.cpp b/geometry/CalibratedCamera.cpp
index b695441fe..951cf49e7 100644
--- a/geometry/CalibratedCamera.cpp
+++ b/geometry/CalibratedCamera.cpp
@@ -10,27 +10,6 @@
 
 namespace gtsam {
 
-	/* ************************************************************************* */
-	// Auxiliary functions
-	/* ************************************************************************* */
-
-	Point2 project_to_camera(const Point3& P) {
-		return Point2(P.x() / P.z(), P.y() / P.z());
-	}
-
-	Matrix Dproject_to_camera1(const Point3& P) {
-		double d = 1.0 / P.z(), d2 = d * d;
-		return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
-	}
-
-	Point3 backproject_from_camera(const Point2& p, const double scale) {
-		return Point3(p.x() * scale, p.y() * scale, scale);
-	}
-
-	/* ************************************************************************* */
-	// Methods
-	/* ************************************************************************* */
-
 	CalibratedCamera::CalibratedCamera(const Pose3& pose) :
 		pose_(pose) {
 	}
@@ -39,6 +18,18 @@ namespace gtsam {
 
 	CalibratedCamera::~CalibratedCamera() {}
 
+	Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional<Matrix&> H1) {
+		if (H1) {
+			double d = 1.0 / P.z(), d2 = d * d;
+			*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
+		}
+		return Point2(P.x() / P.z(), P.y() / P.z());
+	}
+
+	Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) {
+		return Point3(p.x() * scale, p.y() * scale, scale);
+	}
+
 	CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) {
 		double st = sin(pose2.theta()), ct = cos(pose2.theta());
 		Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
@@ -48,66 +39,29 @@ namespace gtsam {
 		return CalibratedCamera(pose3);
 	}
 
-	Point2 CalibratedCamera::project(const Point3 & P) const {
-		Point3 cameraPoint = Pose3::transform_to(pose_, P);
-		Point2 intrinsic = project_to_camera(cameraPoint);
-		return intrinsic;
-	}
-
-	/* ************************************************************************* */
-	// measurement functions and derivatives
-	/* ************************************************************************* */
-
-	Point2 project(const CalibratedCamera& camera, const Point3& point) {
-		return camera.project(point);
-	}
-
-	/* ************************************************************************* */
-	Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) {
-		const Pose3& pose = camera.pose();
+	Point2 CalibratedCamera::project(const Point3& point,
+		    boost::optional<Matrix&> D_intrinsic_pose,
+		    boost::optional<Matrix&> D_intrinsic_point) const {
+		const Pose3& pose = pose_;
 		const Rot3& R = pose.rotation();
 		const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
-		Point3 q = Pose3::transform_to(pose, point);
-		double X = q.x(), Y = q.y(), Z = q.z();
-		double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
-		return Matrix_(2,6,
-				X*Yd2, -Z*d-X*Xd2,  d*Y, -d*r1.x()+r3.x()*Xd2, -d*r1.y()+r3.y()*Xd2, -d*r1.z()+r3.z()*Xd2,
-				d*Z+Y*Yd2, -X*Yd2, -d*X, -d*r2.x()+r3.x()*Yd2, -d*r2.y()+r3.y()*Yd2, -d*r2.z()+r3.z()*Yd2);
-	}
+		Point3 q = pose.transform_to(point);
 
-	/* ************************************************************************* */
-	Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) {
-		const Pose3& pose = camera.pose();
-		const Rot3& R = pose.rotation();
-		const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
-		Point3 q = Pose3::transform_to(pose, point);
-		double X = q.x(), Y = q.y(), Z = q.z();
-		double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
-		return Matrix_(2,3,
-				d*r1.x()-r3.x()*Xd2, d*r1.y()-r3.y()*Xd2, d*r1.z()-r3.z()*Xd2,
-				d*r2.x()-r3.x()*Yd2, d*r2.y()-r3.y()*Yd2, d*r2.z()-r3.z()*Yd2);
-	}
-
-	/* ************************************************************************* */
-	Point2 Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
-			Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
-
-		const Pose3& pose = camera.pose();
-		const Rot3& R = pose.rotation();
-		const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
-		Point3 q = Pose3::transform_to(pose, point);
-		double X = q.x(), Y = q.y(), Z = q.z();
-		double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
-		double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
-		double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
-		D_intrinsic_pose = Matrix_(2,6,
-				X*Yd2, -Z*d-X*Xd2,  d*Y, -dp11, -dp12, -dp13,
-				d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
-		D_intrinsic_point = Matrix_(2,3,
+		if (D_intrinsic_pose || D_intrinsic_point) {
+			double X = q.x(), Y = q.y(), Z = q.z();
+			double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
+			double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
+			double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
+			if (D_intrinsic_pose)
+				*D_intrinsic_pose = Matrix_(2,6,
+						X*Yd2, -Z*d-X*Xd2,  d*Y, -dp11, -dp12, -dp13,
+						d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
+			if (D_intrinsic_point)
+				*D_intrinsic_point = Matrix_(2,3,
 						dp11, dp12, dp13,
 						dp21, dp22, dp23);
+		}
 		return project_to_camera(q);
 	}
-
 /* ************************************************************************* */
 }
diff --git a/geometry/CalibratedCamera.h b/geometry/CalibratedCamera.h
index 4412b23df..697492ec8 100644
--- a/geometry/CalibratedCamera.h
+++ b/geometry/CalibratedCamera.h
@@ -15,22 +15,6 @@ namespace gtsam {
 
 	class Point2;
 
-	/**
-	 * projects a 3-dimensional point in camera coordinates into the
-	 * camera and returns a 2-dimensional point, no calibration applied
-	 */
-	Point2 project_to_camera(const Point3& cameraPoint);
-
-	/**
-	 * Derivative of project_to_camera
-	 */
-	Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/
-
-	/**
-	 * backproject a 2-dimensional point to a 3-dimension point
-	 */
-	Point3 backproject_from_camera(const Point2& p, const double scale);
-
 	/**
 	 * A Calibrated camera class [R|-R't], calibration K=I.
 	 * If calibration is known, it is more computationally efficient
@@ -67,35 +51,35 @@ namespace gtsam {
 		 */
 		static CalibratedCamera level(const Pose2& pose2, double height);
 
-		Point2 project(const Point3& P) const;
+		/* ************************************************************************* */
+		// measurement functions and derivatives
+		/* ************************************************************************* */
+
+		/**
+		 * This function receives the camera pose and the landmark location and
+		 * returns the location the point is supposed to appear in the image
+		 * @param camera the CalibratedCamera
+		 * @param point a 3D point to be projected
+		 * @return the intrinsic coordinates of the projected point
+		 */
+		Point2 project(const Point3& point,
+			    boost::optional<Matrix&> D_intrinsic_pose = boost::none,
+			    boost::optional<Matrix&> D_intrinsic_point = boost::none) const;
+
+		/**
+		 * projects a 3-dimensional point in camera coordinates into the
+		 * camera and returns a 2-dimensional point, no calibration applied
+		 * With optional 2by3 derivative
+		 */
+		static Point2 project_to_camera(const Point3& cameraPoint,
+				boost::optional<Matrix&> H1 = boost::none);
+
+		/**
+		 * backproject a 2-dimensional point to a 3-dimension point
+		 */
+		static Point3 backproject_from_camera(const Point2& p, const double scale);
+
 	};
-
-	/* ************************************************************************* */
-	// measurement functions and derivatives
-	/* ************************************************************************* */
-
-	/**
-	 * This function receives the camera pose and the landmark location and
-	 * returns the location the point is supposed to appear in the image
-	 * @param camera the CalibratedCamera
-	 * @param point a 3D point to be projected
-	 * @return the intrinsic coordinates of the projected point
-	 */
-	Point2 project(const CalibratedCamera& camera, const Point3& point);
-
-	/**
-	 * Derivatives of project, same paramaters as project
-	 */
-	Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point);
-	Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point);
-
-	/**
-	 * super-duper combined evaluation + derivatives
-	 * saves a lot of time because a lot of computation is shared
-	 * @return project(camera,point)
-	 */
-	Point2 Dproject_pose_point(const CalibratedCamera& camera,
-			const Point3& point, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
 }
 
 #endif /* CalibratedCAMERA_H_ */
diff --git a/geometry/Point3.cpp b/geometry/Point3.cpp
index 30a92e9c2..25543c3ec 100644
--- a/geometry/Point3.cpp
+++ b/geometry/Point3.cpp
@@ -48,43 +48,40 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Point3 Point3::add(const Point3 &p, const Point3 &q) {
-    return p+q;
-  }
+//  Point3 Point3::add(const Point3 &q) const {
+//    return *this + q;
+//  }
   /* ************************************************************************* */
-  Point3 Point3::add(const Point3 &p, const Point3 &q,
-	      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+  Point3 Point3::add(const Point3 &q,
+	      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	  if (H1) *H1 = eye(3,3);
 	  if (H2) *H2 = eye(3,3);
-	  return add(p,q);
+	  return *this + q;
   }
   /* ************************************************************************* */
-  Point3 Point3::sub(const Point3 &p, const Point3 &q) {
-    return p+q;
-  }
+//  Point3 Point3::sub(const Point3 &q) const {
+//    return *this - q;
+//  }
   /* ************************************************************************* */
-  Point3 Point3::sub(const Point3 &p, const Point3 &q,
-	      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+  Point3 Point3::sub(const Point3 &q,
+	      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	  if (H1) *H1 = eye(3,3);
 	  if (H2) *H2 = -eye(3,3);
-	  return sub(p,q);
+	  return *this - q;
   }
   /* ************************************************************************* */
-  Point3 Point3::cross(const Point3 &p, const Point3 &q)
-  {
-    return Point3( p.y_*q.z_ - p.z_*q.y_,
-        p.z_*q.x_ - p.x_*q.z_,
-        p.x_*q.y_ - p.y_*q.x_ );
+  Point3 Point3::cross(const Point3 &q) const {
+    return Point3( y_*q.z_ - z_*q.y_,
+        z_*q.x_ - x_*q.z_,
+        x_*q.y_ - y_*q.x_ );
   }
   /* ************************************************************************* */
-  double Point3::dot(const Point3 &p, const Point3 &q)
-  {
-    return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ );
+  double Point3::dot(const Point3 &q) const {
+    return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
   }
   /* ************************************************************************* */
-  double Point3::norm(const Point3 &p)
-  {
-    return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
+  double Point3::norm() const {
+    return sqrt( x_*x_ + y_*y_ + z_*z_ );
   }
   /* ************************************************************************* */
 
diff --git a/geometry/Point3.h b/geometry/Point3.h
index e45ba022d..98f81a4f3 100644
--- a/geometry/Point3.h
+++ b/geometry/Point3.h
@@ -84,29 +84,22 @@ namespace gtsam {
       return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
     }
 
-    /** add two points, add(p,q) is same as p+q */
-    static Point3 add (const Point3 &p, const Point3 &q);
-    static Point3 add (const Point3 &p, const Point3 &q,
-  	      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
+    /** 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;
 
-    /** subtract two points, sub(p,q) is same as p-q */
-    static Point3 sub (const Point3 &p, const Point3 &q);
-    static Point3 sub (const Point3 &p, const Point3 &q,
-  	      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none);
+    /** subtract two points, sub(this,q) is same as this - q */
+    Point3 sub (const Point3 &q,
+  	      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
-    /** cross product */
-    static Point3 cross(const Point3 &p, const Point3 &q);
+    /** 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 */
-    static double dot(const Point3 &p, const Point3 &q);
-
-    /** dot product */
-    static double norm(const Point3 &p);
-
-//    /** friends */
-//    friend Point3 cross(const Point3 &p1, const Point3 &p2);
-//    friend double dot(const Point3 &p1, const Point3 &p2);
-//    friend double norm(const Point3 &p1);
+    double norm() const;
 
   private:
     /** Serialization function */
diff --git a/geometry/Pose2.cpp b/geometry/Pose2.cpp
index 7ef36ba99..0888b3401 100644
--- a/geometry/Pose2.cpp
+++ b/geometry/Pose2.cpp
@@ -83,135 +83,118 @@ namespace gtsam {
   /* ************************************************************************* */
   // Calculate Adjoint map
   // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
-  Matrix AdjointMap(const Pose2& p) {
-		const Rot2 R = p.r();
-		const Point2 t = p.t();
-  	double c = R.c(), s = R.s(), x = t.x(), y = t.y();
-		return Matrix_(3,3,
-				  c,  -s,   y,
-				  s,   c,  -x,
-				0.0, 0.0, 1.0
-				);
-	}
+  Matrix Pose2::AdjointMap() const {
+	  double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
+	  return Matrix_(3,3,
+			  c,  -s,   y,
+			  s,   c,  -x,
+			  0.0, 0.0, 1.0
+	  );
+  }
 
   /* ************************************************************************* */
   Pose2 Pose2::inverse() const {
-	  const Rot2& R = r_;
-	  const Point2& t = t_;
-	  return Pose2(R.inverse(), R.unrotate(Point2(-t.x(), -t.y())));
+	  return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
   }
 
   Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1) {
-	  if (H1) *H1 = -AdjointMap(pose);
+	  if (H1) *H1 = -pose.AdjointMap();
 	  return pose.inverse();
   }
 
   /* ************************************************************************* */
   // see doc/math.lyx, SE(2) section
-  Point2 Pose2::transform_to(const Pose2& pose, const Point2& point, boost::optional<
-			Matrix&> H1, boost::optional<Matrix&> H2) {
-		const Rot2& R = pose.r();
-		Point2 d = point - pose.t();
-		Point2 q = R.unrotate(d);
+  Point2 Pose2::transform_to(const Point2& point,
+		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+		Point2 d = point - t_;
+		Point2 q = r_.unrotate(d);
 		if (!H1 && !H2) return q;
 		if (H1) *H1 = Matrix_(2, 3,
 					-1.0, 0.0,  q.y(),
 					0.0, -1.0, -q.x());
-		if (H2) *H2 = R.transpose();
+		if (H2) *H2 = r_.transpose();
 		return q;
 	}
 
   /* ************************************************************************* */
   // see doc/math.lyx, SE(2) section
-
   Pose2 compose(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
       boost::optional<Matrix&> H2) {
     // TODO: inline and reuse?
-    if(H1) *H1 = AdjointMap(inverse(p2));
+    if(H1) *H1 = inverse(p2).AdjointMap();
     if(H2) *H2 = I3;
     return p1*p2;
   }
 
   /* ************************************************************************* */
   // see doc/math.lyx, SE(2) section
-  Point2 Pose2::transform_from(const Pose2& pose, const Point2& p,
-  		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-  	const Rot2& rot = pose.r();
-		const Point2 q = rot * p;
-  	if (H1 || H2) {
-			const Matrix R = rot.matrix();
-			const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
-	  	if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
-			if (H2) *H2 = R;                         // R
-  	}
-		return q + pose.t();
+  Point2 Pose2::transform_from(const Point2& p,
+		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	  const Point2 q = r_ * p;
+	  if (H1 || H2) {
+		  const Matrix R = r_.matrix();
+		  const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
+		  if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
+		  if (H2) *H2 = R;                         // R
+	  }
+	  return q + t_;
   }
 
   /* ************************************************************************* */
   Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
-			boost::optional<Matrix&> H2) {
-  	// get cosines and sines from rotation matrices
-  	const Rot2& R1 = p1.r(), R2 = p2.r();
-  	double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
+		  boost::optional<Matrix&> H2) {
+	  // get cosines and sines from rotation matrices
+	  const Rot2& R1 = p1.r(), R2 = p2.r();
+	  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
 
-  	// Calculate delta rotation = between(R1,R2)
-		double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
-    Rot2 R(Rot2::atan2(s,c)); // normalizes
+	  // Calculate delta rotation = between(R1,R2)
+	  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
+	  Rot2 R(Rot2::atan2(s,c)); // normalizes
 
-  	// Calculate delta translation = unrotate(R1, dt);
-		Point2 dt = p2.t() - p1.t();
-		double x = dt.x(), y = dt.y();
-		Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
+	  // Calculate delta translation = unrotate(R1, dt);
+	  Point2 dt = p2.t() - p1.t();
+	  double x = dt.x(), y = dt.y();
+	  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
 
-		// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
-		if (H1) {
-			double dt1 = -s2 * x + c2 * y;
-			double dt2 = -c2 * x - s2 * y;
-			H1->resize(3,3);
-			double data[9] = {
-				-c,  -s,  dt1,
-				 s,  -c,  dt2,
-			 0.0, 0.0, -1.0};
-			 copy(data, data+9, H1->data().begin());
-		}
-		if (H2) *H2 = I3;
+	  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
+	  if (H1) {
+		  double dt1 = -s2 * x + c2 * y;
+		  double dt2 = -c2 * x - s2 * y;
+		  H1->resize(3,3);
+		  double data[9] = {
+				  -c,  -s,  dt1,
+				  s,  -c,  dt2,
+				  0.0, 0.0, -1.0};
+		  copy(data, data+9, H1->data().begin());
+	  }
+	  if (H2) *H2 = I3;
 
-		return Pose2(R,t);
-	}
+	  return Pose2(R,t);
+  }
 
   /* ************************************************************************* */
-	Rot2 bearing(const Pose2& pose, const Point2& point) {
-		Point2 d = Pose2::transform_to(pose, point);
-		return Rot2::relativeBearing(d);
-	}
-
-	Rot2 bearing(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-		if (!H1 && !H2) return bearing(pose, point);
-		Point2 d = Pose2::transform_to(pose, point, H1, H2);
-		Matrix D_result_d;
-		Rot2 result = Rot2::relativeBearing(d, D_result_d);
-		if (H1) *H1 = D_result_d * (*H1);
-		if (H2) *H2 = D_result_d * (*H2);
-		return result;
-	}
+  Rot2 Pose2::bearing(const Point2& point,
+		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	  Point2 d = transform_to(point, H1, H2);
+	  if (!H1 && !H2) return Rot2::relativeBearing(d);
+	  Matrix D_result_d;
+	  Rot2 result = Rot2::relativeBearing(d, D_result_d);
+	  if (H1) *H1 = D_result_d * (*H1);
+	  if (H2) *H2 = D_result_d * (*H2);
+	  return result;
+  }
 
   /* ************************************************************************* */
-	double range(const Pose2& pose, const Point2& point) {
-		Point2 d = Pose2::transform_to(pose, point);
-		return d.norm();
-	}
-
-	double range(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-		if (!H1 && !H2) return range(pose, point);
-		Point2 d = Pose2::transform_to(pose, point, H1, H2);
-		double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
-		Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
-		if (H1) *H1 = D_result_d * (*H1);
-		if (H2) *H2 = D_result_d * (*H2);
-		return n;
-	}
+  double Pose2::range(const Point2& point,
+		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	  if (!H1 && !H2) return transform_to(point).norm();
+	  Point2 d = transform_to(point, H1, H2);
+	  double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
+	  Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
+	  if (H1) *H1 = D_result_d * (*H1);
+	  if (H2) *H2 = D_result_d * (*H2);
+	  return n;
+  }
 
   /* ************************************************************************* */
 } // namespace gtsam
diff --git a/geometry/Pose2.h b/geometry/Pose2.h
index 9538868fc..31dc6a26a 100644
--- a/geometry/Pose2.h
+++ b/geometry/Pose2.h
@@ -63,6 +63,7 @@ namespace gtsam {
     /** assert equality up to a tolerance */
     bool equals(const Pose2& pose, double tol = 1e-9) const;
 
+    /** compose syntactic sugar */
     inline Pose2 operator*(const Pose2& p2) const {
     	return Pose2(r_*p2.r(), t_ + r_*p2.t());
     }
@@ -81,6 +82,9 @@ namespace gtsam {
     /** compose with another pose */
     inline Pose2 compose(const Pose2& p) const { return *this * p; }
 
+    /** syntactic sugar for transform_from */
+    inline Point2 operator*(const Point2& point) { return transform_from(point);}
+
     /**
      * Exponential map from se(2) to SE(2)
      */
@@ -97,18 +101,53 @@ namespace gtsam {
     /**
      * Return point coordinates in pose coordinate frame
      */
-    static inline Point2 transform_to(const Pose2& pose, const Point2& point)
-  		{ return Rot2::unrotate(pose.r(), point - pose.t());}
-    static Point2 transform_to(const Pose2& pose, const Point2& point,
-  		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+    Point2 transform_to(const Point2& point,
+    		boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
     /**
      * Return point coordinates in global frame
      */
-    static inline Point2 transform_from(const Pose2& pose, const Point2& point)
-  		{ return Rot2::rotate(pose.r(), point) + pose.t();}
-    static Point2 transform_from(const Pose2& pose, const Point2& point,
-    	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+    Point2 transform_from(const Point2& point,
+    	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+
+	/**
+	 * Calculate bearing to a landmark
+	 * @param point 2D location of landmark
+	 * @return 2D rotation \in SO(2)
+	 */
+	Rot2 bearing(const Point2& point,
+			boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+
+	/**
+	 * Calculate range to a landmark
+	 * @param point 2D location of landmark
+	 * @return range (double)
+	 */
+	double range(const Point2& point,
+			boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+
+	/**
+	 * Calculate Adjoint map
+	 * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
+	 */
+	Matrix AdjointMap() const;
+	inline Vector Adjoint(const Vector& xi) const {
+		return AdjointMap()*xi;
+	}
+
+	/**
+	 * wedge for SE(2):
+	 * @param xi 3-dim twist (v,omega) where
+	 *  omega is angular velocity
+	 *  v (vx,vy) = 2D velocity
+	 * @return xihat, 3*3 element of Lie algebra that can be exponentiated
+	 */
+	static inline Matrix wedge(double vx, double vy, double w) {
+		return Matrix_(3,3,
+				0.,-w,  vx,
+				w,  0., vy,
+				0., 0.,  0.);
+	}
 
     /** get functions for x, y, theta */
     inline double x()     const { return t_.x(); }
@@ -128,30 +167,10 @@ namespace gtsam {
     }
   }; // Pose2
 
-  /**
-  * Calculate Adjoint map
-  * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
-  */
-  Matrix AdjointMap(const Pose2& p);
-  inline Vector Adjoint(const Pose2& p, const Vector& xi) { return AdjointMap(p)*xi;}
-
-  /**
-   * wedge for SE(2):
-   * @param xi 3-dim twist (v,omega) where
-   *  omega is angular velocity
-   *  v (vx,vy) = 2D velocity
-   * @return xihat, 3*3 element of Lie algebra that can be exponentiated
-   */
-  inline Matrix wedge(double vx, double vy, double w) {
-  	return Matrix_(3,3,
-  			 0.,-w,  vx,
-  			 w,  0., vy,
-  			 0., 0.,  0.);
-  }
-
+  /** specialization for pose2 wedge function (generic template in Lie.h) */
   template <>
   inline Matrix wedge<Pose2>(const Vector& xi) {
-  	return wedge(xi(0),xi(1),xi(2));
+  	return Pose2::wedge(xi(0),xi(1),xi(2));
   }
 
   /**
@@ -166,35 +185,11 @@ namespace gtsam {
     boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2 = boost::none);
 
-  /** syntactic sugar for transform_from */
-  inline Point2 operator*(const Pose2& pose, const Point2& point)
-		{ return Pose2::transform_from(pose, point);}
-
   /**
    * Return relative pose between p1 and p2, in p1 coordinate frame
    */
   Pose2 between(const Pose2& p1, const Pose2& p2,
   	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
-	/**
-	 * Calculate bearing to a landmark
-	 * @param pose 2D pose of robot
-	 * @param point 2D location of landmark
-	 * @return 2D rotation \in SO(2)
-	 */
-	Rot2 bearing(const Pose2& pose, const Point2& point);
-	Rot2 bearing(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
-
-	/**
-	 * Calculate range to a landmark
-	 * @param pose 2D pose of robot
-	 * @param point 2D location of landmark
-	 * @return range (double)
-	 */
-	double range(const Pose2& pose, const Point2& point);
-	double range(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
-
 } // namespace gtsam
 
diff --git a/geometry/Pose3.cpp b/geometry/Pose3.cpp
index 482d24340..6e76e0d86 100644
--- a/geometry/Pose3.cpp
+++ b/geometry/Pose3.cpp
@@ -21,9 +21,9 @@ namespace gtsam {
   // Calculate Adjoint map
   // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
   // Experimental - unit tests of derivatives based on it do not check out yet
-  Matrix AdjointMap(const Pose3& p) {
-		const Matrix R = p.rotation().matrix();
-		const Vector t = p.translation().vector();
+  Matrix Pose3::AdjointMap() const {
+		const Matrix R = R_.matrix();
+		const Vector t = t_.vector();
 		Matrix A = skewSymmetric(t)*R;
 		Matrix DR = collect(2, &R, &Z3);
 		Matrix Dt = collect(2, &A, &R);
@@ -134,55 +134,56 @@ namespace gtsam {
   /* ************************************************************************* */
   Pose3 Pose3::transform_to(const Pose3& pose) const {
 		Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_));
-		Point3 t = Pose3::transform_to(pose, t_);
+		Point3 t = pose.transform_to(t_);
 		return Pose3(cRv, t);
 	}
 
   /* ************************************************************************* */
-  Point3 Pose3::transform_from(const Pose3& pose, const Point3& p) {
-    return pose.rotation() * p + pose.translation();
-  }
+//  Point3 Pose3::transform_from(const Pose3& pose, const Point3& p) {
+//    return pose.rotation() * p + pose.translation();
+//  }
 
   /* ************************************************************************* */
-  Point3 Pose3::transform_from(const Pose3& pose, const Point3& p,
-		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+  Point3 Pose3::transform_from(const Point3& p,
+		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	  if (H1) {
 #ifdef CORRECT_POSE3_EXMAP
-			const Matrix R = pose.rotation().matrix();
+			const Matrix R = R_.matrix();
 			Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
 			*H1 = collect(2,&DR,&R);
 #else
 			Matrix DR;
-			Rot3::rotate(pose.rotation(), p, DR, boost::none);
+			R_.rotate(p, DR, boost::none);
 			*H1 = collect(2,&DR,&I3);
 #endif
 	  }
-	  if (H2) *H2 = pose.rotation().matrix();
-	  return Pose3::transform_from(pose, p);
+	  if (H2) *H2 = R_.matrix();
+	  return R_ * p + t_;
   }
 
   /* ************************************************************************* */
-  Point3 Pose3::transform_to(const Pose3& pose, const Point3& p) {
-    Point3 sub = p - pose.translation();
-    return Rot3::unrotate(pose.rotation(), sub);
-  }
+//  Point3 Pose3::transform_to(const Pose3& pose, const Point3& p) {
+//    Point3 sub = p - pose.translation();
+//    return pose.rotation().unrotate(sub);
+//  }
 
   /* ************************************************************************* */
-  Point3 Pose3::transform_to(const Pose3& pose, const Point3& p,
-    		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+  Point3 Pose3::transform_to(const Point3& p,
+    		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	  const Point3 result = R_.unrotate(p - t_);
 	  if (H1) { // *H1 = Dtransform_to1(pose, p);
-		Point3 q = transform_to(pose,p);
+		const Point3& q = result;
 		Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
 #ifdef CORRECT_POSE3_EXMAP
 		*H1 =  collect(2, &DR, &_I3);
 #else
-		Matrix DT = - pose.rotation().transpose(); // negative because of sub
+		Matrix DT = - R_.transpose(); // negative because of sub
 		*H1 =  collect(2,&DR,&DT);
 #endif
 	  }
 
-	  if (H2) *H2 = pose.rotation().transpose();
-	  return Pose3::transform_to(pose, p);
+	  if (H2) *H2 = R_.transpose();
+	  return result;
   }
 
   /* ************************************************************************* */
@@ -197,7 +198,7 @@ namespace gtsam {
 		Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
 		Matrix DR = collect(2, &DR_R1, &DR_t1);
 		Matrix Dt;
-		Pose3::transform_from(p1,t2, Dt, boost::none);
+		p1.transform_from(t2, Dt, boost::none);
 		*H1 = gtsam::stack(2, &DR, &Dt);
 #endif
 	  }
@@ -226,7 +227,7 @@ namespace gtsam {
 		const Point3& t = p.translation();
 		Matrix Rt = R.transpose();
 		Matrix DR_R1 = -R.matrix(), DR_t1 = Z3;
-		Matrix Dt_R1 = -skewSymmetric(Rot3::unrotate(R,t).vector()), Dt_t1 = -Rt;
+		Matrix Dt_R1 = -skewSymmetric(R.unrotate(t).vector()), Dt_t1 = -Rt;
 		Matrix DR = collect(2, &DR_R1, &DR_t1);
 		Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
 		*H1 = gtsam::stack(2, &DR, &Dt);
diff --git a/geometry/Pose3.h b/geometry/Pose3.h
index 34be60a5b..ded52a718 100644
--- a/geometry/Pose3.h
+++ b/geometry/Pose3.h
@@ -87,14 +87,15 @@ namespace gtsam {
     inline Pose3 compose(const Pose3& t) const { return *this * t; }
 
     /** receives the point in Pose coordinates and transforms it to world coordinates */
-    static Point3 transform_from(const Pose3& pose, const Point3& p);
-    static Point3 transform_from(const Pose3& pose, const Point3& p,
-  		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+    Point3 transform_from(const Point3& p,
+  		  	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+
+    /** syntactic sugar for transform */
+    inline Point3 operator*(const Point3& p) { return transform_from(p); }
 
     /** receives the point in world coordinates and transforms it to Pose coordinates */
-    static Point3 transform_to(const Pose3& pose, const Point3& p);
-    static Point3 transform_to(const Pose3& pose, const Point3& p,
-    		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+    Point3 transform_to(const Point3& p,
+    		  	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
     /** Exponential map at identity - create a pose with a translation and
      * rotation (in canonical coordinates). */
@@ -104,6 +105,28 @@ namespace gtsam {
      * coordinates of a pose. */
     static Vector Logmap(const Pose3& p);
 
+    /**
+     * Calculate Adjoint map
+     * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
+     */
+    Matrix AdjointMap() const;
+    inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
+
+    /**
+     * wedge for Pose3:
+     * @param xi 6-dim twist (omega,v) where
+     *  omega = (wx,wy,wz) 3D angular velocity
+     *  v (vx,vy,vz) = 3D velocity
+     * @return xihat, 4*4 element of Lie algebra that can be exponentiated
+     */
+    static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
+    	return Matrix_(4,4,
+    			 0.,-wz,  wy,  vx,
+    			 wz,  0.,-wx,  vy,
+    			-wy, wx,   0., vz,
+    			 0.,  0.,  0.,  0.);
+    }
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
@@ -120,9 +143,6 @@ namespace gtsam {
   /** Logarithm map around another pose T1 */
   Vector logmap(const Pose3& T1, const Pose3& T2);
 
-  /** syntactic sugar for transform */
-  inline Point3 operator*(const Pose3& pose, const Point3& p) { return Pose3::transform_from(pose, p); }
-
   /**
    * Derivatives of compose
    */
@@ -140,21 +160,6 @@ namespace gtsam {
   Pose3 between(const Pose3& p1, const Pose3& p2,
   		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
-  /**
-   * wedge for Pose3:
-   * @param xi 6-dim twist (omega,v) where
-   *  omega = (wx,wy,wz) 3D angular velocity
-   *  v (vx,vy,vz) = 3D velocity
-   * @return xihat, 4*4 element of Lie algebra that can be exponentiated
-   */
-  inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
-  	return Matrix_(4,4,
-  			 0.,-wz,  wy,  vx,
-  			 wz,  0.,-wx,  vy,
-  			-wy, wx,   0., vz,
-  			 0.,  0.,  0.,  0.);
-  }
-
   /**
    * wedge for Pose3:
    * @param xi 6-dim twist (omega,v) where
@@ -164,14 +169,7 @@ namespace gtsam {
    */
   template <>
   inline Matrix wedge<Pose3>(const Vector& xi) {
-  	return wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
+  	return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
   }
 
-  /**
-   * Calculate Adjoint map
-   * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
-   */
-  Matrix AdjointMap(const Pose3& p);
-  inline Vector Adjoint(const Pose3& p, const Vector& xi) {return AdjointMap(p)*xi; }
-
 } // namespace gtsam
diff --git a/geometry/Rot2.cpp b/geometry/Rot2.cpp
index f18572a1a..185b14909 100644
--- a/geometry/Rot2.cpp
+++ b/geometry/Rot2.cpp
@@ -12,98 +12,82 @@ using namespace std;
 
 namespace gtsam {
 
-  /** Explicit instantiation of base class to export members */
-  INSTANTIATE_LIE(Rot2);
+/** Explicit instantiation of base class to export members */
+INSTANTIATE_LIE(Rot2);
 
-	/* ************************************************************************* */
-	Rot2 Rot2::fromCosSin(double c, double s) {
-		if (fabs(c * c + s * s - 1.0) > 1e-9) {
-    	double norm_cs = sqrt(c*c + s*s);
-    	c = c/norm_cs;
-    	s = s/norm_cs;
-		}
-		return Rot2(c, s);
+/* ************************************************************************* */
+Rot2 Rot2::fromCosSin(double c, double s) {
+	if (fabs(c * c + s * s - 1.0) > 1e-9) {
+		double norm_cs = sqrt(c*c + s*s);
+		c = c/norm_cs;
+		s = s/norm_cs;
 	}
+	return Rot2(c, s);
+}
 
-	/* ************************************************************************* */
-	Rot2 Rot2::atan2(double y, double x) {
-		Rot2 R(x, y);
-		return R.normalize();
+/* ************************************************************************* */
+Rot2 Rot2::atan2(double y, double x) {
+	Rot2 R(x, y);
+	return R.normalize();
+}
+
+/* ************************************************************************* */
+void Rot2::print(const string& s) const {
+	cout << s << ":" << theta() << endl;
+}
+
+/* ************************************************************************* */
+bool Rot2::equals(const Rot2& R, double tol) const {
+	return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
+}
+
+/* ************************************************************************* */
+Rot2& Rot2::normalize() {
+	double scale = c_*c_ + s_*s_;
+	if(fabs(scale-1.0)>1e-9) {
+		scale = pow(scale, -0.5);
+		c_ *= scale;
+		s_ *= scale;
 	}
+	return *this;
+}
 
-	/* ************************************************************************* */
-	void Rot2::print(const string& s) const {
-		cout << s << ":" << theta() << endl;
-	}
+/* ************************************************************************* */
+Matrix Rot2::matrix() const {
+	return Matrix_(2, 2, c_, -s_, s_, c_);
+}
 
-	/* ************************************************************************* */
-	bool Rot2::equals(const Rot2& R, double tol) const {
-		return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
-	}
+/* ************************************************************************* */
+Matrix Rot2::transpose() const {
+	return Matrix_(2, 2, c_, s_, -s_, c_);
+}
 
-	/* ************************************************************************* */
-  Rot2& Rot2::normalize() {
-  	double scale = c_*c_ + s_*s_;
-  	if(fabs(scale-1.0)>1e-9) {
-  		scale = pow(scale, -0.5);
-  		c_ *= scale;
-  		s_ *= scale;
-  	}
-  	return *this;
-  }
+/* ************************************************************************* */
+// see doc/math.lyx, SO(2) section
+Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
+		boost::optional<Matrix&> H2) const {
+	const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
+	if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
+	if (H2) *H2 = matrix();
+	return q;
+}
 
-	/* ************************************************************************* */
-	Matrix Rot2::matrix() const {
-		return Matrix_(2, 2, c_, -s_, s_, c_);
-	}
+/* ************************************************************************* */
+// see doc/math.lyx, SO(2) section
+Point2 Rot2::unrotate(const Point2& p,
+		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+	const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
+	if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x());  // R_{pi/2}q
+	if (H2) *H2 = transpose();
+	return q;
+}
 
-	/* ************************************************************************* */
-	Matrix Rot2::transpose() const {
-		return Matrix_(2, 2, c_, s_, -s_, c_);
-	}
-
-	/* ************************************************************************* */
-	Point2 Rot2::rotate(const Point2& p) const {
-		return Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
-	}
-
-	/* ************************************************************************* */
-	Point2 Rot2::unrotate(const Point2& p) const {
-		return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
-	}
-
-	/* ************************************************************************* */
-  // see doc/math.lyx, SO(2) section
-	Point2 Rot2::rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1,
-			boost::optional<Matrix&> H2) {
-		Point2 q = R * p;
-		if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
-		if (H2) *H2 = R.matrix();
-		return q;
-	}
-
-	/* ************************************************************************* */
-  // see doc/math.lyx, SO(2) section
-	Point2 Rot2::unrotate(const Rot2 & R, const Point2& p,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-		Point2 q = R.unrotate(p);
-		if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x());  // R_{pi/2}q
-		if (H2) *H2 = R.transpose();
-		return q;
-	}
-
-	/* ************************************************************************* */
-	Rot2 Rot2::relativeBearing(const Point2& d) {
-		double n = d.norm();
-		return Rot2::fromCosSin(d.x() / n, d.y() / n);
-	}
-
-	/* ************************************************************************* */
-	Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
-		double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
-		if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
-		return Rot2::fromCosSin(x / n, y / n);
-	}
+/* ************************************************************************* */
+Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
+	double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
+	if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
+	return Rot2::fromCosSin(x / n, y / n);
+}
 
 /* ************************************************************************* */
 
diff --git a/geometry/Rot2.h b/geometry/Rot2.h
index 5c5b845e2..a16b93c59 100644
--- a/geometry/Rot2.h
+++ b/geometry/Rot2.h
@@ -57,23 +57,16 @@ namespace gtsam {
     /** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
     static Rot2 fromCosSin(double c, double s);
 
-    /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
-
 	/**
-	 * Named constructor
+	 * Named constructor with derivative
 	 * Calculate relative bearing to a landmark in local coordinate frame
 	 * @param point 2D location of landmark
 	 * @param H optional reference for Jacobian
 	 * @return 2D rotation \in SO(2)
 	 */
-	static Rot2 relativeBearing(const Point2& d);
-
-	/**
-	 * Named constructor with derivative
-	 * Calculate relative bearing and optional derivative
-	 */
-	static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H);
+	static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H=boost::none);
 
+	/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
   	static Rot2 atan2(double y, double x);
 
   	/** return angle (RADIANS) */
@@ -133,25 +126,22 @@ namespace gtsam {
 			return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
 		}
 
-    /** rotate from world to rotated = R*p */
-    Point2 rotate(const Point2& p) const;
-
     /**
      * rotate point from rotated coordinate frame to
      * world = R*p
      */
-  	static Point2 rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
-  			boost::none, boost::optional<Matrix&> H2 = boost::none);
+  	Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 =
+  			boost::none, boost::optional<Matrix&> H2 = boost::none) const;
 
-    /** rotate from world to rotated = R'*p */
-    Point2 unrotate(const Point2& p) const;
+    /** syntactic sugar for rotate */
+    inline Point2 operator*(const Point2& p) const {return rotate(p);}
 
     /**
      * rotate point from world to rotated
      * frame = R'*p
      */
-    static Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
-  			boost::none, boost::optional<Matrix&> H2 = boost::none);
+    Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 =
+  			boost::none, boost::optional<Matrix&> H2 = boost::none) const;
 
   private:
     /** Serialization function */
@@ -164,9 +154,6 @@ namespace gtsam {
 
   }; // Rot2
 
-  /** syntactic sugar for rotate */
-  inline Point2 operator*(const Rot2& R, const Point2& p) {return R.rotate(p);}
-
 } // gtsam
 
 #endif /* ROT2_H_ */
diff --git a/geometry/Rot3.cpp b/geometry/Rot3.cpp
index ca6d1c1c6..fc7847444 100644
--- a/geometry/Rot3.cpp
+++ b/geometry/Rot3.cpp
@@ -166,24 +166,24 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Point3 Rot3::rotate(const Rot3& R, const Point3& p,
-  		  boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) {
-	  if (H1) *H1 = R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
-	  if (H2) *H2 = R.matrix();
-	  return rotate(R,p);
+  Point3 Rot3::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();
   }
 
-  /* ************************************************************************* */
-  Point3 Rot3::unrotate(const Rot3& R, const Point3& p) {
-    const Matrix Rt(R.transpose());
-    return Rt*p.vector(); // q = Rt*p
-  }
+//  /* ************************************************************************* */
+//  Point3 Rot3::unrotate(const Rot3& R, const Point3& p) {
+//    const Matrix Rt(R.transpose());
+//    return Rt*p.vector(); // q = Rt*p
+//  }
 
   /* ************************************************************************* */
   // see doc/math.lyx, SO(3) section
-  Point3 Rot3::unrotate(const Rot3& R, const Point3& p,
-  		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
-    const Matrix Rt(R.transpose());
+  Point3 Rot3::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;
diff --git a/geometry/Rot3.h b/geometry/Rot3.h
index 68a15161e..aca4f05b8 100644
--- a/geometry/Rot3.h
+++ b/geometry/Rot3.h
@@ -173,25 +173,25 @@ namespace gtsam {
      * rotate point from rotated coordinate frame to
      * world = R*p
      */
-    Point3 rotate(const Point3& p) const
-			{return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
+//    Point3 rotate(const Point3& p) const
+//			{return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
     inline Point3 operator*(const Point3& p) const { return rotate(p);}
 
     /**
      * rotate point from rotated coordinate frame to
      * world = R*p
      */
-    static inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
-    static Point3 rotate(const Rot3& R, const Point3& p,
-  	boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2);
+//    static inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
+    Point3 rotate(const Point3& p,
+  	boost::optional<Matrix&> H1=boost::none,  boost::optional<Matrix&> H2=boost::none) const;
 
     /**
      * rotate point from world to rotated
      * frame = R'*p
      */
-    static Point3 unrotate(const Rot3& R, const Point3& p);
-    static Point3 unrotate(const Rot3& R, const Point3& p,
-    	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+//    static Point3 unrotate(const Rot3& R, const Point3& p);
+    Point3 unrotate(const Point3& p,
+    	boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
 
   private:
     /** Serialization function */
diff --git a/geometry/SimpleCamera.cpp b/geometry/SimpleCamera.cpp
index 97628fff5..762ebfd15 100644
--- a/geometry/SimpleCamera.cpp
+++ b/geometry/SimpleCamera.cpp
@@ -27,73 +27,40 @@ namespace gtsam {
 	}
 
 	pair<Point2, bool> SimpleCamera::projectSafe(const Point3& P) const {
-		Point3 cameraPoint = Pose3::transform_to(calibrated_.pose(), P);
-		Point2 intrinsic = project_to_camera(cameraPoint);
-		Point2 projection = uncalibrate(K_, intrinsic);
+		Point3 cameraPoint = calibrated_.pose().transform_to(P);
+		Point2 intrinsic = CalibratedCamera::project_to_camera(cameraPoint);
+		Point2 projection = K_.uncalibrate(intrinsic);
 		return pair<Point2, bool>(projection, cameraPoint.z() > 0);
 	}
 
-	Point2 SimpleCamera::project(const Point3 & P) const {
-		pair<Point2, bool> projected = projectSafe(P);
-		return projected.first;
+	Point2 SimpleCamera::project(const Point3& point,
+		    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+
+		Point2 intrinsic = calibrated_.project(point, H1, H2);
+		if (!H1 && !H2)
+			return K_.uncalibrate(intrinsic);
+
+		Matrix D_projection_intrinsic;
+		Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
+
+		if (H1) {
+			*H1 = D_projection_intrinsic * (*H1);
+		}
+		if (H2) {
+			*H2 = D_projection_intrinsic * (*H2);
+		}
+		return projection;
 	}
 
 	Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
 		Point2 intrinsic = K_.calibrate(projection);
-		Point3 cameraPoint = backproject_from_camera(intrinsic, scale);
-		return Pose3::transform_from(calibrated_.pose(), cameraPoint);
+		Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
+		return calibrated_.pose().transform_from(cameraPoint);
 	}
 
 	SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {
 		return SimpleCamera(K, CalibratedCamera::level(pose2, height));
 	}
 
-	/* ************************************************************************* */
-	// measurement functions and derivatives
-	/* ************************************************************************* */
-
-	pair<Point2, bool> projectSafe(const SimpleCamera& camera, const Point3& point) {
-		return camera.projectSafe(point);
-	}
-
-	Point2 project(const SimpleCamera& camera, const Point3& point) {
-		return camera.project(point);
-	}
-
-	/* ************************************************************************* */
-	Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point) {
-		Point2 intrinsic = project(camera.calibrated_, point);
-		Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
-		Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
-		Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
-		return D_projection_pose;
-	}
-
-	/* ************************************************************************* */
-	Matrix Dproject_point(const SimpleCamera& camera, const Point3& point) {
-		Point2 intrinsic = project(camera.calibrated_, point);
-		Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
-		Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
-		Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
-		return D_projection_point;
-	}
-
-	/* ************************************************************************* */
-	Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
-		Matrix& D_projection_pose, Matrix& D_projection_point) {
-
-		Point2 intrinsic = project(camera.calibrated_, point);
-		Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
-		Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
-
-		Point2 projection = uncalibrate(camera.K_, intrinsic);
-		Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
-
-		D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
-		D_projection_point = D_projection_intrinsic * D_intrinsic_point;
-
-		return projection;
-	}
-
 /* ************************************************************************* */
 }
diff --git a/geometry/SimpleCamera.h b/geometry/SimpleCamera.h
index 59dace4f5..2c431efbb 100644
--- a/geometry/SimpleCamera.h
+++ b/geometry/SimpleCamera.h
@@ -37,17 +37,11 @@ namespace gtsam {
 			return K_;
 		}
 
-
 		/**
 		 * project a 3d point to the camera and also check the depth
 		 */
 		std::pair<Point2,bool> projectSafe(const Point3& P) const;
 
-		/**
-		 * project a 3d point to the camera
-		 */
-		Point2 project(const Point3& P) const;
-
 		/**
 		 * backproject a 2d point from the camera up to a given scale
 		 */
@@ -60,44 +54,16 @@ namespace gtsam {
 		 */
 		static SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
 
-		// Friends
-		friend Matrix Dproject_pose(const SimpleCamera& camera,  const Point3& point);
-		friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
-		friend Point2	Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
-					Matrix& D_projection_pose, Matrix& D_projection_point);
+		/**
+		 * This function receives the camera pose and the landmark location and
+		 * returns the location the point is supposed to appear in the image
+		 */
+		Point2 project(const Point3& point,
+			    boost::optional<Matrix&> H1 = boost::none,
+			    boost::optional<Matrix&> H2 = boost::none) const;
 
 	};
 
-	/* ************************************************************************* */
-	// measurement functions and derivatives
-	/* ************************************************************************* */
-
-	/**
-	 * This function receives the camera pose and the landmark location and
-	 * returns the location the point is supposed to appear in the image
-	 * as well as the sign of the depth.
-	 */
-	std::pair<Point2, bool> projectSafe(const SimpleCamera& camera, const Point3& point);
-
-	/**
-	 * This function receives the camera pose and the landmark location and
-	 * returns the location the point is supposed to appear in the image
-	 */
-	Point2 project(const SimpleCamera& camera, const Point3& point);
-
-	/**
-	 * Derivatives of project.
-	 */
-	Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
-	Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
-
-	/**
-	 * super-duper combined evaluation + derivatives
-	 * saves a lot of time because a lot of computation is shared
-	 */
-	Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
-					Matrix& D_projection_pose, Matrix& D_projection_point);
-
 }
 
 #endif /* SIMPLECAMERA_H_ */
diff --git a/geometry/StereoCamera.cpp b/geometry/StereoCamera.cpp
index a2e3de5e8..dc6bc91dc 100644
--- a/geometry/StereoCamera.cpp
+++ b/geometry/StereoCamera.cpp
@@ -21,10 +21,69 @@ StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double ba
 	cy_ = calibration(4);
 }
 
-/* ************************************************************************* */
-StereoPoint2 StereoCamera::project(const Point3& point) const {
+///* ************************************************************************* */
+//StereoPoint2 StereoCamera::project(const Point3& point) const {
+//
+//	Point3 cameraPoint = leftCamPose_.transform_to(point);
+//
+//	double d = 1.0 / cameraPoint.z();
+//	double uL = cx_ + d * fx_ *  cameraPoint.x();
+//	double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_);
+//	double v  = cy_ + d * fy_ *  cameraPoint.y();
+//	return StereoPoint2(uL,uR,v);
+//}
+
+/* ************************************************************************* */
+StereoPoint2 StereoCamera::project(const Point3& point,
+		boost::optional<Matrix&> Dproject_stereo_pose,
+		boost::optional<Matrix&> Dproject_stereo_point) const {
+
+	Point3 cameraPoint = leftCamPose_.transform_to(point);
+
+	if (Dproject_stereo_pose) {
+		//Point2 intrinsic = project(camera.calibrated_, point);  // unused
+
+		//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
+		//**** above function call inlined
+		  Matrix D_cameraPoint_pose;
+		  Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none);
+		  //cout << "D_cameraPoint_pose" << endl;
+		  //print(D_cameraPoint_pose);
+
+		  //Point2 intrinsic = project_to_camera(cameraPoint);  // unused
+		  Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
+		  //cout << "myJacobian" << endl;
+		  //print(D_intrinsic_cameraPoint);
+
+		  Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
+
+		//****
+		//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+		Matrix D_projection_intrinsic = Duncalibrate2(K());  // 3x3
+
+		*Dproject_stereo_pose =  D_projection_intrinsic * D_intrinsic_pose;
+	}
+
+	if (Dproject_stereo_point) {
+		//Point2 intrinsic = project(camera.calibrated_, point);   // unused
+
+		//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
+		//**** above function call inlined
+		  Matrix D_cameraPoint_point;
+		  Point3 cameraPoint = pose().transform_to(point, boost::none, D_cameraPoint_point);
+
+			//Point2 intrinsic = project_to_camera(cameraPoint);  // unused
+			Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
+
+			Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
+
+		//****
+		//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+		Matrix D_projection_intrinsic = Duncalibrate2(K());  // 3x3
+
+		*Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point;
+	}
 
-	Point3 cameraPoint = Pose3::transform_to(leftCamPose_, point);
 
 	double d = 1.0 / cameraPoint.z();
 	double uL = cx_ + d * fx_ *  cameraPoint.x();
@@ -34,14 +93,14 @@ StereoPoint2 StereoCamera::project(const Point3& point) const {
 }
 
 /* ************************************************************************* */
-Matrix Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P) {
+Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
 	double d = 1.0 / P.z(), d2 = d * d;
 	//return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
-	return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x()-camera.baseline()) * d2, 0.0, d, -P.y() * d2);
+	return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x()-baseline()) * d2, 0.0, d, -P.y() * d2);
 }
 
 /* ************************************************************************* */
-Matrix Duncalibrate2(const Cal3_S2& K) {
+Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
 	Vector calibration = K.vector();  // I want fx, fx, fy
 	Vector calibration2(3);
 	calibration2(0) = calibration(0);
@@ -51,53 +110,53 @@ Matrix Duncalibrate2(const Cal3_S2& K) {
 	//return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
 }
 
-/* ************************************************************************* */
-Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
-	//Point2 intrinsic = project(camera.calibrated_, point);  // unused
+///* ************************************************************************* */
+//Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
+//	//Point2 intrinsic = project(camera.calibrated_, point);  // unused
+//
+//	//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
+//	//**** above function call inlined
+//	  Matrix D_cameraPoint_pose;
+//	  Point3 cameraPoint = camera.pose().transform_to(point, D_cameraPoint_pose, boost::none);
+//	  //cout << "D_cameraPoint_pose" << endl;
+//	  //print(D_cameraPoint_pose);
+//
+//	  //Point2 intrinsic = project_to_camera(cameraPoint);  // unused
+//	  Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
+//	  //cout << "myJacobian" << endl;
+//	  //print(D_intrinsic_cameraPoint);
+//
+//	  Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
+//
+//	//****
+//	//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+//	Matrix D_projection_intrinsic = Duncalibrate2(camera.K());  // 3x3
+//
+//	Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
+//	return D_projection_pose;
+//}
 
-	//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
-	//**** above function call inlined
-	  Matrix D_cameraPoint_pose;
-	  Point3 cameraPoint = Pose3::transform_to(camera.pose(), point, D_cameraPoint_pose, boost::none);
-	  //cout << "D_cameraPoint_pose" << endl;
-	  //print(D_cameraPoint_pose);
-
-	  //Point2 intrinsic = project_to_camera(cameraPoint);  // unused
-	  Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
-	  //cout << "myJacobian" << endl;
-	  //print(D_intrinsic_cameraPoint);
-
-	  Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
-
-	//****
-	//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
-	Matrix D_projection_intrinsic = Duncalibrate2(camera.K());  // 3x3
-
-	Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
-	return D_projection_pose;
-}
-
-/* ************************************************************************* */
-Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
-	//Point2 intrinsic = project(camera.calibrated_, point);   // unused
-
-	//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
-	//**** above function call inlined
-	  Matrix D_cameraPoint_point;
-	  Point3 cameraPoint = Pose3::transform_to(camera.pose(), point, boost::none, D_cameraPoint_point);
-
-		//Point2 intrinsic = project_to_camera(cameraPoint);  // unused
-		Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
-
-		Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
-
-	//****
-	//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
-	Matrix D_projection_intrinsic = Duncalibrate2(camera.K());  // 3x3
-
-	Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
-	return D_projection_point;
-}
+///* ************************************************************************* */
+//Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
+//	//Point2 intrinsic = project(camera.calibrated_, point);   // unused
+//
+//	//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
+//	//**** above function call inlined
+//	  Matrix D_cameraPoint_point;
+//	  Point3 cameraPoint = camera.pose().transform_to(point, boost::none, D_cameraPoint_point);
+//
+//		//Point2 intrinsic = project_to_camera(cameraPoint);  // unused
+//		Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
+//
+//		Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
+//
+//	//****
+//	//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
+//	Matrix D_projection_intrinsic = Duncalibrate2(camera.K());  // 3x3
+//
+//	Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
+//	return D_projection_point;
+//}
 
 
 // calibrated cameras
diff --git a/geometry/StereoCamera.h b/geometry/StereoCamera.h
index b7f5c5874..6bd60402a 100644
--- a/geometry/StereoCamera.h
+++ b/geometry/StereoCamera.h
@@ -43,7 +43,17 @@ namespace gtsam {
 			return baseline_;
 		}
 
-		StereoPoint2 project(const Point3& point) const;
+		StereoPoint2 project(const Point3& point,
+				boost::optional<Matrix&> Dproject_stereo_pose = boost::none,
+				boost::optional<Matrix&> Dproject_stereo_point = boost::none) const;
+
+//		Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point);
+//		Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point);
+
+	private:
+		/** utility functions */
+		Matrix Dproject_to_stereo_camera1(const Point3& P) const;
+		static Matrix Duncalibrate2(const Cal3_S2& K);
 
 	};
 
@@ -57,15 +67,5 @@ namespace gtsam {
     return StereoCamera(expmap(p0.pose(),d),p0.K(),p0.baseline());
   }
 
-	inline StereoPoint2 project(const StereoCamera& camera, const Point3& point) {
-		return camera.project(point);
-	}
-
-	Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point);
-	Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point);
-
-	Matrix
-			Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P);
-	Matrix Duncalibrate2(const Cal3_S2& K);
 
 }
diff --git a/geometry/tests/testCal3_S2.cpp b/geometry/tests/testCal3_S2.cpp
index d67d389fe..34385d657 100644
--- a/geometry/tests/testCal3_S2.cpp
+++ b/geometry/tests/testCal3_S2.cpp
@@ -36,18 +36,19 @@ TEST( Cal3_S2, calibrate)
 }
 
 /* ************************************************************************* */
+Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
 TEST( Cal3_S2, Duncalibrate1)
 {
-	Matrix computed = Duncalibrate1(K, p);
-	Matrix numerical = numericalDerivative21(uncalibrate, K, p);
+	Matrix computed; K.uncalibrate(p, computed, boost::none);
+	Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
 	CHECK(assert_equal(numerical,computed,1e-8));
 }
 
 /* ************************************************************************* */
 TEST( Cal3_S2, Duncalibrate2)
 {
-	Matrix computed = Duncalibrate2(K, p);
-	Matrix numerical = numericalDerivative22(uncalibrate, K, p);
+	Matrix computed; K.uncalibrate(p, boost::none, computed);
+	Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
 	CHECK(assert_equal(numerical,computed,1e-9));
 }
 
diff --git a/geometry/tests/testCalibratedCamera.cpp b/geometry/tests/testCalibratedCamera.cpp
index e7148d565..4d8578191 100644
--- a/geometry/tests/testCalibratedCamera.cpp
+++ b/geometry/tests/testCalibratedCamera.cpp
@@ -71,40 +71,27 @@ TEST( CalibratedCamera, project)
 }
 
 /* ************************************************************************* */
-
 TEST( CalibratedCamera, Dproject_to_camera1) {
 	Point3 pp(155,233,131);
-	Matrix test1 = Dproject_to_camera1(pp);
-	Matrix test2 = numericalDerivative11(project_to_camera, pp);
+	Matrix test1;
+	CalibratedCamera::project_to_camera(pp, test1);
+	Matrix test2 = numericalDerivative11<Point2,Point3>(
+			boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp);
 	CHECK(assert_equal(test1, test2));
 }
 
 /* ************************************************************************* */
 Point2 project2(const Pose3& pose, const Point3& point) {
-	return project(CalibratedCamera(pose), point);
-}
-
-TEST( CalibratedCamera, Dproject_pose)
-{
-	Matrix computed = Dproject_pose(camera, point1);
-	Matrix numerical = numericalDerivative21(project2, pose1, point1);
-	CHECK(assert_equal(computed, numerical,1e-7));
-}
-
-TEST( CalibratedCamera, Dproject_point)
-{
-	Matrix computed = Dproject_point(camera, point1);
-	Matrix numerical = numericalDerivative22(project2, pose1, point1);
-	CHECK(assert_equal(computed, numerical,1e-7));
+	return CalibratedCamera(pose).project(point);
 }
 
 TEST( CalibratedCamera, Dproject_point_pose)
 {
 	Matrix Dpose, Dpoint;
-	Point2 result = Dproject_pose_point(camera, point1, Dpose, Dpoint);
+	Point2 result = camera.project(point1, Dpose, Dpoint);
 	Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1);
 	Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
-  CHECK(assert_equal(result, Point2(-.16,  .16) ));
+	CHECK(assert_equal(result, Point2(-.16,  .16) ));
 	CHECK(assert_equal(Dpose,  numerical_pose, 1e-7));
 	CHECK(assert_equal(Dpoint, numerical_point,1e-7));
 }
diff --git a/geometry/tests/testPoint3.cpp b/geometry/tests/testPoint3.cpp
index 63e474420..ac2076958 100644
--- a/geometry/tests/testPoint3.cpp
+++ b/geometry/tests/testPoint3.cpp
@@ -33,8 +33,9 @@ TEST( Point3, equals)
 /* ************************************************************************* */
 TEST( Point3, dot)
 {
-	CHECK(Point3::dot(Point3(0,0,0),Point3(1,1,0)) == 0);
-	CHECK(Point3::dot(Point3(1,1,1),Point3(1,1,0)) == 2);
+	Point3 origin, ones(1,1,1);
+	CHECK(origin.dot(Point3(1,1,0)) == 0);
+	CHECK(ones.dot(Point3(1,1,0)) == 2);
 }
 
 /* ************************************************************************* */
diff --git a/geometry/tests/testPose2.cpp b/geometry/tests/testPose2.cpp
index bdbe21a70..51bf2f60d 100644
--- a/geometry/tests/testPose2.cpp
+++ b/geometry/tests/testPose2.cpp
@@ -117,6 +117,10 @@ TEST(Pose2, logmap) {
 }
 
 /* ************************************************************************* */
+Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
+	return pose.transform_to(point);
+}
+
 TEST( Pose2, transform_to )
 {
   Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
@@ -129,25 +133,29 @@ TEST( Pose2, transform_to )
 
   // actual
   Matrix actualH1, actualH2;
-  Point2 actual = Pose2::transform_to(pose,point, actualH1, actualH2);
+  Point2 actual = pose.transform_to(point, actualH1, actualH2);
   CHECK(assert_equal(expected,actual));
 
   CHECK(assert_equal(expectedH1,actualH1));
-  Matrix numericalH1 = numericalDerivative21(Pose2::transform_to, pose, point, 1e-5);
+  Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point, 1e-5);
   CHECK(assert_equal(numericalH1,actualH1));
 
   CHECK(assert_equal(expectedH2,actualH2));
-  Matrix numericalH2 = numericalDerivative22(Pose2::transform_to, pose, point, 1e-5);
+  Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point, 1e-5);
   CHECK(assert_equal(numericalH2,actualH2));
 }
 
 /* ************************************************************************* */
+Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
+	return pose.transform_from(point);
+}
+
 TEST (Pose2, transform_from)
 {
 	Pose2 pose(1., 0., M_PI_2);
 	Point2 pt(2., 1.);
 	Matrix H1, H2;
-	Point2 actual = Pose2::transform_from(pose, pt, H1, H2);
+	Point2 actual = pose.transform_from(pt, H1, H2);
 
 	Point2 expected(0., 2.);
 	CHECK(assert_equal(expected, actual));
@@ -155,11 +163,11 @@ TEST (Pose2, transform_from)
 	Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
 	Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
 
-	Matrix numericalH1 = numericalDerivative21(Pose2::transform_from, pose, pt, 1e-5);
+	Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5);
 	CHECK(assert_equal(H1_expected, H1));
 	CHECK(assert_equal(H1_expected, numericalH1));
 
-	Matrix numericalH2 = numericalDerivative22(Pose2::transform_from, pose, pt, 1e-5);
+	Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5);
 	CHECK(assert_equal(H2_expected, H2));
 	CHECK(assert_equal(H2_expected, numericalH2));
 }
@@ -193,8 +201,8 @@ TEST(Pose2, compose_a)
 
   Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
   Point2 expected_point(-1.0, -1.0);
-  Point2 actual_point1 = Pose2::transform_to(pose1 * pose2, point);
-  Point2 actual_point2 = Pose2::transform_to(pose2, Pose2::transform_to(pose1, point));
+  Point2 actual_point1 = (pose1 * pose2).transform_to(point);
+  Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point));
   CHECK(assert_equal(expected_point, actual_point1));
   CHECK(assert_equal(expected_point, actual_point2));
 }
@@ -345,7 +353,7 @@ TEST( Pose2, between )
   CHECK(assert_equal(expectedH1,actualH1));
   CHECK(assert_equal(numericalH1,actualH1));
 	// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
-  CHECK(assert_equal(-AdjointMap(between(gT2,gT1)),actualH1));
+  CHECK(assert_equal(-between(gT2,gT1).AdjointMap(),actualH1));
 
   Matrix expectedH2 = Matrix_(3,3,
        1.0, 0.0, 0.0,
@@ -395,65 +403,72 @@ Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
 Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
 
 /* ************************************************************************* */
+Rot2 bearing_proxy(const Pose2& pose, const Point2& pt) {
+	return pose.bearing(pt);
+}
+
 TEST( Pose2, bearing )
 {
 	Matrix expectedH1, actualH1, expectedH2, actualH2;
 
 	// establish bearing is indeed zero
-	CHECK(assert_equal(Rot2(),bearing(x1,l1)));
+	CHECK(assert_equal(Rot2(),x1.bearing(l1)));
 
 	// establish bearing is indeed 45 degrees
-	CHECK(assert_equal(Rot2::fromAngle(M_PI_4),bearing(x1,l2)));
+	CHECK(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(l2)));
 
 	// establish bearing is indeed 45 degrees even if shifted
-	Rot2 actual23 = bearing(x2, l3, actualH1, actualH2);
+	Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
 	CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
 
 	// Check numerical derivatives
-	expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5);
+	expectedH1 = numericalDerivative21(bearing_proxy, x2, l3, 1e-5);
 	CHECK(assert_equal(expectedH1,actualH1));
-	expectedH2 = numericalDerivative22(bearing, x2, l3, 1e-5);
+	expectedH2 = numericalDerivative22(bearing_proxy, x2, l3, 1e-5);
 	CHECK(assert_equal(expectedH1,actualH1));
 
 	// establish bearing is indeed 45 degrees even if rotated
-	Rot2 actual34 = bearing(x3, l4, actualH1, actualH2);
+	Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
 	CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
 
 	// Check numerical derivatives
-	expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5);
-	expectedH2 = numericalDerivative22(bearing, x3, l4, 1e-5);
+	expectedH1 = numericalDerivative21(bearing_proxy, x3, l4, 1e-5);
+	expectedH2 = numericalDerivative22(bearing_proxy, x3, l4, 1e-5);
 	CHECK(assert_equal(expectedH1,actualH1));
 	CHECK(assert_equal(expectedH1,actualH1));
 }
 
 /* ************************************************************************* */
+double range_proxy(const Pose2& pose, const Point2& point) {
+	return pose.range(point);
+}
 TEST( Pose2, range )
 {
 	Matrix expectedH1, actualH1, expectedH2, actualH2;
 
 	// establish range is indeed zero
-	DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9);
+	DOUBLES_EQUAL(1,x1.range(l1),1e-9);
 
 	// establish range is indeed 45 degrees
-	DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9);
+	DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9);
 
 	// Another pair
-	double actual23 = gtsam::range(x2, l3, actualH1, actualH2);
+	double actual23 = x2.range(l3, actualH1, actualH2);
 	DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
 
 	// Check numerical derivatives
-	expectedH1 = numericalDerivative21(range, x2, l3, 1e-5);
+	expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5);
 	CHECK(assert_equal(expectedH1,actualH1));
-	expectedH2 = numericalDerivative22(range, x2, l3, 1e-5);
+	expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5);
 	CHECK(assert_equal(expectedH1,actualH1));
 
 	// Another test
-	double actual34 = gtsam::range(x3, l4, actualH1, actualH2);
+	double actual34 = x3.range(l4, actualH1, actualH2);
 	DOUBLES_EQUAL(2,actual34,1e-9);
 
 	// Check numerical derivatives
-	expectedH1 = numericalDerivative21(range, x3, l4, 1e-5);
-	expectedH2 = numericalDerivative22(range, x3, l4, 1e-5);
+	expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5);
+	expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5);
 	CHECK(assert_equal(expectedH1,actualH1));
 	CHECK(assert_equal(expectedH1,actualH1));
 }
diff --git a/geometry/tests/testPose3.cpp b/geometry/tests/testPose3.cpp
index 5e6c600d2..d26b00b4e 100644
--- a/geometry/tests/testPose3.cpp
+++ b/geometry/tests/testPose3.cpp
@@ -216,11 +216,12 @@ TEST( Pose3, compose_inverse)
 }
 
 /* ************************************************************************* */
+Point3 transform_from_(const Pose3& pose, const Point3& point) { return pose.transform_from(point); }
 TEST( Pose3, Dtransform_from1_a)
 {
 	Matrix actualDtransform_from1;
-	Pose3::transform_from(T, P, actualDtransform_from1, boost::none);
-	Matrix numerical = numericalDerivative21(Pose3::transform_from,T,P);
+	T.transform_from(P, actualDtransform_from1, boost::none);
+	Matrix numerical = numericalDerivative21(transform_from_,T,P);
 	CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
 }
 
@@ -228,8 +229,8 @@ TEST( Pose3, Dtransform_from1_b)
 {
 	Pose3 origin;
 	Matrix actualDtransform_from1;
-	Pose3::transform_from(origin, P, actualDtransform_from1, boost::none);
-	Matrix numerical = numericalDerivative21(Pose3::transform_from,origin,P);
+	origin.transform_from(P, actualDtransform_from1, boost::none);
+	Matrix numerical = numericalDerivative21(transform_from_,origin,P);
 	CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
 }
 
@@ -238,8 +239,8 @@ TEST( Pose3, Dtransform_from1_c)
 	Point3 origin;
 	Pose3 T0(R,origin);
 	Matrix actualDtransform_from1;
-	Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
-	Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
+	T0.transform_from(P, actualDtransform_from1, boost::none);
+	Matrix numerical = numericalDerivative21(transform_from_,T0,P);
 	CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
 }
 
@@ -249,9 +250,9 @@ TEST( Pose3, Dtransform_from1_d)
 	Point3 t0(100,0,0);
 	Pose3 T0(I,t0);
 	Matrix actualDtransform_from1;
-	Pose3::transform_from(T0, P, actualDtransform_from1, boost::none);
+	T0.transform_from(P, actualDtransform_from1, boost::none);
 	//print(computed, "Dtransform_from1_d computed:");
-	Matrix numerical = numericalDerivative21(Pose3::transform_from,T0,P);
+	Matrix numerical = numericalDerivative21(transform_from_,T0,P);
 	//print(numerical, "Dtransform_from1_d numerical:");
 	CHECK(assert_equal(numerical,actualDtransform_from1,1e-8));
 }
@@ -260,17 +261,18 @@ TEST( Pose3, Dtransform_from1_d)
 TEST( Pose3, Dtransform_from2)
 {
 	Matrix actualDtransform_from2;
-	Pose3::transform_from(T,P, boost::none, actualDtransform_from2);
-	Matrix numerical = numericalDerivative22(Pose3::transform_from,T,P);
+	T.transform_from(P, boost::none, actualDtransform_from2);
+	Matrix numerical = numericalDerivative22(transform_from_,T,P);
 	CHECK(assert_equal(numerical,actualDtransform_from2,1e-8));
 }
 
 /* ************************************************************************* */
+Point3 transform_to_(const Pose3& pose, const Point3& point) { return pose.transform_to(point); }
 TEST( Pose3, Dtransform_to1)
 {
 	Matrix computed;
-	Pose3::transform_to(T, P, computed, boost::none);
-	Matrix numerical = numericalDerivative21(Pose3::transform_to,T,P);
+	T.transform_to(P, computed, boost::none);
+	Matrix numerical = numericalDerivative21(transform_to_,T,P);
 	CHECK(assert_equal(numerical,computed,1e-8));
 }
 
@@ -278,8 +280,8 @@ TEST( Pose3, Dtransform_to1)
 TEST( Pose3, Dtransform_to2)
 {
 	Matrix computed;
-	Pose3::transform_to(T,P, boost::none, computed);
-	Matrix numerical = numericalDerivative22(Pose3::transform_to,T,P);
+	T.transform_to(P, boost::none, computed);
+	Matrix numerical = numericalDerivative22(transform_to_,T,P);
 	CHECK(assert_equal(numerical,computed,1e-8));
 }
 
@@ -287,9 +289,9 @@ TEST( Pose3, Dtransform_to2)
 TEST( Pose3, transform_to_with_derivatives)
 {
 	Matrix actH1, actH2;
-	Pose3::transform_to(T,P,actH1,actH2);
-	Matrix expH1 = numericalDerivative21(Pose3::transform_to, T,P),
-		   expH2 = numericalDerivative22(Pose3::transform_to, T,P);
+	T.transform_to(P,actH1,actH2);
+	Matrix expH1 = numericalDerivative21(transform_to_, T,P),
+		   expH2 = numericalDerivative22(transform_to_, T,P);
 	CHECK(assert_equal(expH1, actH1, 1e-8));
 	CHECK(assert_equal(expH2, actH2, 1e-8));
 }
@@ -298,9 +300,9 @@ TEST( Pose3, transform_to_with_derivatives)
 TEST( Pose3, transform_from_with_derivatives)
 {
 	Matrix actH1, actH2;
-	Pose3::transform_from(T,P,actH1,actH2);
-	Matrix expH1 = numericalDerivative21(Pose3::transform_from, T,P),
-		   expH2 = numericalDerivative22(Pose3::transform_from, T,P);
+	T.transform_from(P,actH1,actH2);
+	Matrix expH1 = numericalDerivative21(transform_from_, T,P),
+		   expH2 = numericalDerivative22(transform_from_, T,P);
 	CHECK(assert_equal(expH1, actH1, 1e-8));
 	CHECK(assert_equal(expH2, actH2, 1e-8));
 }
@@ -308,7 +310,7 @@ TEST( Pose3, transform_from_with_derivatives)
 /* ************************************************************************* */
 TEST( Pose3, transform_to_translate)
 {
-		Point3 actual = Pose3::transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.));
+		Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.));
 		Point3 expected(9.,18.,27.);
 		CHECK(assert_equal(expected, actual));
 }
@@ -317,7 +319,7 @@ TEST( Pose3, transform_to_translate)
 TEST( Pose3, transform_to_rotate)
 {
 		Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3());
-		Point3 actual = Pose3::transform_to(transform, Point3(2,1,10));
+		Point3 actual = transform.transform_to(Point3(2,1,10));
 		Point3 expected(-1,2,10);
 		CHECK(assert_equal(expected, actual, 0.001));
 }
@@ -326,7 +328,7 @@ TEST( Pose3, transform_to_rotate)
 TEST( Pose3, transform_to)
 {
 		Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0));
-		Point3 actual = Pose3::transform_to(transform, Point3(3,2,10));
+		Point3 actual = transform.transform_to(Point3(3,2,10));
 		Point3 expected(2,1,10);
 		CHECK(assert_equal(expected, actual, 0.001));
 }
@@ -334,7 +336,7 @@ TEST( Pose3, transform_to)
 /* ************************************************************************* */
 TEST( Pose3, transform_from)
 {
-		Point3 actual = Pose3::transform_from(T3, Point3());
+		Point3 actual = T3.transform_from(Point3());
 		Point3 expected = Point3(1.,2.,3.);
 		CHECK(assert_equal(expected, actual));
 }
@@ -342,7 +344,7 @@ TEST( Pose3, transform_from)
 /* ************************************************************************* */
 TEST( Pose3, transform_roundtrip)
 {
-		Point3 actual = Pose3::transform_from(T3, Pose3::transform_to(T3, Point3(12., -0.11,7.0)));
+		Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0)));
 		Point3 expected(12., -0.11,7.0);
 		CHECK(assert_equal(expected, actual));
 }
diff --git a/geometry/tests/testRot2.cpp b/geometry/tests/testRot2.cpp
index e4fa33112..b5a88d69c 100644
--- a/geometry/tests/testRot2.cpp
+++ b/geometry/tests/testRot2.cpp
@@ -66,7 +66,7 @@ inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);}
 TEST( Rot2, rotate)
 {
 	Matrix H1, H2;
-	Point2 actual = Rot2::rotate(R, P, H1, H2);
+	Point2 actual = R.rotate(P, H1, H2);
 	CHECK(assert_equal(actual,R*P));
 	Matrix numerical1 = numericalDerivative21(rotate_, R, P);
 	CHECK(assert_equal(numerical1,H1));
@@ -76,11 +76,11 @@ TEST( Rot2, rotate)
 
 /* ************************************************************************* */
 // unrotate and derivatives
-inline Point2 unrotate_(const Rot2 & R, const Point2& p) {return R.unrotate(p);}
+inline Point2 unrotate_(const Rot2& R, const Point2& p) {return R.unrotate(p);}
 TEST( Rot2, unrotate)
 {
 	Matrix H1, H2;
-	Point2 w = R * P, actual = Rot2::unrotate(R, w, H1, H2);
+	Point2 w = R * P, actual = R.unrotate(w, H1, H2);
 	CHECK(assert_equal(actual,P));
 	Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
 	CHECK(assert_equal(numerical1,H1));
@@ -89,6 +89,7 @@ TEST( Rot2, unrotate)
 }
 
 /* ************************************************************************* */
+inline Rot2 relativeBearing_(const Point2& pt) {return Rot2::relativeBearing(pt); }
 TEST( Rot2, relativeBearing )
 {
 	Point2 l1(1, 0), l2(1, 1);
@@ -99,7 +100,7 @@ TEST( Rot2, relativeBearing )
 	CHECK(assert_equal(Rot2(),actual1));
 
 	// Check numerical derivative
-	expectedH = numericalDerivative11(Rot2::relativeBearing, l1, 1e-5);
+	expectedH = numericalDerivative11(relativeBearing_, l1, 1e-5);
 	CHECK(assert_equal(expectedH,actualH));
 
 	// establish relativeBearing is indeed 45 degrees
@@ -107,7 +108,7 @@ TEST( Rot2, relativeBearing )
 	CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2));
 
 	// Check numerical derivative
-	expectedH = numericalDerivative11(Rot2::relativeBearing, l2, 1e-5);
+	expectedH = numericalDerivative11(relativeBearing_, l2, 1e-5);
 	CHECK(assert_equal(expectedH,actualH));
 }
 
diff --git a/geometry/tests/testRot3.cpp b/geometry/tests/testRot3.cpp
index 4127ec307..41730c2ae 100644
--- a/geometry/tests/testRot3.cpp
+++ b/geometry/tests/testRot3.cpp
@@ -207,7 +207,7 @@ public:
 };
 
 AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
-	return AngularVelocity::cross(X, Y);
+	return X.cross(Y);
 }
 
 /* ************************************************************************* */
@@ -224,31 +224,33 @@ TEST(Rot3, BCH)
 }
 
 /* ************************************************************************* */
+inline Point3 rotate_(const Rot3& r, const Point3& pt) { return r.rotate(pt); }
 TEST( Rot3, rotate_derivatives)
 {
 	Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
-	Rot3::rotate(R, P, actualDrotate1a, actualDrotate2);
-	Rot3::rotate(R.inverse(), P, actualDrotate1b, boost::none);
-	Matrix numerical1 = numericalDerivative21(Rot3::rotate, R, P);
-	Matrix numerical2 = numericalDerivative21(Rot3::rotate, R.inverse(), P);
-	Matrix numerical3 = numericalDerivative22(Rot3::rotate, R, P);
+	R.rotate(P, actualDrotate1a, actualDrotate2);
+	R.inverse().rotate(P, actualDrotate1b, boost::none);
+	Matrix numerical1 = numericalDerivative21(rotate_, R, P);
+	Matrix numerical2 = numericalDerivative21(rotate_, R.inverse(), P);
+	Matrix numerical3 = numericalDerivative22(rotate_, R, P);
 	EXPECT(assert_equal(numerical1,actualDrotate1a,error));
 	EXPECT(assert_equal(numerical2,actualDrotate1b,error));
 	EXPECT(assert_equal(numerical3,actualDrotate2, error));
 }
 
 /* ************************************************************************* */
+inline Point3 unrotate_(const Rot3& r, const Point3& pt) { return r.unrotate(pt); }
 TEST( Rot3, unrotate)
 {
 	Point3 w = R * P;
 	Matrix H1,H2;
-	Point3 actual = Rot3::unrotate(R,w,H1,H2);
+	Point3 actual = R.unrotate(w,H1,H2);
 	CHECK(assert_equal(P,actual));
 
-	Matrix numerical1 = numericalDerivative21(Rot3::unrotate, R, w);
+	Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
 	CHECK(assert_equal(numerical1,H1,error));
 
-	Matrix numerical2 = numericalDerivative22(Rot3::unrotate, R, w);
+	Matrix numerical2 = numericalDerivative22(unrotate_, R, w);
 	CHECK(assert_equal(numerical2,H2,error));
 }
 
diff --git a/geometry/tests/testSimpleCamera.cpp b/geometry/tests/testSimpleCamera.cpp
index 0992d5f3a..9c7aa0c93 100644
--- a/geometry/tests/testSimpleCamera.cpp
+++ b/geometry/tests/testSimpleCamera.cpp
@@ -76,7 +76,6 @@ TEST( SimpleCamera, backproject2)
 	Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
 	SimpleCamera camera(K, Pose3(rot, origin));
 
-
 	Point3 actual = camera.backproject(Point2(), 1.);
 	Point3 expected(0., 1., 0.);
 	pair<Point2, bool> x = camera.projectSafe(expected);
@@ -88,30 +87,16 @@ TEST( SimpleCamera, backproject2)
 
 /* ************************************************************************* */
 Point2 project2(const Pose3& pose, const Point3& point) {
-	return project(SimpleCamera(K,pose), point);
-}
-
-TEST( SimpleCamera, Dproject_pose)
-{
-	Matrix computed = Dproject_pose(camera, point1);
-	Matrix numerical = numericalDerivative21(project2, pose1, point1);
-	CHECK(assert_equal(computed, numerical,1e-7));
-}
-
-TEST( SimpleCamera, Dproject_point)
-{
-	Matrix computed = Dproject_point(camera, point1);
-	Matrix numerical = numericalDerivative22(project2, pose1, point1);
-	CHECK(assert_equal(computed, numerical,1e-7));
+	return SimpleCamera(K,pose).project(point);
 }
 
 TEST( SimpleCamera, Dproject_point_pose)
 {
 	Matrix Dpose, Dpoint;
-	Point2 result = Dproject_pose_point(camera, point1, Dpose, Dpoint);
+	Point2 result = camera.project(point1, Dpose, Dpoint);
 	Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1);
 	Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
-  CHECK(assert_equal(result, Point2(-100,  100) ));
+	CHECK(assert_equal(result, Point2(-100,  100) ));
 	CHECK(assert_equal(Dpose,  numerical_pose, 1e-7));
 	CHECK(assert_equal(Dpoint, numerical_point,1e-7));
 }
diff --git a/geometry/tests/testStereoCamera.cpp b/geometry/tests/testStereoCamera.cpp
index 3d7731f33..0a8b5f135 100644
--- a/geometry/tests/testStereoCamera.cpp
+++ b/geometry/tests/testStereoCamera.cpp
@@ -76,18 +76,19 @@ StereoCamera stereoCam(Pose3(), K, 0.5);
 Point3 p(0, 0, 5);
 
 /* ************************************************************************* */
+StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
 TEST( StereoCamera, Dproject_stereo_pose)
 {
-	Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project,stereoCam, p);
-	Matrix actual = Dproject_stereo_pose(stereoCam, p);
+	Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
+	Matrix actual; stereoCam.project(p, actual, boost::none);
 	CHECK(assert_equal(expected,actual,1e-7));
 }
 
 /* ************************************************************************* */
 TEST( StereoCamera, Dproject_stereo_point)
 {
-	Matrix expected = numericalDerivative22<StereoPoint2,StereoCamera,Point3>(project,stereoCam, p);
-	Matrix actual = Dproject_stereo_point(stereoCam, p);
+	Matrix expected = numericalDerivative22<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
+	Matrix actual; stereoCam.project(p, boost::none, actual);
 	CHECK(assert_equal(expected,actual,1e-8));
 }
 
diff --git a/geometry/timeCalibratedCamera.cpp b/geometry/timeCalibratedCamera.cpp
index ce016dc8f..b3e65871a 100644
--- a/geometry/timeCalibratedCamera.cpp
+++ b/geometry/timeCalibratedCamera.cpp
@@ -14,66 +14,71 @@ using namespace gtsam;
 
 int main()
 {
-  int n = 100000;
+	int n = 100000;
 
-  const Pose3 pose1(Matrix_(3,3,
-  				      1., 0., 0.,
-  				      0.,-1., 0.,
-  				      0., 0.,-1.
-  				      ),
-  			      Point3(0,0,0.5));
+	const Pose3 pose1(Matrix_(3,3,
+			1., 0., 0.,
+			0.,-1., 0.,
+			0., 0.,-1.
+	),
+	Point3(0,0,0.5));
 
-  const CalibratedCamera camera(pose1);
-  const Point3 point1(-0.08,-0.08, 0.0);
+	const CalibratedCamera camera(pose1);
+	const Point3 point1(-0.08,-0.08, 0.0);
 
-  // Aug 8, iMac 3.06GHz Core i3
-  //  378870 calls/second
-  //  2.63943 musecs/call
-  // AFTER collapse:
+	/**
+	 * NOTE: because we only have combined derivative functions now,
+	 * parts of this test are no longer useful.
+	 */
+
+	// Aug 8, iMac 3.06GHz Core i3
+	//  378870 calls/second
+	//  2.63943 musecs/call
+	// AFTER collapse:
 	//  1.57399e+06 calls/second
 	//  0.63533 musecs/call
-  {
-	Matrix computed;
-  long timeLog = clock();
-  for(int i = 0; i < n; i++)
-  	computed = Dproject_pose(camera, point1);
-  long timeLog2 = clock();
-  double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-  cout << ((double)n/seconds) << " calls/second" << endl;
-  cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
-  }
+//	{
+//		Matrix computed;
+//		long timeLog = clock();
+//		for(int i = 0; i < n; i++)
+//			computed = Dproject_pose(camera, point1);
+//		long timeLog2 = clock();
+//		double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+//		cout << ((double)n/seconds) << " calls/second" << endl;
+//		cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
+//	}
 
-  // Aug 8, iMac 3.06GHz Core i3
-  // AFTER collapse:
+	// Aug 8, iMac 3.06GHz Core i3
+	// AFTER collapse:
 	//  1.55383e+06 calls/second
 	//  0.64357 musecs/call
-  {
-  	Matrix computed;
-  long timeLog = clock();
-  for(int i = 0; i < n; i++)
-  	computed = Dproject_point(camera, point1);
-  long timeLog2 = clock();
-  double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-  cout << ((double)n/seconds) << " calls/second" << endl;
-  cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
-  }
+//	{
+//		Matrix computed;
+//		long timeLog = clock();
+//		for(int i = 0; i < n; i++)
+//			computed = Dproject_point(camera, point1);
+//		long timeLog2 = clock();
+//		double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+//		cout << ((double)n/seconds) << " calls/second" << endl;
+//		cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
+//	}
 
-  // Aug 8, iMac 3.06GHz Core i3
+	// Aug 8, iMac 3.06GHz Core i3
 	//  371153 calls/second
 	//  2.69431 musecs/call
-  // AFTER collapse:
+	// AFTER collapse:
 	//  1.10733e+06 calls/second
 	//  0.90307 musecs/call
-  {
-	Matrix computed1, computed2;
-  long timeLog = clock();
-  for(int i = 0; i < n; i++)
-  	Dproject_pose_point(camera, point1, computed1, computed2);
-  long timeLog2 = clock();
-  double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-  cout << ((double)n/seconds) << " calls/second" << endl;
-  cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
-  }
+	{
+		Matrix computed1, computed2;
+		long timeLog = clock();
+		for(int i = 0; i < n; i++)
+			camera.project(point1, computed1, computed2);
+		long timeLog2 = clock();
+		double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+		cout << ((double)n/seconds) << " calls/second" << endl;
+		cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
+	}
 
-  return 0;
+	return 0;
 }
diff --git a/nonlinear/LieConfig-inl.h b/nonlinear/LieConfig-inl.h
index 1c38fe9bd..640f2eb43 100644
--- a/nonlinear/LieConfig-inl.h
+++ b/nonlinear/LieConfig-inl.h
@@ -14,6 +14,7 @@
 #include <stdexcept>
 
 #include <gtsam/linear/VectorConfig.h>
+#include <gtsam/base/LieVector.h>
 #include <gtsam/base/Lie-inl.h>
 
 #include <gtsam/nonlinear/LieConfig.h>
diff --git a/slam/BearingFactor.h b/slam/BearingFactor.h
index ec224b95a..6ab89014c 100644
--- a/slam/BearingFactor.h
+++ b/slam/BearingFactor.h
@@ -35,7 +35,7 @@ namespace gtsam {
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
 		Vector evaluateError(const Pose2& pose, const Point2& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-			Rot2 hx = bearing(pose, point, H1, H2);
+			Rot2 hx = pose.bearing(point, H1, H2);
 			return logmap(between(z_, hx));
 		}
 
diff --git a/slam/BearingRangeFactor.h b/slam/BearingRangeFactor.h
index 44356e743..889ae4de9 100644
--- a/slam/BearingRangeFactor.h
+++ b/slam/BearingRangeFactor.h
@@ -44,10 +44,10 @@ namespace gtsam {
 			boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
 			boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
 
-			Rot2 y1 = gtsam::bearing(pose, point, H11_, H12_);
+			Rot2 y1 = pose.bearing(point, H11_, H12_);
 			Vector e1 = logmap(between(bearing_, y1));
 
-			double y2 = gtsam::range(pose, point, H21_, H22_);
+			double y2 = pose.range(point, H21_, H22_);
 			Vector e2 = Vector_(1, y2 - range_);
 
 			if (H1) *H1 = gtsam::stack(2, &H11, &H21);
diff --git a/slam/RangeFactor.h b/slam/RangeFactor.h
index 2c97ded3f..7c0e38f15 100644
--- a/slam/RangeFactor.h
+++ b/slam/RangeFactor.h
@@ -35,7 +35,7 @@ namespace gtsam {
 		/** h(x)-z */
 		Vector evaluateError(const Pose2& pose, const Point2& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-			double hx = gtsam::range(pose, point, H1, H2);
+			double hx = pose.range(point, H1, H2);
 			return Vector_(1, hx - z_);
 		}
 
diff --git a/slam/Simulated3D.h b/slam/Simulated3D.h
index 552e76322..397470468 100644
--- a/slam/Simulated3D.h
+++ b/slam/Simulated3D.h
@@ -9,6 +9,7 @@
 #pragma once
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/LieVector.h>
 #include <gtsam/linear/VectorConfig.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/inference/Key.h>
diff --git a/slam/TransformConstraint.h b/slam/TransformConstraint.h
index 6168199a2..5e0861985 100644
--- a/slam/TransformConstraint.h
+++ b/slam/TransformConstraint.h
@@ -48,19 +48,19 @@ public:
 				boost::optional<Matrix&> Dlocal = boost::none) const  {
 		if (Dforeign) {
 			Matrix Af;
-			Transform::transform_from(trans, foreign, boost::none, Af);
+			trans.transform_from(foreign, boost::none, Af);
 			*Dforeign = Af;
 		}
 		if (Dtrans) {
 			Matrix At;
-			Transform::transform_from(trans, foreign, At, boost::none);
+			trans.transform_from(foreign, At, boost::none);
 			*Dtrans = At;
 		}
 		if (Dlocal) {
 			Point dummy;
 			*Dlocal = -1* eye(dummy.dim());
 		}
-		return gtsam::logmap(gtsam::between<Point>(local, Transform::transform_from(trans, foreign)));
+		return gtsam::logmap(gtsam::between<Point>(local, trans.transform_from(foreign)));
 	}
 };
 } // \namespace gtsam
diff --git a/slam/pose2SLAM.h b/slam/pose2SLAM.h
index 98a6e2324..ba04c7778 100644
--- a/slam/pose2SLAM.h
+++ b/slam/pose2SLAM.h
@@ -7,6 +7,7 @@
 #pragma once
 
 #include <gtsam/inference/Key.h>
+#include <gtsam/base/LieVector.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LieConfig.h>
 #include <gtsam/slam/PriorFactor.h>
diff --git a/slam/visualSLAM.h b/slam/visualSLAM.h
index 7820e9070..61833253c 100644
--- a/slam/visualSLAM.h
+++ b/slam/visualSLAM.h
@@ -98,9 +98,7 @@ namespace gtsam { namespace visualSLAM {
     Vector evaluateError(const Pose3& pose, const Point3& point,
         boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
       SimpleCamera camera(*K_, pose);
-      if (H1) *H1=Dproject_pose(camera,point);
-      if (H2) *H2=Dproject_point(camera,point);
-      Point2 reprojectionError(project(camera, point) - z_);
+      Point2 reprojectionError(camera.project(point, H1, H2) - z_);
       return reprojectionError.vector();
     }
 
diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp
index 33ac38e57..219f2c161 100644
--- a/tests/testTransformConstraint.cpp
+++ b/tests/testTransformConstraint.cpp
@@ -94,7 +94,7 @@ TEST( TransformConstraint, jacobians_zero ) {
 	// get values that are ideal
 	Pose2 trans(2.0, 3.0, 0.0);
 	Point2 global(5.0, 6.0);
-	Point2 local = Pose2::transform_from(trans, global);
+	Point2 local = trans.transform_from(global);
 
 	PointTransformConstraint tc(lA1, t1, lB1);
 	Vector actCost = tc.evaluateError(global, trans, local),
@@ -129,8 +129,8 @@ TEST( TransformConstraint, converge_trans ) {
 	Pose2 transIdeal(7.0, 3.0, M_PI/2);
 
 	// verify direction
-	CHECK(assert_equal(local1, Pose2::transform_from(transIdeal, global1)));
-	CHECK(assert_equal(local2, Pose2::transform_from(transIdeal, global2)));
+	CHECK(assert_equal(local1, transIdeal.transform_from(global1)));
+	CHECK(assert_equal(local2, transIdeal.transform_from(global2)));
 
 	// choose transform
 //	Pose2 trans = transIdeal; // ideal - works
@@ -187,7 +187,7 @@ TEST( TransformConstraint, converge_local ) {
 //	Pose2 trans(1.5, 2.5, 1.0); // larger rotation
 	Pose2 trans(1.5, 2.5, 3.1); // significant rotation
 
-	Point2 idealLocal = Pose2::transform_from(trans, global);
+	Point2 idealLocal = trans.transform_from(global);
 
 	// perturb the initial estimate
 //	Point2 local = idealLocal; // Ideal case - works
@@ -226,7 +226,7 @@ TEST( TransformConstraint, converge_global ) {
 //	Pose2 trans(1.5, 2.5, 1.0); // larger rotation
 	Pose2 trans(1.5, 2.5, 3.1); // significant rotation
 
-	Point2 idealForeign = Pose2::transform_from(inverse(trans), local);
+	Point2 idealForeign = inverse(trans).transform_from(local);
 
 	// perturb the initial estimate
 //	Point2 global = idealForeign; // Ideal - works