diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index d5dc6d53a..1e7252b31 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -114,8 +114,8 @@ namespace gtsam { // used as soft prior /* ************************************************************************* */ Vector hPose (const Vector& x) { - Pose3 pose(x); // transform from vector to Pose3 - Vector w = pose.rotation().ypr(); // get angle differences + Pose3 pose(x); // transform from vector to Pose3 + Vector w = pose.rotation().ypr(); // get angle differences Vector d = pose.translation().vector(); // get translation differences return concatVectors(2,&w,&d); } diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 92a15484e..81af053d8 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -12,22 +12,38 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + // static member functions to construct rotations + + Rot3 Rot3::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); + } + + Rot3 Rot3::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); + } + + Rot3 Rot3::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); + } + /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); } - - /* ************************************************************************* */ - // Vector Rot3::vector() const { - // double r[] = { r1_.x(), r1_.y(), r1_.z(), - // r2_.x(), r2_.y(), r2_.z(), - // r3_.x(), r3_.y(), r3_.z() }; - // Vector v(9); - // copy(r,r+9,v.begin()); - // return v; - // } - /* ************************************************************************* */ Matrix Rot3::matrix() const { double r[] = { r1_.x(), r2_.x(), r3_.x(), @@ -55,12 +71,17 @@ namespace gtsam { } /* ************************************************************************* */ - Vector Rot3::ypr() const { + Vector Rot3::xyz() const { Matrix I;Vector q; boost::tie(I,q)=RQ(matrix()); return q; } + Vector Rot3::ypr() const { + Vector q = xyz(); + return Vector_(3,q(2),q(1),q(0)); + } + /* ************************************************************************* */ Rot3 rodriguez(const Vector& n, double t) { double n0 = n(0), n1=n(1), n2=n(2); @@ -149,41 +170,23 @@ namespace gtsam { } /* ************************************************************************* */ - pair RQ(const Matrix& A) { - double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22); - double Cx = A22 / a; //cosX - double Sx = -A21 / a; //sinX - Matrix Qx = Matrix_(3, 3, - 1.0, 0.0, 0.0, - 0.0, Cx, -Sx, - 0.0, Sx, Cx); - Matrix B = A * Qx; + pair RQ(const Matrix& A) { - double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22); - double Cy = B22 / b; //cosY - double Sy = B20 / b; //sinY - Matrix Qy = Matrix_(3,3, - Cy, 0.0, Sy, - 0.0, 1.0, 0.0, - -Sy, 0.0, Cy); - Matrix C = B * Qy; + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3 Qx = Rot3::Rx(-x); + Matrix B = A * Qx.matrix(); - double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11); - double Cz = C11 / c; //cosZ - double Sz = -C10 / c; //sinZ - Matrix Qz = Matrix_(3, 3, - Cz, -Sz, 0.0, - Sz, Cz, 0.0, - 0.0, 0.0, 1.0); - Matrix R = C * Qz; + double y = -atan2(B(2, 0), B(2, 2)); + Rot3 Qy = Rot3::Ry(-y); + Matrix C = B * Qy.matrix(); - Vector angles(3); - angles(0) = -atan2(Sx, Cx); - angles(1) = -atan2(Sy, Cy); - angles(2) = -atan2(Sz, Cz); + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3 Qz = Rot3::Rz(-z); + Matrix R = C * Qz.matrix(); - return make_pair(R,angles); - } + Vector xyz = Vector_(3, x, y, z); + return make_pair(R, xyz); + } /* ************************************************************************* */ diff --git a/cpp/Rot3.h b/cpp/Rot3.h index b8857116d..4675b4187 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -54,6 +54,29 @@ namespace gtsam { r2_(Point3(R(0,1), R(1,1), R(2,1))), r3_(Point3(R(0,2), R(1,2), R(2,2))) {} + /** Static member function to generate some well known rotations */ + + /** + * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix + * Counterclockwise when looking from unchanging axis. + */ + 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);} + + /** + * 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);} + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} @@ -72,7 +95,16 @@ namespace gtsam { Point3 r2() const { return r2_; } Point3 r3() const { return r3_; } - /** use RQ to calculate yaw-pitch-roll angle representation */ + /** + * Use RQ to calculate xyz angle representation + * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) + */ + Vector xyz() const; + + /** + * Use RQ to calculate yaw-pitch-roll angle representation + * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + */ Vector ypr() const; private: diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 66ecff3bd..b74941e1a 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -103,6 +103,40 @@ TEST( Rot3, expmap) CHECK(assert_equal(expmap(R,v), R)); } +/* ************************************************************************* */ +TEST(Rot3, log) +{ + Vector w1 = Vector_(3, 0.1, 0.0, 0.0); + Rot3 R1 = rodriguez(w1); + CHECK(assert_equal(w1, logmap(R1))); + + Vector w2 = Vector_(3, 0.0, 0.1, 0.0); + Rot3 R2 = rodriguez(w2); + CHECK(assert_equal(w2, logmap(R2))); + + Vector w3 = Vector_(3, 0.0, 0.0, 0.1); + Rot3 R3 = rodriguez(w3); + CHECK(assert_equal(w3, logmap(R3))); + + Vector w = Vector_(3, 0.1, 0.4, 0.2); + Rot3 R = rodriguez(w); + CHECK(assert_equal(w, logmap(R))); +} + +/* ************************************************************************* */ + TEST(Rot3, manifold) +{ + Rot3 t1 = rodriguez(0.1, 0.4, 0.2); + Rot3 t2 = rodriguez(0.3, 0.1, 0.7); + Rot3 origin; + 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)); +} + /* ************************************************************************* */ // rotate derivatives @@ -128,8 +162,6 @@ TEST( Rot3, Drotate2_DNrotate2) } /* ************************************************************************* */ -// unrotate - TEST( Rot3, unrotate) { Point3 w = R*P; @@ -175,6 +207,11 @@ TEST( Rot3, compose ) /* ************************************************************************* */ TEST( Rot3, between ) { + Rot3 R = rodriguez(0.1, 0.4, 0.2); + Rot3 origin; + CHECK(assert_equal(R, between(origin,R))); + CHECK(assert_equal(inverse(R), between(R,origin))); + Rot3 R1 = rodriguez(0.1, 0.2, 0.3); Rot3 R2 = rodriguez(0.2, 0.3, 0.5); @@ -191,6 +228,71 @@ TEST( Rot3, between ) CHECK(assert_equal(numericalH2,actualH2)); } +/* ************************************************************************* */ +TEST( Rot3, xyz ) +{ + double t = 0.1, st = sin(t), ct = cos(t); + + 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); + CHECK(assert_equal(expected2,Rot3::Ry(t))); + + // yaw is around z axis + Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + CHECK(assert_equal(expected3,Rot3::Rz(t))); +} + +/* ************************************************************************* */ +TEST( Rot3, yaw_pitch_roll ) +{ + double t = 0.1; + + // yaw is around z axis + CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); + + // pitch is around y axis + CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); + + // roll is around x axis + CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); +} + +/* ************************************************************************* */ +TEST( Rot3, RQ) +{ + // Try RQ on a pure rotation + Matrix actualK; Vector actual; + boost::tie(actualK,actual) = RQ(R.matrix()); + Vector expected = Vector_(3,0.14715, 0.385821, 0.231671); + CHECK(assert_equal(eye(3),actualK)); + CHECK(assert_equal(expected,actual,1e-6)); + + // Try using xyz call + CHECK(assert_equal(expected,R.xyz(),1e-6)); + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz(),1e-6)); + + // Try using xyz call + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr(),1e-6)); + + // Try ypr for pure yaw-pitch-roll matrices + CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr(),1e-6)); + CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr(),1e-6)); + CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr(),1e-6)); + + // Try RQ to recover calibration from 3*3 sub-block of projection matrix + Matrix K = Matrix_(3,3, + 500.0, 0.0, 320.0, + 0.0, 500.0, 240.0, + 0.0, 0.0, 1.0 + ); + Matrix A = K*R.matrix(); + boost::tie(actualK,actual) = RQ(A); + CHECK(assert_equal(K,actualK)); + CHECK(assert_equal(expected,actual,1e-6)); +} + /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */