diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index 283147e4c..f8ec9c9e6 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -129,6 +129,14 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   }
 }
 
+/* ************************************************************************* */
+static Rot2 ClosestTo(const Matrix2& M) {
+   double theta_rad = atan2(M(0,0), M(1,0));
+   double c = cos(theta_rad);
+   double s = sin(theta_rad);
+   return Rot2::fromCosSin(c, s);
+}
+  
 /* ************************************************************************* */
 
 } // gtsam