Add user-defined copy constructor for Rot2

borglab · Jun 29, 2021 · 7c358aa · 7c358aa
1 parent 6ee17c2
commit 7c358aa
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 8 deletions.
diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
@@ -25,12 +25,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Rot2 Rot2::fromCosSin(double c, double s) {
-  if (std::abs(c * c + s * s - 1.0) > 1e-9) {
-    double norm_cs = sqrt(c*c + s*s);
-    c = c/norm_cs;
-    s = s/norm_cs;
-  }
-  return Rot2(c, s);
+  Rot2 R(c, s);
+  return R.normalize();
 }
 
 /* ************************************************************************* */
@@ -59,8 +55,8 @@ bool Rot2::equals(const Rot2& R, double tol) const {
 /* ************************************************************************* */
 Rot2& Rot2::normalize() {
   double scale = c_*c_ + s_*s_;
-  if(std::abs(scale-1.0)>1e-10) {
-    scale = pow(scale, -0.5);
+  if(std::abs(scale-1.0) > 1e-10) {
+    scale = 1 / sqrt(scale);
     c_ *= scale;
     s_ *= scale;
   }

diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
@@ -50,6 +50,9 @@ namespace gtsam {
 
     /** default constructor, zero rotation */
     Rot2() : c_(1.0), s_(0.0) {}
+
+    /** copy constructor */
+    Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
 
     /// Constructor from angle in radians == exponential map at identity
     Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}