From f9f3b1c47d98e41e85dbb9ad283c6e45f7b4d1c0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2012 19:40:42 +0000 Subject: [PATCH] Did more extensive testing on Logmap, cleaned that up, and replaced Taylor expansion on theta with one on (trace-3) --- gtsam/geometry/Rot3M.cpp | 64 ++++++++++++++------------ gtsam/geometry/tests/testRot3M.cpp | 72 +++++++++++++++++++----------- 2 files changed, 81 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 5cc637a47..0aef05f1b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -20,8 +20,9 @@ #ifndef GTSAM_DEFAULT_QUATERNIONS -#include #include +#include +#include using namespace std; @@ -119,7 +120,7 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; #ifndef NDEBUG double l_n = wwTxx + wwTyy + wwTzz; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); + if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); #endif double c = cos(theta), s = sin(theta), c_1 = 1 - c; @@ -205,34 +206,39 @@ Point3 Rot3::unrotate(const Point3& p, /* ************************************************************************* */ // 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(); - // FIXME should tr in statement below be absolute value? - if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) - return zero(3); - } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) - double theta = acos((tr-1.0)/2.0); - // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) - return (0.5 + theta*theta/12)*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - // FIXME: in statement below, is this the right comparision? - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(fabs(R.r3().z() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * - Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(fabs(R.r2().y() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * - Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * - Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); + + static const double PI = boost::math::constants::pi(); + + // Get trace(R) + double tr = R.r1_.x()+R.r2_.y()+R.r3_.z(); + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr+1.0) < 1e-10) { + if(std::abs(R.r3_.z()+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*R.r3_.z())) * + Vector_(3, R.r3_.x(), R.r3_.y(), 1.0+R.r3_.z()); + else if(std::abs(R.r2_.y()+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*R.r2_.y())) * + Vector_(3, R.r2_.x(), 1.0+R.r2_.y(), R.r2_.z()); + else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + return (PI / sqrt(2.0+2.0*R.r1_.x())) * + Vector_(3, 1.0+R.r1_.x(), R.r1_.y(), R.r1_.z()); } else { - double theta = acos((tr-1.0)/2.0); - return (theta/2.0/sin(theta))*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); + double magnitude; + double tr_3 = tr-3.0; // always negative + if (tr_3<-1e-7) { + double theta = acos((tr-1.0)/2.0); + magnitude = theta/(2.0*sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3*tr_3/12.0; + } + return magnitude*Vector_(3, + R.r2_.z()-R.r3_.y(), + R.r3_.x()-R.r1_.z(), + R.r1_.y()-R.r2_.x()); } } diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 88836280e..6dacc75c9 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -15,16 +15,20 @@ * @author Alireza Fathi */ -#include #include -#include #include #include + #include #include +#include + +#include + #ifndef GTSAM_DEFAULT_QUATERNIONS +using namespace std; using namespace gtsam; Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); @@ -147,37 +151,53 @@ TEST( Rot3, expmap) /* ************************************************************************* */ TEST(Rot3, log) { - Vector w1 = Vector_(3, 0.1, 0.0, 0.0); - Rot3 R1 = Rot3::rodriguez(w1); - CHECK(assert_equal(w1, Rot3::Logmap(R1))); + static const double PI = boost::math::constants::pi(); + Vector w; + Rot3 R; - Vector w2 = Vector_(3, 0.0, 0.1, 0.0); - Rot3 R2 = Rot3::rodriguez(w2); - CHECK(assert_equal(w2, Rot3::Logmap(R2))); +#define CHECK_OMEGA(X,Y,Z) \ + w = Vector_(3, (double)X, (double)Y, double(Z)); \ + R = Rot3::rodriguez(w); \ + EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); - Vector w3 = Vector_(3, 0.0, 0.0, 0.1); - Rot3 R3 = Rot3::rodriguez(w3); - CHECK(assert_equal(w3, Rot3::Logmap(R3))); + // Check zero + CHECK_OMEGA( 0, 0, 0) - Vector w = Vector_(3, 0.1, 0.4, 0.2); - Rot3 R = Rot3::rodriguez(w); - CHECK(assert_equal(w, Rot3::Logmap(R))); + // create a random direction: + double norm=sqrt(1.0+16.0+4.0); + double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector w5 = Vector_(3, 0.0, 0.0, 0.0); - Rot3 R5 = Rot3::rodriguez(w5); - CHECK(assert_equal(w5, Rot3::Logmap(R5))); + // Check very small rotation for Taylor expansion + // Note that tolerance above is 1e-12, so Taylor is pretty good ! + double d = 0.0001; + CHECK_OMEGA( d, 0, 0) + CHECK_OMEGA( 0, d, 0) + CHECK_OMEGA( 0, 0, d) + CHECK_OMEGA(x*d, y*d, z*d) - Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); - Rot3 R6 = Rot3::rodriguez(w6); - CHECK(assert_equal(w6, Rot3::Logmap(R6))); + // check normal rotation + d = 0.1; + CHECK_OMEGA( d, 0, 0) + CHECK_OMEGA( 0, d, 0) + CHECK_OMEGA( 0, 0, d) + CHECK_OMEGA(x*d, y*d, z*d) - Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); - Rot3 R7 = Rot3::rodriguez(w7); - CHECK(assert_equal(w7, Rot3::Logmap(R7))); + // Check 180 degree rotations + CHECK_OMEGA( PI, 0, 0) + CHECK_OMEGA( 0, PI, 0) + CHECK_OMEGA( 0, 0, PI) + CHECK_OMEGA(x*PI,y*PI,z*PI) - Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); - Rot3 R8 = Rot3::rodriguez(w8); - CHECK(assert_equal(w8, Rot3::Logmap(R8))); + // Check 360 degree rotations +#define CHECK_OMEGA_ZERO(X,Y,Z) \ + w = Vector_(3, (double)X, (double)Y, double(Z)); \ + R = Rot3::rodriguez(w); \ + EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); + + CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) + CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) + CHECK_OMEGA_ZERO( 0, 0, 2.0*PI) + CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } /* ************************************************************************* */