From 729b35fe11328816c9ef98d1047be259579b753d Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Mon, 2 May 2011 16:59:33 +0000 Subject: [PATCH] Logmap fix from last week. Rot3 trace > 3 is now handled, previously caused NaN --- gtsam/geometry/Rot3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 7156a2df1..882dd69e8 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -161,7 +161,7 @@ namespace gtsam { // Log map at identity - return the canonical coordinates of this rotation Vector Rot3::Logmap(const Rot3& R) { double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (fabs(tr-3.0) < 1e-10) { // when theta = 0, +-2pi, +-4pi, etc. + if (tr > 3.0 - 1e-10) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) return zero(3); } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. if(fabs(R.r3().z() - -1.0) > 1e-10)