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