From 784bdc64c5266990201e5d8659d05a2084fe7567 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 6 Feb 2022 00:08:31 -0500 Subject: [PATCH] minor fixes to Pose2 and Rot2 --- gtsam/geometry/Pose2.h | 4 ++-- gtsam/geometry/Rot2.cpp | 16 ++++++++-------- gtsam/geometry/Rot2.h | 2 +- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 12087a34c..eb8ddfb5b 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -324,8 +324,8 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); // Convenience typedef using Pose2Pair = std::pair; -using Pose2Pairs = std::vector >; - +using Pose2Pairs = std::vector; + template <> struct traits : public internal::LieGroup {}; diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9f62869e4..d43b9b12d 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -130,15 +130,15 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { } /* ************************************************************************* */ -static Rot2 ClosestTo(const Matrix2& M) { - 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); +Rot2 Rot2::ClosestTo(const Matrix2& M) { + double c = M(0, 0); + double s = M(1, 0); + double theta_rad = std::atan2(s, c); + c = cos(theta_rad); + s = sin(theta_rad); + return Rot2::fromCosSin(c, s); } - + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 42dba3487..2690ca248 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -209,7 +209,7 @@ 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);