From 37af8cfbecf7bf05446976e485f44f0396c2a819 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 11:59:28 -0700
Subject: [PATCH] finish Rot2.ClosestTo()

---
 gtsam/geometry/Rot2.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

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