Specialized Hat/Vee

gtsam/geometry/SOn-inl.h

@@ -22,62 +22,16 @@
 
 namespace gtsam {
 
+// Implementation for N>5 just uses dynamic version
 template <int N>
-Matrix SO<N>::Hat(const Vector& xi) {
+typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
-  size_t n = AmbientDim(xi.size());
+  return SOn::Hat(xi);
-  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
-
-  Matrix X(n, n);  // allocate space for n*n skew-symmetric matrix
-  X.setZero();
-  if (n == 2) {
-    // Handle SO(2) case as recursion bottom
-    assert(xi.size() == 1);
-    X << 0, -xi(0), xi(0), 0;
-  } else {
-    // Recursively call SO(n-1) call for top-left block
-    const size_t dmin = (n - 1) * (n - 2) / 2;
-    X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
-
-    // Now fill last row and column
-    double sign = 1.0;
-    for (size_t i = 0; i < n - 1; i++) {
-      const size_t j = n - 2 - i;
-      X(n - 1, j) = sign * xi(i);
-      X(j, n - 1) = -X(n - 1, j);
-      sign = -sign;
-    }
-  }
-  return X;
 }
 
+// Implementation for N>5 just uses dynamic version
 template <int N>
-Vector SO<N>::Vee(const Matrix& X) {
+typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
-  const size_t n = X.rows();
+  return SOn::Vee(X);
-  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
-
-  if (n == 2) {
-    // Handle SO(2) case as recursion bottom
-    Vector xi(1);
-    xi(0) = X(1, 0);
-    return xi;
-  } else {
-    // Calculate dimension and allocate space
-    const size_t d = n * (n - 1) / 2;
-    Vector xi(d);
-
-    // Fill first n-1 spots from last row of X
-    double sign = 1.0;
-    for (size_t i = 0; i < n - 1; i++) {
-      const size_t j = n - 2 - i;
-      xi(i) = sign * X(n - 1, j);
-      sign = -sign;
-    }
-
-    // Recursively call Vee to fill remainder of x
-    const size_t dmin = (n - 1) * (n - 2) / 2;
-    xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1));
-    return xi;
-  }
 }
 
 template <int N>

gtsam/geometry/SOn.cpp

@@ -20,6 +20,64 @@
 
 namespace gtsam {
 
+template <>
+Matrix SOn::Hat(const Vector& xi) {
+  size_t n = AmbientDim(xi.size());
+  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
+
+  Matrix X(n, n);  // allocate space for n*n skew-symmetric matrix
+  X.setZero();
+  if (n == 2) {
+    // Handle SO(2) case as recursion bottom
+    assert(xi.size() == 1);
+    X << 0, -xi(0), xi(0), 0;
+  } else {
+    // Recursively call SO(n-1) call for top-left block
+    const size_t dmin = (n - 1) * (n - 2) / 2;
+    X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
+
+    // Now fill last row and column
+    double sign = 1.0;
+    for (size_t i = 0; i < n - 1; i++) {
+      const size_t j = n - 2 - i;
+      X(n - 1, j) = sign * xi(i);
+      X(j, n - 1) = -X(n - 1, j);
+      sign = -sign;
+    }
+  }
+  return X;
+}
+
+template <>
+Vector SOn::Vee(const Matrix& X) {
+  const size_t n = X.rows();
+  if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
+
+  if (n == 2) {
+    // Handle SO(2) case as recursion bottom
+    Vector xi(1);
+    xi(0) = X(1, 0);
+    return xi;
+  } else {
+    // Calculate dimension and allocate space
+    const size_t d = n * (n - 1) / 2;
+    Vector xi(d);
+
+    // Fill first n-1 spots from last row of X
+    double sign = 1.0;
+    for (size_t i = 0; i < n - 1; i++) {
+      const size_t j = n - 2 - i;
+      xi(i) = sign * X(n - 1, j);
+      sign = -sign;
+    }
+
+    // Recursively call Vee to fill remainder of x
+    const size_t dmin = (n - 1) * (n - 2) / 2;
+    xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1));
+    return xi;
+  }
+}
+
 template <>
 SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
                                            DynamicJacobian H2) const {

gtsam/geometry/SOn.h

@@ -83,6 +83,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   template <typename Derived>
   explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
 
+  /// Construct dynamic SO(n) from Fixed SO<M>
+  template <int M, int N_ = N, typename = IsDynamic<N_>>
+  explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
+
   /// Constructor from AngleAxisd
   template <int N_ = N, typename = IsSO3<N_>>
   SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
@@ -188,12 +192,12 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
    *  -d  c -b  a  0
    * This scheme behaves exactly as expected for SO(2) and SO(3).
    */
-  static Matrix Hat(const Vector& xi);
+  static MatrixNN Hat(const TangentVector& xi);
 
   /**
    * Inverse of Hat. See note about xi element order in Hat.
    */
-  static Vector Vee(const Matrix& X);
+  static TangentVector Vee(const MatrixNN& X);
 
   // Chart at origin
   struct ChartAtOrigin {
@@ -259,8 +263,20 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
 using SOn = SO<Eigen::Dynamic>;
 
 /*
- * Fully specialize compose and between, because the derivative is unknowable by
+ * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
- * the LieGroup implementations, who return a fixed-size matrix for H2.
+ * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
+ * and implementation for other fixed N is in SOn-inl.h.
+ */
+
+template <>
+Matrix SOn::Hat(const Vector& xi);
+
+template <>
+Vector SOn::Vee(const Matrix& X);
+
+/*
+ * Specialize dynamic compose and between, because the derivative is unknowable
+ * by the LieGroup implementations, who return a fixed-size matrix for H2.
  */
 
 using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;