From cd168210746dab9c589babd977401372f05d7aac Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Fri, 26 Jul 2024 14:23:21 +0200 Subject: [PATCH] Add Lie bracket (#300) * Add Lie bracket Signed-off-by: artivis * bracket spe & doc fix --------- Signed-off-by: artivis --- include/manif/impl/bracket.h | 35 +++++++++++++++++++++ include/manif/impl/rn/RnTangent_base.h | 8 +++++ include/manif/impl/tangent_base.h | 43 ++++++++++++++++++++++++++ test/common_tester.h | 33 +++++++++++++++++++- 4 files changed, 118 insertions(+), 1 deletion(-) create mode 100644 include/manif/impl/bracket.h diff --git a/include/manif/impl/bracket.h b/include/manif/impl/bracket.h new file mode 100644 index 00000000..ddc6b0f2 --- /dev/null +++ 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 +struct BracketEvaluatorImpl { + template + static typename Derived::Tangent run(const TL& a, const TR& b) { + return a.smallAdj() * b; + } +}; + +template +struct BracketEvaluator : BracketEvaluatorImpl { + using Base = BracketEvaluatorImpl; + + 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 index 3f9996f3..d8d66734 100644 --- a/include/manif/impl/rn/RnTangent_base.h +++ b/include/manif/impl/rn/RnTangent_base.h @@ -202,6 +202,14 @@ struct RandomEvaluatorImpl> } }; +template +struct BracketEvaluatorImpl> { + template + 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 index abfe81aa..014397f6 100644 --- 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 + 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 + 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 +template +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 template bool TangentBase<_Derived>::isApprox( @@ -695,6 +730,14 @@ TangentBase<_Derived>::InnerWeights() typename internal::traits<_Derived>::Base>::run(); } +template +template +typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket( + const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b +) { + return a.bracket(b); +} + // Math template diff --git a/test/common_tester.h b/test/common_tester.h index 48e1d99c..5ad7698c 100644 --- 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; \ @@ -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