Special case for trace==1 in Rot3 logmap

cpp/Rot3.cpp

@@ -109,6 +109,32 @@ namespace gtsam {
     return Vector_(3,q(2),q(1),q(0));
   }
 
+  /* ************************************************************************* */
+  // Log map at identity - return the canonical coordinates of this rotation
+  inline Vector logmap(const Rot3& R) {
+    double tr = R.r1().x()+R.r2().y()+R.r3().z();
+    if (tr==3.0)         // when theta = 0, +-2pi, +-4pi, etc.
+      return zero(3);
+    else if (tr==-1.0) { // when theta = +-pi, +-3pi, +-5pi, etc.
+      if(R.r3().z() != -1.0)
+        return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
+        Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
+      else if(R.r2().y() != -1.0)
+        return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
+        Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
+      else if(R.r1().x() != -1.0)
+        return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
+        Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
+    } else {
+      double theta = acos((tr-1.0)/2.0);
+      return (theta/2.0/sin(theta))*Vector_(3,
+          R.r2().z()-R.r3().y(),
+          R.r3().x()-R.r1().z(),
+          R.r1().y()-R.r2().x());
+    }
+  }
+
+
   /* ************************************************************************* */
   Rot3 rodriguez(const Vector& n, double t) {
     double n0 = n(0), n1=n(1), n2=n(2);

cpp/Rot3.h

@@ -10,6 +10,7 @@
 
 #pragma once
 
+#include <boost/math/constants/constants.hpp>
 #include "Point3.h"
 #include "Testable.h"
 #include "Lie.h"
@@ -156,16 +157,7 @@ namespace gtsam {
   }
 
   // Log map at identity - return the canonical coordinates of this rotation
-  inline Vector logmap(const Rot3& R) {
+  Vector logmap(const Rot3& R);
-    double tr = R.r1().x()+R.r2().y()+R.r3().z();
-    if (tr==3.0) return zero(3);
-    if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");;
-    double theta = acos((tr-1.0)/2.0);
-    return (theta/2.0/sin(theta))*Vector_(3,
-        R.r2().z()-R.r3().y(),
-        R.r3().x()-R.r1().z(),
-        R.r1().y()-R.r2().x());
-  }
 
   // Compose two rotations
   inline Rot3 compose(const Rot3& R1, const Rot3& R2) {

cpp/testRot3.cpp

@@ -5,6 +5,7 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <boost/math/constants/constants.hpp>
 #include "numericalDerivative.h"
 #include "Point3.h"
 #include "Rot3.h"
@@ -126,9 +127,17 @@ TEST(Rot3, log)
 	Rot3 R5 = rodriguez(w5);
 	CHECK(assert_equal(w5, logmap(R5)));
 
-//	Vector w6 = Vector_(3, M_PI, 0.0, 0.0);
+	Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
-//	Rot3 R6 = rodriguez(w6);
+	Rot3 R6 = rodriguez(w6);
-//	CHECK(assert_equal(w6, logmap(R6)));
+	CHECK(assert_equal(w6, logmap(R6)));
+
+    Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
+    Rot3 R7 = rodriguez(w7);
+    CHECK(assert_equal(w7, logmap(R7)));
+
+    Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
+    Rot3 R8 = rodriguez(w8);
+    CHECK(assert_equal(w8, logmap(R8)));
 }
 
 /* ************************************************************************* */