From c9fcf95501b501f40862f0f59167414277f9c0eb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 3 Mar 2010 05:34:08 +0000 Subject: [PATCH] fixed some issues in SE(2) "branch" --- cpp/Pose2.cpp | 2 +- cpp/Pose2.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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;