From dfb9497d8162206f2dd419f218e4bf6c435f04d5 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 11:53:34 -0700
Subject: [PATCH] add Rot2.ClosestTo()

---
 gtsam/geometry/Rot2.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index ec30c6657..42dba3487 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -14,6 +14,7 @@
  * @brief 2D rotation
  * @date Dec 9, 2009
  * @author Frank Dellaert
+ * @author John Lambert
  */
 
 #pragma once
@@ -208,6 +209,9 @@ namespace gtsam {
 
     /** return 2*2 transpose (inverse) rotation matrix   */
     Matrix2 transpose() const;
+    
+    /** Find closest valid rotation matrix, given a 2x2 matrix */
+    static Rot2 ClosestTo(const Matrix2& M);
 
   private:
     /** Serialization function */