From 2357e8aefc2293a5e8fe5d883519292eca0e0dfe Mon Sep 17 00:00:00 2001 From: Alireza Fathi Date: Thu, 3 Sep 2009 17:08:52 +0000 Subject: [PATCH] There was a serious mistake in rodriguez, instead of R was returning R' --- cpp/Rot3.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 438e4ec06..e8c9ab49a 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -23,12 +23,9 @@ namespace gtsam { double ct = cos(t), st = sin(t); - Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(1) * w(0) * (1 - ct) - w( - 2) * st, w(1) * st + w(2) * w(0) * (1 - ct)); - Point3 r2 = Point3(w(2) * st + w(0) * w(1) * (1 - ct), w(1) * w(1) * (1 - - ct) + ct, -w(0) * st + w(2) * w(1) * (1 - ct)); - Point3 r3 = Point3(-w(1) * st + w(0) * w(2) * (1 - ct), w(1) * w(2) * (1 - - ct) + w(0) * st, ct + w(2) * w(2) * (1 - ct)); + Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(2) * st + w(0) * w(1) * (1 - ct), -w(1) * st + w(0) * w(2) * (1 - ct)); + Point3 r2 = Point3(w(1) * w(0) * (1 - ct) - w(2) * st, w(1) * w(1) * (1 - ct) + ct, w(1) * w(2) * (1 - ct) + w(0) * st); + Point3 r3 = Point3(w(1) * st + w(2) * w(0) * (1 - ct), -w(0) * st + w(2) * w(1) * (1 - ct), ct + w(2) * w(2) * (1 - ct)); return Rot3(r1, r2, r2); }