Rescued some specializations for SO3

gtsam/geometry/SOt.cpp

@@ -133,12 +133,12 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 
 }  // namespace sot
 
-/* ************************************************************************* */
+//******************************************************************************
 // SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
 //   return sot::ExpmapFunctor(axis, theta).expmap();
 // }
 
-/* ************************************************************************* */
+//******************************************************************************
 // SO3 SO3::ClosestTo(const Matrix3& M) {
 //   Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU |
 //   Eigen::ComputeFullV); const auto& U = svd.matrixU(); const auto& V =
@@ -146,7 +146,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 //   U * Vector3(1, 1, det).asDiagonal() * V.transpose();
 // }
 
-/* ************************************************************************* */
+//******************************************************************************
 // SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
 //   //  See Hartley13ijcv:
 //   //  Cost function C(R) = \sum sqr(|R-R_i|_F)
@@ -170,6 +170,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 //   return xi;
 // }
 
+//******************************************************************************
 template <>
 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
   if (H) {
@@ -185,97 +186,101 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
 //   return sot::DexpFunctor(omega).dexp();
 // }
 
-/* ************************************************************************* */
+//******************************************************************************
-// Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
+/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
-//   using std::sin;
+ equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
-//   using std::sqrt;
+ Groups", Volume 2, 2008.
 
-//   // note switch to base 1
+   logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
-//   const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
-//   const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
-//   const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
 
-//   // Get trace(R)
+ where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
-//   const double tr = R.trace();
+ manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
+ omega)
+ */
+template <>
+Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
+  using std::cos;
+  using std::sin;
 
-//   Vector3 omega;
+  double theta2 = omega.dot(omega);
+  if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
+  double theta = std::sqrt(theta2);  // rotation angle
 
-//   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
+  // element of Lie algebra so(3): W = omega^
-//   // we do something special
+  const Matrix3 W = Hat(omega);
-//   if (std::abs(tr + 1.0) < 1e-10) {
+  return I_3x3 + 0.5 * W +
-//     if (std::abs(R33 + 1.0) > 1e-10)
+         (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
-//       omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
+             W * W;
-//     else if (std::abs(R22 + 1.0) > 1e-10)
+}
-//       omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
-//     else
-//       // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
-//       omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
-//   } else {
-//     double magnitude;
-//     const double tr_3 = tr - 3.0;  // always negative
-//     if (tr_3 < -1e-7) {
-//       double theta = acos((tr - 1.0) / 2.0);
-//       magnitude = theta / (2.0 * sin(theta));
-//     } else {
-//       // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
-//       // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
-//       magnitude = 0.5 - tr_3 * tr_3 / 12.0;
-//     }
-//     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
-//   }
 
-//   if (H) *H = LogmapDerivative(omega);
+//******************************************************************************
-//   return omega;
+template <>
-// }
+Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
+  using std::sin;
+  using std::sqrt;
 
-/* ************************************************************************* */
+  // note switch to base 1
-// Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
+  const Matrix3& R = Q.matrix();
-//   using std::cos;
+  const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
-//   using std::sin;
+  const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
+  const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
 
-//   double theta2 = omega.dot(omega);
+  // Get trace(R)
-//   if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
+  const double tr = R.trace();
-//   double theta = std::sqrt(theta2);  // rotation angle
-//   /** Right Jacobian for Log map in SO(3) - equation (10.86) and following
-//    * equations in G.S. Chirikjian, "Stochastic Models, Information Theory,
-//    and
-//    * Lie Groups", Volume 2, 2008. logmap( Rhat * expmap(omega) ) \approx
-//    logmap(
-//    * Rhat ) + Jrinv * omega where Jrinv = LogmapDerivative(omega); This maps
-//    a
-//    * perturbation on the manifold (expmap(omega)) to a perturbation in the
-//    * tangent space (Jrinv * omega)
-//    */
-//   const Matrix3 W =
-//       skewSymmetric(omega);  // element of Lie algebra so(3): W = omega^
-//   return I_3x3 + 0.5 * W +
-//          (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta)))
-//          *
-//              W * W;
-// }
 
-/* ************************************************************************* */
+  Vector3 omega;
-// static Vector9 vec(const SO3& R) { return Eigen::Map<const
-// Vector9>(R.data()); }
 
-// static const std::vector<const Matrix3> G({SO3::Hat(Vector3::Unit(0)),
+  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
-//                                            SO3::Hat(Vector3::Unit(1)),
+  // we do something special
-//                                            SO3::Hat(Vector3::Unit(2))});
+  if (std::abs(tr + 1.0) < 1e-10) {
+    if (std::abs(R33 + 1.0) > 1e-10)
+      omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
+    else if (std::abs(R22 + 1.0) > 1e-10)
+      omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
+    else
+      // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
+      omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
+  } else {
+    double magnitude;
+    const double tr_3 = tr - 3.0;  // always negative
+    if (tr_3 < -1e-7) {
+      double theta = acos((tr - 1.0) / 2.0);
+      magnitude = theta / (2.0 * sin(theta));
+    } else {
+      // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
+      // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
+      magnitude = 0.5 - tr_3 * tr_3 / 12.0;
+    }
+    omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
+  }
 
-// static const Matrix93 P =
+  if (H) *H = SO3::LogmapDerivative(omega);
-//     (Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished();
+  return omega;
+}
 
-/* ************************************************************************* */
+//******************************************************************************
-// Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
+static Vector9 vec3(const Matrix3& R) {
-//   const SO3& R = *this;
+  return Eigen::Map<const Vector9>(R.data());
-//   if (H) {
+}
-//     // As Luca calculated (for SO4), this is (I3 \oplus R) * P
-//     *H << R * P.block<3, 3>(0, 0), R * P.block<3, 3>(3, 0),
-//         R * P.block<3, 3>(6, 0);
-//   }
-//   return gtsam::vec(R);
-// };
 
-/* ************************************************************************* */
+static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
+                                            SO3::Hat(Vector3::Unit(1)),
+                                            SO3::Hat(Vector3::Unit(2))});
+
+static const Matrix93 P3 =
+    (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
+
+//******************************************************************************
+template <>
+Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
+  const Matrix3& R = matrix_;
+  if (H) {
+    // As Luca calculated (for SO4), this is (I3 \oplus R) * P3
+    *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
+        R * P3.block<3, 3>(6, 0);
+  }
+  return gtsam::vec3(R);
+}
+//******************************************************************************
 
 }  // end namespace gtsam

gtsam/geometry/SOt.h

@@ -42,7 +42,10 @@ using SO3 = SO<3>;
 //   static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
 //   static Vector3 Vee(const Matrix3 &X);  ///< inverse of Hat
 
-//******************************************************************************
+/**
+ * Exponential map at identity - create a rotation from canonical coordinates
+ * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
+ */
 template <>
 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
 
@@ -51,12 +54,6 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
 //   return sot::DexpFunctor(omega).dexp();
 // }
 
-/**
- * Exponential map at identity - create a rotation from canonical coordinates
- * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
- */
-//   static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
-
 /// Derivative of Expmap
 //   static Matrix3 ExpmapDerivative(const Vector3& omega);
 
@@ -64,49 +61,28 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
  * Log map at identity - returns the canonical coordinates
  * \f$ [R_x,R_y,R_z] \f$ of this rotation
  */
-//   static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
-
-/// Derivative of Logmap
-//   static Matrix3 LogmapDerivative(const Vector3& omega);
-
-//   Matrix3 AdjointMap() const {
-//     return *this;
-//   }
-
-//   // Chart at origin
-//   struct ChartAtOrigin {
-//     static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
-//       return Expmap(omega, H);
-//     }
-//     static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
-//       return Logmap(R, H);
-//     }
-//   };
-
-//******************************************************************************
-static Vector9 vec3(const Matrix3& R) {
-  return Eigen::Map<const Vector9>(R.data());
-}
-
-static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
-                                            SO3::Hat(Vector3::Unit(1)),
-                                            SO3::Hat(Vector3::Unit(2))});
-
-static const Matrix93 P3 =
-    (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
-
-//******************************************************************************
 template <>
-Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
+Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
-  const Matrix3& R = matrix_;
+
-  if (H) {
+template <>
-    // As Luca calculated (for SO4), this is (I3 \oplus R) * P3
+Matrix3 SO3::AdjointMap() const {
-    *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
+  return matrix_;
-        R * P3.block<3, 3>(6, 0);
-  }
-  return gtsam::vec3(R);
 }
 
+// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
+template <>
+SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
+  return Expmap(omega, H);
+}
+
+template <>
+Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
+  return Logmap(R, H);
+}
+
+template <>
+Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
+
 //   private:
 
 //     /** Serialization function */