diff --git a/cpp/Lie.h b/cpp/Lie.h index 6d6f52623..5aafb70d1 100644 --- a/cpp/Lie.h +++ b/cpp/Lie.h @@ -104,4 +104,19 @@ namespace gtsam { inline Vector logmap(const Vector& p) { return p;} inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;} + /** + * Three term approximation of the BakerÐCampbellÐHausdorff formula + * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) + * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH + * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 + * http://en.wikipedia.org/wiki/BakerÐCampbellÐHausdorff_formula + */ + template + T BCH(const T& X, const T& Y) { + static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; + T X_Y = bracket(X, Y); + return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, + bracket(X, X_Y)); + } + } // namespace gtsam diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index ffb202a90..ba588c84a 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -12,96 +12,106 @@ using namespace gtsam; -Rot3 R = rodriguez(0.1,0.4,0.2); -Point3 P(0.2,0.7,-2.0); -double error = 1e-9, epsilon=0.001; +Rot3 R = rodriguez(0.1, 0.4, 0.2); +Point3 P(0.2, 0.7, -2.0); +double error = 1e-9, epsilon = 0.001; /* ************************************************************************* */ -TEST( Rot3, constructor) { - Rot3 expected(eye(3,3)); - Vector r1(3), r2(3), r3(3); - r1(0)=1;r1(1)=0;r1(2)=0; - r2(0)=0;r2(1)=1;r2(2)=0; - r3(0)=0;r3(1)=0;r3(2)=1; - Rot3 actual(r1,r2,r3); - CHECK(assert_equal(actual,expected)); +TEST( Rot3, constructor) +{ + Rot3 expected(eye(3, 3)); + Vector r1(3), r2(3), r3(3); + r1(0) = 1; + r1(1) = 0; + r1(2) = 0; + r2(0) = 0; + r2(1) = 1; + r2(2) = 0; + r3(0) = 0; + r3(1) = 0; + r3(2) = 1; + Rot3 actual(r1, r2, r3); + CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3, constructor2) { - Matrix R = Matrix_(3,3, - 11.,12.,13., - 21.,22.,23., - 31.,32.,33.); - Rot3 actual(R); - Rot3 expected(11,12,13, - 21,22,23, - 31,32,33); - CHECK(assert_equal(actual,expected)); +TEST( Rot3, constructor2) +{ + Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.); + Rot3 actual(R); + Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); + CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3, constructor3) { - Rot3 expected(1,2,3,4,5,6,7,8,9); - Point3 r1(1,4,7), r2(2,5,8), r3(3,6,9); - CHECK(assert_equal(Rot3(r1,r2,r3),expected)); +TEST( Rot3, constructor3) +{ + Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9); + Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9); + CHECK(assert_equal(Rot3(r1,r2,r3),expected)); } /* ************************************************************************* */ -TEST( Rot3, transpose) { - Rot3 R(1,2,3,4,5,6,7,8,9); - Point3 r1(1,2,3), r2(4,5,6), r3(7,8,9); - CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3))); +TEST( Rot3, transpose) +{ + Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9); + Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9); + CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3))); } /* ************************************************************************* */ -TEST( Rot3, equals) { - CHECK(R.equals(R)); - Rot3 zero; - CHECK(!R.equals(zero)); +TEST( Rot3, equals) +{ + CHECK(R.equals(R)); + Rot3 zero; + CHECK(!R.equals(zero)); } /* ************************************************************************* */ Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); - Matrix J = skewSymmetric(w/t); + Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); Matrix R = eye(3, 3) + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; // matrix constructor will be tripped } /* ************************************************************************* */ -TEST( Rot3, rodriguez) { - Rot3 R1 = rodriguez(epsilon, 0, 0); - Vector w = Vector_(3,epsilon,0.,0.); - Rot3 R2 = slow_but_correct_rodriguez(w); - CHECK(assert_equal(R1,R2)); +TEST( Rot3, rodriguez) +{ + Rot3 R1 = rodriguez(epsilon, 0, 0); + Vector w = Vector_(3, epsilon, 0., 0.); + Rot3 R2 = slow_but_correct_rodriguez(w); + CHECK(assert_equal(R1,R2)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez2) { - Vector v(3); v(0) = 0; v(1) = 1; v(2) = 0; - Rot3 R1 = rodriguez(v, 3.14/4.0); - Rot3 R2(0.707388,0,0.706825, - 0,1,0, - -0.706825,0,0.707388); - CHECK(assert_equal(R1,R2,1e-5)); +TEST( Rot3, rodriguez2) +{ + Vector v(3); + v(0) = 0; + v(1) = 1; + v(2) = 0; + Rot3 R1 = rodriguez(v, 3.14 / 4.0); + Rot3 R2(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); + CHECK(assert_equal(R1,R2,1e-5)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez3) { - Vector w = Vector_(3,0.1,0.2,0.3); - Rot3 R1 = rodriguez(w/norm_2(w), norm_2(w)); - Rot3 R2 = slow_but_correct_rodriguez(w); - CHECK(assert_equal(R1,R2)); +TEST( Rot3, rodriguez3) +{ + Vector w = Vector_(3, 0.1, 0.2, 0.3); + Rot3 R1 = rodriguez(w / norm_2(w), norm_2(w)); + Rot3 R2 = slow_but_correct_rodriguez(w); + CHECK(assert_equal(R1,R2)); } /* ************************************************************************* */ TEST( Rot3, expmap) { - Vector v(3); - fill(v.begin(), v.end(), 0); - CHECK(assert_equal(expmap(R,v), R)); + Vector v(3); + fill(v.begin(), v.end(), 0); + CHECK(assert_equal(expmap(R,v), R)); } /* ************************************************************************* */ @@ -131,17 +141,17 @@ TEST(Rot3, log) Rot3 R6 = rodriguez(w6); CHECK(assert_equal(w6, logmap(R6))); - Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); - Rot3 R7 = rodriguez(w7); - CHECK(assert_equal(w7, logmap(R7))); + Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); + Rot3 R7 = rodriguez(w7); + CHECK(assert_equal(w7, logmap(R7))); - Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); - Rot3 R8 = rodriguez(w8); - CHECK(assert_equal(w8, logmap(R8))); + Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); + Rot3 R8 = rodriguez(w8); + CHECK(assert_equal(w8, logmap(R8))); } /* ************************************************************************* */ - TEST(Rot3, manifold) +TEST(Rot3, manifold) { Rot3 gR1 = rodriguez(0.1, 0.4, 0.2); Rot3 gR2 = rodriguez(0.3, 0.1, 0.7); @@ -159,46 +169,74 @@ TEST(Rot3, log) 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); + 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); + 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)); } +/* ************************************************************************* */ +class AngularVelocity: public Point3 { +public: + AngularVelocity(const Point3& p) : + Point3(p) { + } + AngularVelocity(double wx, double wy, double wz) : + Point3(wx, wy, wz) { + } +}; + +AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { + return cross(X, Y); +} + +/* ************************************************************************* */ +TEST(Rot3, BCH) +{ + // Approximate exmap by BCH formula + AngularVelocity w1(0.2, -0.1, 0.1); + AngularVelocity w2(0.01, 0.02, -0.03); + Rot3 R1 = expmap (w1.vector()), R2 = expmap (w2.vector()); + Rot3 R3 = R1 * R2; + Vector expected = logmap(R3); + Vector actual = BCH(w1, w2).vector(); + CHECK(assert_equal(expected, actual,1e-5)); +} + /* ************************************************************************* */ // rotate derivatives TEST( Rot3, Drotate1) { - Matrix computed = Drotate1(R, P); - Matrix numerical = numericalDerivative21(rotate,R,P); - CHECK(assert_equal(numerical,computed,error)); + Matrix computed = Drotate1(R, P); + Matrix numerical = numericalDerivative21(rotate, R, P); + CHECK(assert_equal(numerical,computed,error)); } TEST( Rot3, Drotate1_) - { - Matrix computed = Drotate1(inverse(R), P); - Matrix numerical = numericalDerivative21(rotate,inverse(R),P); - CHECK(assert_equal(numerical,computed,error)); +{ + Matrix computed = Drotate1(inverse(R), P); + Matrix numerical = numericalDerivative21(rotate, inverse(R), P); + CHECK(assert_equal(numerical,computed,error)); } TEST( Rot3, Drotate2_DNrotate2) { - Matrix computed = Drotate2(R); - Matrix numerical = numericalDerivative22(rotate,R,P); - CHECK(assert_equal(numerical,computed,error)); + Matrix computed = Drotate2(R); + Matrix numerical = numericalDerivative22(rotate, R, P); + CHECK(assert_equal(numerical,computed,error)); } /* ************************************************************************* */ TEST( Rot3, unrotate) { - Point3 w = R*P; - CHECK(assert_equal(unrotate(R,w),P)); + Point3 w = R * P; + CHECK(assert_equal(unrotate(R,w),P)); } /* ************************************************************************* */ @@ -206,16 +244,16 @@ TEST( Rot3, unrotate) TEST( Rot3, Dunrotate1) { - Matrix computed = Dunrotate1(R, P); - Matrix numerical = numericalDerivative21(unrotate,R,P); - CHECK(assert_equal(numerical,computed,error)); + Matrix computed = Dunrotate1(R, P); + Matrix numerical = numericalDerivative21(unrotate, R, P); + CHECK(assert_equal(numerical,computed,error)); } TEST( Rot3, Dunrotate2_DNunrotate2) { - Matrix computed = Dunrotate2(R); - Matrix numerical = numericalDerivative22(unrotate,R,P); - CHECK(assert_equal(numerical,computed,error)); + Matrix computed = Dunrotate2(R); + Matrix numerical = numericalDerivative22(unrotate, R, P); + CHECK(assert_equal(numerical,computed,error)); } /* ************************************************************************* */ @@ -224,16 +262,18 @@ TEST( Rot3, compose ) Rot3 R1 = rodriguez(0.1, 0.2, 0.3); Rot3 R2 = rodriguez(0.2, 0.3, 0.5); - Rot3 expected = R1*R2; + Rot3 expected = R1 * R2; Rot3 actual = compose(R1, R2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(compose, R1, R2, 1e-5); + Matrix numericalH1 = numericalDerivative21 (compose, R1, + R2, 1e-5); Matrix actualH1 = Dcompose1(R1, R2); CHECK(assert_equal(numericalH1,actualH1)); Matrix actualH2 = Dcompose2(R1, R2); - Matrix numericalH2 = numericalDerivative22(compose, R1, R2, 1e-5); + Matrix numericalH2 = numericalDerivative22 (compose, R1, + R2, 1e-5); CHECK(assert_equal(numericalH2,actualH2)); } @@ -247,7 +287,7 @@ TEST( Rot3, inverse ) CHECK(assert_equal(I,R*inverse(R))); CHECK(assert_equal(I,inverse(R)*R)); - Matrix numericalH = numericalDerivative11(inverse, R, 1e-5); + Matrix numericalH = numericalDerivative11 (inverse, R, 1e-5); Matrix actualH = Dinverse(R); CHECK(assert_equal(numericalH,actualH)); } @@ -263,7 +303,7 @@ TEST( Rot3, between ) Rot3 R1 = rodriguez(0.1, 0.2, 0.3); Rot3 R2 = rodriguez(0.2, 0.3, 0.5); - Rot3 expected = inverse(R1)*R2; + Rot3 expected = inverse(R1) * R2; Rot3 actual = between(R1, R2); CHECK(assert_equal(expected,actual)); @@ -287,32 +327,23 @@ TEST( Rot3, xyz ) // z // | * Y=(ct,st) // x----y - Rot3 expected1( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); + Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); CHECK(assert_equal(expected1,Rot3::Rx(t))); // x // | * Z=(ct,st) // y----z - Rot3 expected2( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); + Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); CHECK(assert_equal(expected2,Rot3::Ry(t))); // y // | X=* (ct,st) // z----x - Rot3 expected3( - ct, -st, 0, - st, ct, 0, - 0, 0, 1); + 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); + 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))); } @@ -331,7 +362,7 @@ TEST( Rot3, yaw_pitch_roll ) 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); + 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))); } @@ -339,37 +370,37 @@ TEST( Rot3, yaw_pitch_roll ) 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)); + 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, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] - 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())); + // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] + 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())); - // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + // Try ypr for pure yaw-pitch-roll matrices + CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); - // 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)); + // 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); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */