Merged in fix/ExpmapFunctor (pull request #420)

fix bug in ExpmapFunctor caused by square root of theta2

Approved-by: Frank Dellaert <dellaert@cc.gatech.edu>

gtsam/geometry/SO3.cpp

@@ -31,7 +31,6 @@ namespace so3 {
 void ExpmapFunctor::init(bool nearZeroApprox) {
   nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
-    theta = std::sqrt(theta2);  // rotation angle
     sin_theta = std::sin(theta);
     const double s2 = std::sin(theta / 2.0);
     one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
@@ -39,7 +38,7 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
-    : theta2(omega.dot(omega)) {
+    : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
   const double wx = omega.x(), wy = omega.y(), wz = omega.z();
   W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
   init(nearZeroApprox);
@@ -50,7 +49,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
-    : theta2(angle * angle) {
+    : theta2(angle * angle), theta(angle) {
   const double ax = axis.x(), ay = axis.y(), az = axis.z();
   K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
   W = K * angle;

gtsam/geometry/tests/testRot3.cpp

@@ -103,6 +103,20 @@ Rot3 slow_but_correct_Rodrigues(const Vector& w) {
   return Rot3(R);
 }
 
+/* ************************************************************************* */
+TEST( Rot3, AxisAngle)
+{
+  Vector axis = Vector3(0., 1., 0.); // rotation around Y
+  double angle = 3.14 / 4.0;
+  Rot3 expected(0.707388, 0, 0.706825,
+                       0, 1,        0,
+               -0.706825, 0, 0.707388);
+  Rot3 actual = Rot3::AxisAngle(axis, angle);
+  CHECK(assert_equal(expected,actual,1e-5));
+  Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
+  CHECK(assert_equal(expected,actual2,1e-5));
+}
+
 /* ************************************************************************* */
 TEST( Rot3, Rodrigues)
 {

gtsam/geometry/tests/testSO3.cpp

@@ -82,6 +82,34 @@ TEST(SO3, ChartDerivatives) {
   CHECK_CHART_DERIVATIVES(R2, R1);
 }
 
+/* ************************************************************************* */
+TEST(SO3, Expmap) {
+  Vector axis = Vector3(0., 1., 0.);  // rotation around Y
+  double angle = 3.14 / 4.0;
+  Matrix expected(3,3);
+  expected << 0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388;
+
+  // axis angle version
+  so3::ExpmapFunctor f1(axis, angle);
+  SO3 actual1 = f1.expmap();
+  CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
+
+  // axis angle version, negative angle
+  so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
+  SO3 actual2 = f2.expmap();
+  CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
+
+  // omega version
+  so3::ExpmapFunctor f3(axis * angle);
+  SO3 actual3 = f3.expmap();
+  CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
+
+  // omega version, negative angle
+  so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
+  SO3 actual4 = f4.expmap();
+  CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
+}
+
 /* ************************************************************************* */
 namespace exmap_derivative {
 static const Vector3 w(0.1, 0.27, -0.2);