diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index 7b1a26e2c..181ff3e97 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -38,41 +38,51 @@ bool Point3::operator==(const Point3& q) const {
   return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator+(const Point3& q) const {
   return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator-(const Point3& q) const {
   return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator*(double s) const {
   return Point3(x_ * s, y_ * s, z_ * s);
 }
 
-/* ************************************************************************* */
 Point3 Point3::operator/(double s) const {
   return Point3(x_ / s, y_ / s, z_ / s);
 }
 #endif
 
 /* ************************************************************************* */
-double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
+double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
                         OptionalJacobian<1, 3> H2) const {
-  double d = (p2 - *this).norm();
-  if (H1) {
-    *H1 << x() - p2.x(), y() - p2.y(), z() - p2.z();
-    *H1 = *H1 *(1. / d);
-  }
+  return gtsam::distance(*this,q,H1,H2);
+}
 
-  if (H2) {
-    *H2 << -x() + p2.x(), -y() + p2.y(), -z() + p2.z();
-    *H2 = *H2 *(1. / d);
-  }
-  return d;
+double Point3::norm(OptionalJacobian<1,3> H) const {
+  return gtsam::norm(*this, H);
+}
+
+Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
+  return gtsam::normalize(*this, H);
+}
+
+Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
+                     OptionalJacobian<3, 3> H2) const {
+  return gtsam::cross(*this, q, H1, H2);
+}
+
+double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
+                   OptionalJacobian<1, 3> H2) const {
+  return gtsam::dot(*this, q, H1, H2);
+}
+
+/* ************************************************************************* */
+ostream &operator<<(ostream &os, const Point3& p) {
+  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
+  return os;
 }
 
 /* ************************************************************************* */
@@ -93,59 +103,56 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
 #endif
 
 /* ************************************************************************* */
-Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
-  if (H_p) {
-    *H_p << skewSymmetric(-q.vector());
+double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
+                OptionalJacobian<1, 3> H2) {
+  double d = (q - p1).norm();
+  if (H1) {
+    *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
+    *H1 = *H1 *(1. / d);
   }
-  if (H_q) {
-    *H_q << skewSymmetric(vector());
+  if (H2) {
+    *H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z();
+    *H2 = *H2 *(1. / d);
   }
-
-  return Point3(y() * q.z() - z() * q.y(), z() * q.x() - x() * q.z(),
-      x() * q.y() - y() * q.x());
+  return d;
 }
 
-/* ************************************************************************* */
-double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
-  if (H_p) {
-    *H_p << q.vector().transpose();
-  }
-  if (H_q) {
-    *H_q << vector().transpose();
-  }
-
-  return (x() * q.x() + y() * q.y() + z() * q.z());
-}
-
-/* ************************************************************************* */
-double Point3::norm(OptionalJacobian<1,3> H) const {
-  double r = std::sqrt(x() * x() + y() * y() + z() * z());
+double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
+  double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
   if (H) {
     if (fabs(r) > 1e-10)
-      *H << x() / r, y() / r, z() / r;
+      *H << p.x() / r, p.y() / r, p.z() / r;
     else
-      *H << 1, 1, 1; // really infinity, why 1 ?
+      *H << 1, 1, 1;  // really infinity, why 1 ?
   }
   return r;
 }
 
-/* ************************************************************************* */
-Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
-  Point3 normalized = *this / norm();
+Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
+  Point3 normalized = p / norm(p);
   if (H) {
     // 3*3 Derivative
-    double x2 = x() * x(), y2 = y() * y(), z2 = z() * z();
-    double xy = x() * y(), xz = x() * z(), yz = y() * z();
-    *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
-    *H /= std::pow(x2 + y2 + z2, 1.5);
+    double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();
+    double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z();
+    *H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2;
+    *H /= pow(x2 + y2 + z2, 1.5);
   }
   return normalized;
 }
 
-/* ************************************************************************* */
-ostream &operator<<(ostream &os, const Point3& p) {
-  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
-  return os;
+Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
+             OptionalJacobian<3, 3> H2) {
+  if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
+  if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
+  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());
+}
+
+double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
+           OptionalJacobian<1, 3> H2) {
+  if (H1) *H1 << q.x(), q.y(), q.z();
+  if (H2) *H2 << p.x(), p.y(), p.z();
+  return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 467703d46..75b8a7829 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -190,14 +190,14 @@ class GTSAM_EXPORT Point3 {
     Point3 operator / (double s) const;
 
     /** distance between two points */
-    double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
+    double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) const;
 
     /** Distance of the point from the origin, with Jacobian */
     double norm(OptionalJacobian<1,3> H = boost::none) const;
 
     /** normalize, with optional Jacobian */
-    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
+    Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
 
     /** cross product @return this x q */
     Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@@ -241,7 +241,8 @@ class GTSAM_EXPORT Point3 {
     Point3 retract(const Vector3& v) const { return compose(Point3(v));}
     static Vector3 Logmap(const Point3& p) { return p.vector();}
     static Point3 Expmap(const Vector3& v) { return Point3(v);}
-    inline double dist(const Point3& p2) const { return (p2 - *this).norm(); }
+    inline double dist(const Point3& q) const { return (q - *this).norm(); }
+    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
     Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
                OptionalJacobian<3, 3> H2 = boost::none) const;
     Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
@@ -282,6 +283,31 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
 template<>
 struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 
+/// distance between two points
+double distance(const Point3& p1, const Point3& q,
+                OptionalJacobian<1, 3> H1 = boost::none,
+                OptionalJacobian<1, 3> H2 = boost::none);
+
+/// Distance of the point from the origin, with Jacobian
+double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
+
+/// normalize, with optional Jacobian
+Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
+
+/// cross product @return this x q
+Point3 cross(const Point3& p, const Point3& q,
+             OptionalJacobian<3, 3> H_p = boost::none,
+             OptionalJacobian<3, 3> H_q = boost::none);
+
+/// dot product
+double dot(const Point3& p, const Point3& q,
+           OptionalJacobian<1, 3> H_p = boost::none,
+           OptionalJacobian<1, 3> H_q = boost::none);
+
+// Convenience typedef
+typedef std::pair<Point3, Point3> Point3Pair;
+std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+
 template <typename A1, typename A2>
 struct Range;
 
@@ -291,7 +317,7 @@ struct Range<Point3, Point3> {
   double operator()(const Point3& p, const Point3& q,
                     OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) {
-    return p.distance(q, H1, H2);
+    return distance(p, q, H1, H2);
   }
 };
 
diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 203a08fb3..69d4276dd 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -45,7 +45,7 @@ namespace gtsam {
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
   // 3*3 Derivative of representation with respect to point is 3*3:
   Matrix3 D_p_point;
-  Point3 normalized = point.normalize(H ? &D_p_point : 0);
+  Point3 normalized = normalize(point, H ? &D_p_point : 0);
   Unit3 direction;
   direction.p_ = normalized.vector();
   if (H)
@@ -109,7 +109,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 
   // Normalize result to get a unit vector: b1 = B1 / |B1|.
   Matrix33 H_b1_B1;
-  Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0);
+  Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0);
 
   // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
   // No need to normalize this, p and b1 are orthogonal unit vectors.
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index d0123af83..37ad105c7 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -96,9 +96,9 @@ TEST (Point3, normalize) {
   Matrix actualH;
   Point3 point(1, -2, 3); // arbitrary point
   Point3 expected(point / sqrt(14.0));
-  EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
+  EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<Point3, Point3>(
-      boost::bind(&Point3::normalize, _1, boost::none), point);
+      boost::bind(gtsam::normalize, _1, boost::none), point);
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp
index 1c4e6f714..f83b0c516 100644
--- a/python/handwritten/geometry/Point3.cpp
+++ b/python/handwritten/geometry/Point3.cpp
@@ -41,7 +41,7 @@ class_<Point3>("Point3")
   .def("dot", &Point3::dot)
   .def("equals", &Point3::equals, equals_overloads(args("q","tol")))
   .def("norm", &Point3::norm)
-  .def("normalize", &Point3::normalize)
+  .def("normalized", &Point3::normalized)
   .def("print", &Point3::print, print_overloads(args("s")))
 #ifndef GTSAM_USE_VECTOR3_POINTS
   .def("vector", &Point3::vector)
diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp
index 2d0576aea..088ec7b1f 100644
--- a/timing/timeCalibratedCamera.cpp
+++ b/timing/timeCalibratedCamera.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -27,12 +27,7 @@ int main()
 {
   int n = 100000;
 
-  const Pose3 pose1(Matrix3((Matrix(3,3) <<
-      1., 0., 0.,
-      0.,-1., 0.,
-      0., 0.,-1.
-  ).finished()),
-  Point3(0,0,0.5));
+  const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
 
   const CalibratedCamera camera(pose1);
   const Point3 point1(-0.08,-0.08, 0.0);
diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp
index 6f7e5e972..458f88db1 100644
--- a/timing/timePinholeCamera.cpp
+++ b/timing/timePinholeCamera.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -28,12 +28,7 @@ int main()
 {
   int n = 1e6;
 
-  const Pose3 pose1((Matrix)(Matrix(3,3) <<
-      1., 0., 0.,
-      0.,-1., 0.,
-      0., 0.,-1.
-  ).finished(),
-  Point3(0,0,0.5));
+  const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
 
   static Cal3Bundler K(500, 1e-3, 2.0*1e-3);
   const PinholeCamera<Cal3Bundler> camera(pose1,K);
diff --git a/timing/timeStereoCamera.cpp b/timing/timeStereoCamera.cpp
index ab8e2fff9..c609f8885 100644
--- a/timing/timeStereoCamera.cpp
+++ b/timing/timeStereoCamera.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -27,12 +27,7 @@ int main()
 {
   int n = 100000;
 
-  const Pose3 pose1((Matrix)(Matrix(3,3) <<
-      1., 0., 0.,
-      0.,-1., 0.,
-      0., 0.,-1.
-  ).finished(),
-  Point3(0,0,0.5));
+  const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
 
   const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
   const StereoCamera camera(pose1, K);