From 458a33dadeb79f1f36cb5be9e4bc34a032833156 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 1 Aug 2020 15:41:19 -0400
Subject: [PATCH] VectorizedGenerators

---
 gtsam/geometry/SOn-inl.h | 10 +++-------
 gtsam/geometry/SOn.h     | 31 ++++++++++++++++++++++++++++++-
 2 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h
index 0d7f3e108..9edce8336 100644
--- a/gtsam/geometry/SOn-inl.h
+++ b/gtsam/geometry/SOn-inl.h
@@ -26,13 +26,13 @@ using namespace std;
 
 namespace gtsam {
 
-// Implementation for N>5 just uses dynamic version
+// Implementation for N>=5 just uses dynamic version
 template <int N>
 typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
   return SOn::Hat(xi);
 }
 
-// Implementation for N>5 just uses dynamic version
+// Implementation for N>=5 just uses dynamic version
 template <int N>
 typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
   return SOn::Vee(X);
@@ -99,12 +99,8 @@ typename SO<N>::VectorN2 SO<N>::vec(
   if (H) {
     // Calculate P matrix of vectorized generators
     // TODO(duy): Should we refactor this as the jacobian of Hat?
+    Matrix P = VectorizedGenerators(n);
     const size_t d = dim();
-    Matrix P(n2, d);
-    for (size_t j = 0; j < d; j++) {
-      const auto X = Hat(Eigen::VectorXd::Unit(d, j));
-      P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
-    }
     H->resize(n2, d);
     for (size_t i = 0; i < n; i++) {
       H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index a6392c2f9..004569416 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -290,7 +290,34 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
    * */
   VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
                    boost::none) const;
-  /// @}
+
+  /// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
+  template <int N_ = N, typename = IsFixed<N_>>
+  static Matrix VectorizedGenerators() {
+    constexpr size_t N2 = static_cast<size_t>(N * N);
+    Matrix G(N2, dimension);
+    for (size_t j = 0; j < dimension; j++) {
+      const auto X = Hat(Vector::Unit(dimension, j));
+      G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
+    }
+    return G;
+  }
+
+  /// Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
+  template <int N_ = N, typename = IsDynamic<N_>>
+  static Matrix VectorizedGenerators(size_t n = 0) {
+    const size_t n2 = n * n, dim = Dimension(n);
+    Matrix G(n2, dim);
+    for (size_t j = 0; j < dim; j++) {
+      const auto X = Hat(Vector::Unit(dim, j));
+      G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
+    }
+    return G;
+  }
+
+  /// @{
+  /// @name Serialization
+  /// @{
 
   template <class Archive>
   friend void save(Archive&, SO&, const unsigned int);
@@ -300,6 +327,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   friend void serialize(Archive&, SO&, const unsigned int);
   friend class boost::serialization::access;
   friend class Rot3;  // for serialize
+
+  /// @}
 };
 
 using SOn = SO<Eigen::Dynamic>;