finish Rot2.ClosestTo()

2021-12-07 11:59:28 -07:00 · 2021-12-07 11:59:28 -07:00 · 37af8cfbec
parent dfb9497d81
commit 37af8cfbec
1 changed files with 8 additions and 0 deletions
--- 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