All tests for SO3 now uncommented

gtsam/geometry/SOn-inl.h

@@ -115,7 +115,7 @@ typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
 
 template <int N>
 typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
-  throw std::runtime_error("SO<N>::Logmap only implemented for SO3 and SO4.");
+  throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
 }
 
 template <int N>

gtsam/geometry/SOt.cpp

@@ -182,9 +182,10 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
   }
 }
 
-// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
+template <>
-//   return sot::DexpFunctor(omega).dexp();
+Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
-// }
+  return sot::DexpFunctor(omega).dexp();
+}
 
 //******************************************************************************
 /* Right Jacobian for Log map in SO(3) - equation (10.86) and following
@@ -254,19 +255,22 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }
 
-  if (H) *H = SO3::LogmapDerivative(omega);
+  if (H) *H = LogmapDerivative(omega);
   return omega;
 }
 
 //******************************************************************************
+// local vectorize
 static Vector9 vec3(const Matrix3& R) {
   return Eigen::Map<const Vector9>(R.data());
 }
 
+// so<3> generators
 static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
                                             SO3::Hat(Vector3::Unit(1)),
                                             SO3::Hat(Vector3::Unit(2))});
 
+// vectorized generators
 static const Matrix93 P3 =
     (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
 

gtsam/geometry/SOt.h

@@ -42,6 +42,15 @@ using SO3 = SO<3>;
 //   static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
 //   static Vector3 Vee(const Matrix3 &X);  ///< inverse of Hat
 
+// Below are all declarations of SO<3> specializations.
+// They are *defined* in SO3.cpp.
+
+/// Adjoint map
+template <>
+Matrix3 SO3::AdjointMap() const {
+  return matrix_;
+}
+
 /**
  * Exponential map at identity - create a rotation from canonical coordinates
  * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
@@ -49,13 +58,9 @@ using SO3 = SO<3>;
 template <>
 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
 
-// template<>
-// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
-//   return sot::DexpFunctor(omega).dexp();
-// }
-
 /// Derivative of Expmap
-//   static Matrix3 ExpmapDerivative(const Vector3& omega);
+template <>
+Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
 
 /**
  * Log map at identity - returns the canonical coordinates
@@ -64,10 +69,9 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
 template <>
 Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
 
+/// Derivative of Logmap
 template <>
-Matrix3 SO3::AdjointMap() const {
+Matrix3 SO3::LogmapDerivative(const Vector3& omega);
-  return matrix_;
-}
 
 // Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
 template <>

gtsam/geometry/tests/testSO3.cpp

@@ -157,7 +157,7 @@ TEST(SO3, ExpmapDerivative) {
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative2) {
   const Vector3 theta(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
@@ -168,7 +168,7 @@ TEST(SO3, ExpmapDerivative2) {
                      SO3::ExpmapDerivative(-theta)));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative3) {
   const Vector3 theta(10, 20, 30);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
@@ -179,7 +179,7 @@ TEST(SO3, ExpmapDerivative3) {
                      SO3::ExpmapDerivative(-theta)));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative4) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
@@ -207,7 +207,7 @@ TEST(SO3, ExpmapDerivative4) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative5) {
   auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
   auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
@@ -223,7 +223,7 @@ TEST(SO3, ExpmapDerivative5) {
   }
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, ExpmapDerivative6) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
@@ -233,7 +233,7 @@ TEST(SO3, ExpmapDerivative6) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, LogmapDerivative) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation
@@ -243,7 +243,7 @@ TEST(SO3, LogmapDerivative) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
-/* ************************************************************************* */
+//******************************************************************************
 TEST(SO3, JacobianLogmap) {
   const Vector3 thetahat(0.1, 0, 0.1);
   const SO3 R = SO3::Expmap(thetahat);  // some rotation