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 + ...
See http://en.wikipedia.org/wiki/Baker–Campbell–Hausdorff_formula

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<class T>
+  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

cpp/testRot3.cpp

@@ -17,45 +17,51 @@ Point3 P(0.2,0.7,-2.0);
 double error = 1e-9, epsilon = 0.001;
 
 /* ************************************************************************* */
-TEST( Rot3, constructor) {
+TEST( Rot3, constructor)
+{
 	Rot3 expected(eye(3, 3));
 	Vector r1(3), r2(3), r3(3);
-  r1(0)=1;r1(1)=0;r1(2)=0;
+	r1(0) = 1;
-  r2(0)=0;r2(1)=1;r2(2)=0;
+	r1(1) = 0;
-  r3(0)=0;r3(1)=0;r3(2)=1;
+	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) {
+TEST( Rot3, constructor2)
-  Matrix R = Matrix_(3,3,
+{
-		       11.,12.,13.,
+	Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
-		       21.,22.,23.,
-		       31.,32.,33.);
 	Rot3 actual(R);
-  Rot3 expected(11,12,13,
+	Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
-		21,22,23,
-		31,32,33);
 	CHECK(assert_equal(actual,expected));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, constructor3) {
+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) {
+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) {
+TEST( Rot3, equals)
+{
 	CHECK(R.equals(R));
 	Rot3 zero;
 	CHECK(!R.equals(zero));
@@ -71,7 +77,8 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
 }
 
 /* ************************************************************************* */
-TEST( Rot3, rodriguez) {
+TEST( Rot3, rodriguez)
+{
 	Rot3 R1 = rodriguez(epsilon, 0, 0);
 	Vector w = Vector_(3, epsilon, 0., 0.);
 	Rot3 R2 = slow_but_correct_rodriguez(w);
@@ -79,17 +86,20 @@ TEST( Rot3, rodriguez) {
 }
 
 /* ************************************************************************* */
-TEST( Rot3, rodriguez2) {
+TEST( Rot3, rodriguez2)
-  Vector v(3); v(0) = 0; v(1) = 1; v(2) = 0;
+{
+	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,
+	Rot3 R2(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388);
-  		0,1,0,
-  		-0.706825,0,0.707388);
 	CHECK(assert_equal(R1,R2,1e-5));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, rodriguez3) {
+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);
@@ -170,6 +180,34 @@ TEST(Rot3, log)
 	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<Rot3> (w1.vector()), R2 = expmap<Rot3> (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
 
@@ -228,12 +266,14 @@ TEST( Rot3, compose )
 	Rot3 actual = compose(R1, R2);
 	CHECK(assert_equal(expected,actual));
 
-	Matrix numericalH1 = numericalDerivative21<Rot3,Rot3,Rot3>(compose, R1, R2, 1e-5);
+	Matrix numericalH1 = numericalDerivative21<Rot3, Rot3, Rot3> (compose, R1,
+			R2, 1e-5);
 	Matrix actualH1 = Dcompose1(R1, R2);
 	CHECK(assert_equal(numericalH1,actualH1));
 
 	Matrix actualH2 = Dcompose2(R1, R2);
-	Matrix numericalH2 = numericalDerivative22<Rot3,Rot3,Rot3>(compose, R1, R2, 1e-5);
+	Matrix numericalH2 = numericalDerivative22<Rot3, Rot3, Rot3> (compose, R1,
+			R2, 1e-5);
 	CHECK(assert_equal(numericalH2,actualH2));
 }
 
@@ -287,28 +327,19 @@ TEST( Rot3, xyz )
 	// z
 	// |   * Y=(ct,st)
 	// x----y
-	Rot3 expected1(
+	Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
-			1,  0,  0,
-			0, ct,-st,
-			0, st, ct);
 	CHECK(assert_equal(expected1,Rot3::Rx(t)));
 
 	// x
 	// |   * Z=(ct,st)
 	// y----z
-	Rot3 expected2(
+	Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
-			 ct, 0, st,
-			  0, 1,  0,
-			-st, 0, ct);
 	CHECK(assert_equal(expected2,Rot3::Ry(t)));
 
 	// y
 	// |   X=* (ct,st)
 	// z----x
-	Rot3 expected3(
+	Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
-			ct, -st, 0,
-			st,  ct, 0,
-			 0,  0,  1);
 	CHECK(assert_equal(expected3,Rot3::Rz(t)));
 
 	// Check compound rotation
@@ -339,7 +370,8 @@ TEST( Rot3, yaw_pitch_roll )
 TEST( Rot3, RQ)
 {
 	// Try RQ on a pure rotation
-	Matrix actualK; Vector actual;
+	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));
@@ -358,11 +390,7 @@ TEST( Rot3, RQ)
 	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,
+	Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
-			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));
@@ -370,6 +398,9 @@ TEST( Rot3, RQ)
 }
 
 /* ************************************************************************* */
-int main(){ TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */