diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index e549cb009..f79b71c32 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -314,35 +314,47 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
 
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
-    OptionalJacobian<1, 3> H2) const {
+                    OptionalJacobian<1, 3> H2) const {
+  Matrix36 D1;
+  Matrix3 D2;
+  Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
   if (!H1 && !H2) {
-    return transform_to(point).norm();
+    return local.norm();
   } else {
-    Matrix36 D1;
-    Matrix3 D2;
-    Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
-    const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
-        n = sqrt(d2);
-    Matrix13 D_result_d;
-    D_result_d << x / n, y / n, z / n;
-    if (H1) *H1 = D_result_d * D1;
-    if (H2) *H2 = D_result_d * D2;
-    return n;
+    Matrix13 D_r_local;
+    const double r = local.norm(D_r_local);
+    if (H1) *H1 = D_r_local * D1;
+    if (H2) *H2 = D_r_local * D2;
+    return r;
   }
 }
 
 /* ************************************************************************* */
-double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
-    OptionalJacobian<1,6> H2) const {
+double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
+                    OptionalJacobian<1, 6> H2) const {
   Matrix13 D2;
-  double r = range(pose.translation(), H1, H2? &D2 : 0);
-  if (H2) {
-    Matrix13 H2_ = D2 * pose.rotation().matrix();
-    *H2 << Matrix13::Zero(), H2_;
-  }
+  double r = range(pose.translation(), H1, H2 ? &D2 : 0);
+  if (H2) *H2 << Matrix13::Zero(), D2 * pose.rotation().matrix();
   return r;
 }
 
+/* ************************************************************************* */
+Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
+                     OptionalJacobian<2, 3> H2) const {
+  Matrix36 D1;
+  Matrix3 D2;
+  Point3 local = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
+  if (!H1 && !H2) {
+    return Unit3(local);
+  } else {
+    Matrix23 D_b_local;
+    Unit3 b = Unit3::FromPoint3(local, D_b_local);
+    if (H1)* H1 = D_b_local * D1;
+    if (H2) *H2 = D_b_local * D2;
+    return b;
+  }
+}
+
 /* ************************************************************************* */
 boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
   const size_t n = pairs.size();
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 8a0f23404..6e36f7ad0 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -262,6 +262,14 @@ public:
   double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
       OptionalJacobian<1, 6> H2 = boost::none) const;
 
+  /**
+   * Calculate bearing to a landmark
+   * @param point 3D location of landmark
+   * @return bearing (Unit3)
+   */
+  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
+      OptionalJacobian<2, 3> H2 = boost::none) const;
+
   /// @}
   /// @name Advanced Interface
   /// @{