diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 574fc0d20..9ffec1a18 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -153,7 +153,7 @@ namespace gtsam { // Calculate delta rotation = between(R1,R2) double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::fromCosSin(c,s)); + Rot2 R(Rot2::atan2(s,c)); // normalizes // Calculate delta translation = unrotate(R1, dt); Point2 dt = p2.t() - p1.t(); diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 111a2db4c..11189dc9c 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -52,7 +52,7 @@ namespace gtsam { /** Constructor from 3*3 matrix */ Pose2(const Matrix &T) : - r_(Rot2::fromCosSin(T(0, 0), T(1, 0))), t_(T(0, 2), T(1, 2)) {} + r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {} /** print with optional string */ void print(const std::string& s = "") const;