From f89a53ed916e8c5fae58cd35268df9cbec881a89 Mon Sep 17 00:00:00 2001 From: justinca Date: Thu, 21 Jan 2010 19:49:12 +0000 Subject: [PATCH] Bugfix in Rot3::logmap. The function checked for a trace of 3, but the check could fail due to rounding errors, causing the function to try to take acos(something larger than 1) resulting in NaNs being returned. --- cpp/Rot3.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 166b550b0..c72621538 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -113,7 +113,8 @@ namespace gtsam { // Log map at identity - return the canonical coordinates of this rotation inline Vector logmap(const Rot3& R) { double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (tr==3.0) // when theta = 0, +-2pi, +-4pi, etc. + if (fabs(tr-3.0) < 1e-10) // when theta = 0, +-2pi, +-4pi, etc. +// if (tr == 3.0) // when theta = 0, +-2pi, +-4pi, etc. return zero(3); else if (tr==-1.0) { // when theta = +-pi, +-3pi, +-5pi, etc. if(R.r3().z() != -1.0)