diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 611cf7cde..415410aa4 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -108,7 +108,7 @@ Matrix3 Unit3::skew() const {
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
+Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
   Matrix23 Bt = basis().transpose();
   Vector2 xi = Bt * q.p_.vector();
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 50ffb55b7..2bcd5dfa8 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -105,13 +105,20 @@ public:
     return p_;
   }
 
+  /// Return unit-norm Vector
+  Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
+    if (H)
+      *H = basis();
+    return (p_.vector ());
+  }
+
   /// Return scaled direction as Point3
   friend Point3 operator*(double s, const Unit3& d) {
     return s * d.p_;
   }
 
   /// Signed, vector-valued error between two directions
-  Vector2 error(const Unit3& q,
+  Vector error(const Unit3& q,
       OptionalJacobian<2,2> H = boost::none) const;
 
   /// Distance between two directions