From 30367e35fbb10e0d665666d3551349804dee853c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 10 Jan 2010 12:25:46 +0000 Subject: [PATCH] Much faster compund rotation using Justin's (indeed correct) formula --- cpp/Rot3.cpp | 23 +++++++++++++++++++++ cpp/Rot3.h | 22 ++------------------ cpp/testRot3.cpp | 52 ++++++++++++++++++++++++++++++++++++++++++++---- cpp/timeRot3.cpp | 22 +++++++++++++++++++- 4 files changed, 94 insertions(+), 25 deletions(-) diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index bd4728d66..710ce4be2 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -43,6 +43,29 @@ namespace gtsam { 0, 0, 1); } + // Considerably faster than composing matrices above ! + Rot3 Rot3::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); + } + /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); diff --git a/cpp/Rot3.h b/cpp/Rot3.h index 723e8194a..bd28d633d 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -63,19 +63,17 @@ namespace gtsam { static Rot3 Rx(double t); static Rot3 Ry(double t); static Rot3 Rz(double t); - static Rot3 RzRyRx(double x, double y, double z) { return Rz(z)*Ry(y)*Rx(x);} + static Rot3 RzRyRx(double x, double y, double z); /** * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf - * Differs from one defined by Justin, who uses, (x,y,z) = (roll,-pitch,yaw) - * and http://en.wikipedia.org/wiki/Yaw,_pitch,_and_roll, where (x,y,z) = (-roll,pitch,yaw) * Assumes vehicle coordinate frame X forward, Y right, Z down */ static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading) static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude) static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) - static Rot3 ypr (double y, double p, double r) { return yaw(y)*pitch(p)*roll(r);} + static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} @@ -178,22 +176,6 @@ namespace gtsam { R.r3().x(), R.r3().y(), R.r3().z()); } - /** - - * Update Rotation with incremental rotation - * @param v a vector of incremental roll,pitch,yaw - * @param R a rotated frame - * @return incremental rotation matrix - */ - //Rot3 exp(const Rot3& R, const Vector& v); - - /** - * @param a rotation R - * @param a rotation S - * @return log(S*R'), i.e. canonical coordinates of between(R,S) - */ - //Vector log(const Rot3& R, const Rot3& S); - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 889a3b8a7..8fc02cd88 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -129,12 +129,28 @@ TEST(Rot3, log) Rot3 t1 = rodriguez(0.1, 0.4, 0.2); Rot3 t2 = rodriguez(0.3, 0.1, 0.7); Rot3 origin; + + // log behaves correctly Vector d12 = logmap(t1, t2); CHECK(assert_equal(t2, expmap(t1,d12))); CHECK(assert_equal(t2, expmap(d12)*t1)); Vector d21 = logmap(t2, t1); CHECK(assert_equal(t1, expmap(t2,d21))); CHECK(assert_equal(t1, expmap(d21)*t2)); + + // Check that log(t1,t2)=-log(t2,t1) + CHECK(assert_equal(d12,-d21)); + + // lines in canonical coordinates correspond to Abelian subgroups in SO(3) + Vector d = Vector_(3,0.1,0.2,0.3); + // exp(-d)=inverse(exp(d)) + CHECK(assert_equal(expmap(-d),inverse(expmap(d)))); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + Rot3 R2 = expmap(2*d); + Rot3 R3 = expmap(3*d); + Rot3 R5 = expmap(5*d); + CHECK(assert_equal(R5,R2*R3)); + CHECK(assert_equal(R5,R3*R2)); } /* ************************************************************************* */ @@ -233,15 +249,39 @@ TEST( Rot3, xyz ) { double t = 0.1, st = sin(t), ct = cos(t); - Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); + // Make sure all counterclockwise + // Diagrams below are all from from unchanging axis + + // z + // | * Y=(ct,st) + // x----y + Rot3 expected1( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); CHECK(assert_equal(expected1,Rot3::Rx(t))); - Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); + // x + // | * Z=(ct,st) + // y----z + Rot3 expected2( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); CHECK(assert_equal(expected2,Rot3::Ry(t))); - // yaw is around z axis - Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + // y + // | X=* (ct,st) + // z----x + Rot3 expected3( + ct, -st, 0, + st, ct, 0, + 0, 0, 1); CHECK(assert_equal(expected3,Rot3::Rz(t))); + + // Check compound rotation + Rot3 expected = Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1); + CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3))); } /* ************************************************************************* */ @@ -257,6 +297,10 @@ TEST( Rot3, yaw_pitch_roll ) // roll is around x axis CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); + + // Check compound rotation + Rot3 expected = Rot3::yaw(0.1)*Rot3::pitch(0.2)*Rot3::roll(0.3); + CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); } /* ************************************************************************* */ diff --git a/cpp/timeRot3.cpp b/cpp/timeRot3.cpp index 850a8ee5a..51c62183c 100644 --- a/cpp/timeRot3.cpp +++ b/cpp/timeRot3.cpp @@ -14,9 +14,10 @@ using namespace gtsam; int main() { - int n = 3000000; + int n = 300000; Vector v = Vector_(3,1.,0.,0.); + // Rodriguez formula given axis angle long timeLog = clock(); for(int i = 0; i < n; i++) rodriguez(v,0.001); @@ -25,6 +26,7 @@ int main() cout << seconds << " seconds" << endl; cout << ((double)n/seconds) << " calls/second" << endl; + // Rodriguez formula given canonical coordinates timeLog = clock(); for(int i = 0; i < n; i++) rodriguez(v); @@ -33,5 +35,23 @@ int main() cout << seconds << " seconds" << endl; cout << ((double)n/seconds) << " calls/second" << endl; + // Slow rotation matrix + timeLog = clock(); + for(int i = 0; i < n; i++) + Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1); + timeLog2 = clock(); + seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << seconds << " seconds" << endl; + cout << ((double)n/seconds) << " calls/second" << endl; + + // Fast Rotation matrix + timeLog = clock(); + for(int i = 0; i < n; i++) + Rot3::RzRyRx(0.1,0.2,0.3); + timeLog2 = clock(); + seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << seconds << " seconds" << endl; + cout << ((double)n/seconds) << " calls/second" << endl; + return 0; }