diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 4e643fde9..82ba979fd 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -67,11 +67,11 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-const Matrix& Unit3::basis() const {
+const Unit3::Matrix32& Unit3::basis() const {
 
   // Return cached version if exists
-  if (B_.rows() == 3)
-    return B_;
+  if (B_)
+    return *B_;
 
   // Get the axis of rotation with the minimum projected length of the point
   Point3 axis;
@@ -92,9 +92,9 @@ const Matrix& Unit3::basis() const {
   b2 = b2 / b2.norm();
 
   // Create the basis matrix
-  B_ = Matrix(3, 2);
-  B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
-  return B_;
+  B_.reset(Unit3::Matrix32());
+  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
+  return *B_;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 8d2c024c0..23dc535e8 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
 #include <boost/random/mersenne_twister.hpp>
+#include <boost/optional.hpp>
 
 namespace gtsam {
 
@@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {
 
 private:
 
+  typedef Eigen::Matrix<double,3,2> Matrix32;
+
   Point3 p_; ///< The location of the point on the unit sphere
-  mutable Matrix B_; ///< Cached basis
+  mutable boost::optional<Matrix32> B_; ///< Cached basis
 
 public:
 
@@ -84,7 +87,7 @@ public:
    * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
    * tangent to the sphere at the current direction.
    */
-  const Matrix& basis() const;
+  const Matrix32& basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
   Matrix skew() const;