From b0fd5396adaae210cf037b0f6ec441debea669ab Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 10 Jan 2010 22:58:30 +0000 Subject: [PATCH] Bug fix in Rot3 for logmap(identity) --- cpp/Rot3.h | 2 +- cpp/testRot3.cpp | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/cpp/Rot3.h b/cpp/Rot3.h index 9c235749a..938035034 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -158,7 +158,7 @@ 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) return ones(3); // todo: identity? + if (tr==3.0) return zero(3); if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");; double theta = acos((tr-1.0)/2.0); return (theta/2.0/sin(theta))*Vector_(3, diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 8fc02cd88..b4ec5bbac 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -121,6 +121,14 @@ TEST(Rot3, log) Vector w = Vector_(3, 0.1, 0.4, 0.2); Rot3 R = rodriguez(w); CHECK(assert_equal(w, logmap(R))); + + Vector w5 = Vector_(3, 0.0, 0.0, 0.0); + Rot3 R5 = rodriguez(w5); + CHECK(assert_equal(w5, logmap(R5))); + +// Vector w6 = Vector_(3, M_PI, 0.0, 0.0); +// Rot3 R6 = rodriguez(w6); +// CHECK(assert_equal(w6, logmap(R6))); } /* ************************************************************************* */