From 68535708af01697ff92ebd0d01b536f4687db728 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Tue, 7 Dec 2021 13:31:39 -0700
Subject: [PATCH] fix typo

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

diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index f8ec9c9e6..9f62869e4 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -131,9 +131,11 @@ 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);
+   double c = M(0,0);
+   double s = M(1,0);
+   double theta_rad = atan2(s, c);
+   c = cos(theta_rad);
+   s = sin(theta_rad);
    return Rot2::fromCosSin(c, s);
 }