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); }