Add Lie bracket (#300)

* Add Lie bracket Signed-off-by: artivis <[email protected]> * bracket spe & doc fix --------- Signed-off-by: artivis <[email protected]>
artivis · Jul 26, 2024 · cd16821 · cd16821
1 parent 534ff8c
commit cd16821
Show file tree

Hide file tree

Showing 4 changed files with 118 additions and 1 deletion.
diff --git a/include/manif/impl/bracket.h b/include/manif/impl/bracket.h
@@ -0,0 +1,35 @@
+#ifndef _MANIF_MANIF_IMPL_BRACKET_H_
+#define _MANIF_MANIF_IMPL_BRACKET_H_
+
+namespace manif {
+namespace internal {
+
+template <typename Derived>
+struct BracketEvaluatorImpl {
+  template <typename TL, typename TR>
+  static typename Derived::Tangent run(const TL& a, const TR& b) {
+    return a.smallAdj() * b;
+  }
+};
+
+template <typename Derived, typename DerivedOther>
+struct BracketEvaluator : BracketEvaluatorImpl<Derived> {
+  using Base = BracketEvaluatorImpl<Derived>;
+
+  BracketEvaluator(const Derived& xptr, const DerivedOther& xptr_o)
+  : xptr_(xptr), xptr_o_(xptr_o) {}
+
+  typename Derived::Tangent run() {
+    return Base::run(xptr_, xptr_o_);
+  }
+
+protected:
+
+  const Derived& xptr_;
+  const DerivedOther& xptr_o_;
+};
+
+} // namespace internal
+} // namespace manif
+
+#endif // _MANIF_MANIF_IMPL_BRACKET_H_
diff --git a/include/manif/impl/rn/RnTangent_base.h b/include/manif/impl/rn/RnTangent_base.h
@@ -202,6 +202,14 @@ struct RandomEvaluatorImpl<RnTangentBase<Derived>>
   }
 };
 
+template <typename Derived>
+struct BracketEvaluatorImpl<RnTangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static typename Derived::Tangent run(const TL&, const TR&) {
+    return Derived::Tangent::Zero();
+  }
+};
+
 } // namespace internal
 } // namespace manif
 

diff --git a/include/manif/impl/tangent_base.h b/include/manif/impl/tangent_base.h
@@ -5,6 +5,7 @@
 #include "manif/impl/traits.h"
 #include "manif/impl/generator.h"
 #include "manif/impl/random.h"
+#include "manif/impl/bracket.h"
 #include "manif/impl/eigen.h"
 
 #include "manif/constants.h"
@@ -255,6 +256,16 @@ struct TangentBase
    */
   Jacobian smallAdj() const;
 
+  /**
+   * @brief Compute the Lie bracket [this,b] in vector form.
+   *
+   * @tparam _DerivedOther
+   * @param b Another tangent object of the same group.
+   * @return The Lie bracket [this,b] in vector form.
+   */
+  template <typename _DerivedOther>
+  Tangent bracket(const TangentBase<_DerivedOther>& b) const;
+
   /**
    * @brief Evaluate whether this and v are 'close'.
    * @details This evaluation is performed element-wise.
@@ -346,6 +357,19 @@ struct TangentBase
   //! Static helper to get a Basis of the Lie group.
   static InnerWeightsMatrix InnerWeights();
 
+  /**
+   * @brief Compute the Lie bracket [a,b] in vector form.
+   *
+   * @tparam _DerivedOther
+   * @param a A Tangent object.
+   * @param b A second Tangent object.
+   * @return The Lie bracket [a,b] in vector form.
+   */
+  template <typename _DerivedOther>
+  static Tangent Bracket(
+    const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
+  );
+
 protected:
 
   inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); }
@@ -622,6 +646,17 @@ TangentBase<_Derived>::smallAdj() const
   return derived().smallAdj();
 }
 
+template <typename _Derived>
+template <typename _DerivedOther>
+typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::bracket(
+  const TangentBase<_DerivedOther>& b
+) const {
+  return internal::BracketEvaluator<
+    typename internal::traits<_Derived>::Base,
+    typename internal::traits<_DerivedOther>::Base
+  >(derived(), b.derived()).run();
+}
+
 template <typename _Derived>
 template <typename _EigenDerived>
 bool TangentBase<_Derived>::isApprox(
@@ -695,6 +730,14 @@ TangentBase<_Derived>::InnerWeights()
       typename internal::traits<_Derived>::Base>::run();
 }
 
+template <typename _Derived>
+template <typename _DerivedOther>
+typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket(
+  const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
+) {
+  return a.bracket(b);
+}
+
 // Math
 
 template <typename _Derived>

diff --git a/test/common_tester.h b/test/common_tester.h
@@ -108,7 +108,9 @@
   TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_CAST)                \
   { evalCast(); }                                                         \
   TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_INVERSE)             \
-  { evalInverse(); }
+  { evalInverse(); }                                                      \
+  TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_BRACKET)             \
+  { evalBracket(); }
 
 #define MANIF_TEST_JACOBIANS(manifold)                                                                                    \
   using manifold##JacobiansTester = JacobianTester<manifold>;                                                             \
@@ -740,6 +742,35 @@ class CommonTester
     EXPECT_MANIF_NEAR(LieGroup::Identity(), getState().inverse()*getState());
   }
 
+  void evalBracket() {
+
+    // [a,b] = (a.smallAdj() * b)^ = a^b^-b^a^
+    EXPECT_EIGEN_NEAR(
+      Tangent::Bracket(getDelta(), getDeltaOther()).hat(),
+      getDelta().hat() * getDeltaOther().hat() - getDeltaOther().hat() * getDelta().hat()
+    );
+
+    // Jacobi identity [x,[y,z]]+[y,[z,x]]+[z,[x,y]]=0
+    const Tangent& x = getDelta();
+    const Tangent& y = getDeltaOther();
+    const Tangent z = getState().log();
+    EXPECT_MANIF_NEAR(
+      Tangent::Bracket(x, Tangent::Bracket(y, z)) +
+      Tangent::Bracket(y, Tangent::Bracket(z, x)) +
+      Tangent::Bracket(z, Tangent::Bracket(x, y)),
+      Tangent::Zero()
+    );
+
+    // Anti-symmetry [x,y] = -[y,x]
+    EXPECT_MANIF_NEAR(
+      Tangent::Bracket(getDelta(), getDeltaOther()),
+      -Tangent::Bracket(getDeltaOther(), getDelta())
+    );
+
+    // [x,x] = 0
+    EXPECT_MANIF_NEAR(Tangent::Bracket(getDelta(), getDelta()), Tangent::Zero());
+  }
+
 protected:
 
   // relax eps for float type