From cd9b81ebe67bf7fea76d73e99a938a1a5d0c8573 Mon Sep 17 00:00:00 2001 From: artivis Date: Sun, 21 Jul 2024 17:50:55 +0200 Subject: [PATCH 1/2] Add Lie bracket Signed-off-by: artivis --- include/manif/impl/bracket.h | 35 +++++++++++++++++++++++++ include/manif/impl/tangent_base.h | 43 +++++++++++++++++++++++++++++++ test/common_tester.h | 33 +++++++++++++++++++++++- 3 files changed, 110 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/tangent_base.h b/include/manif/impl/tangent_base.h index abfe81aa..b54d8172 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 @return LieAlg 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 From 5862f85eb24497f3db7a075586bed7dc297b72cd Mon Sep 17 00:00:00 2001 From: artivis Date: Wed, 24 Jul 2024 11:28:40 +0200 Subject: [PATCH 2/2] bracket spe & doc fix --- include/manif/impl/rn/RnTangent_base.h | 8 ++++++++ include/manif/impl/tangent_base.h | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) 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 b54d8172..014397f6 100644 --- a/include/manif/impl/tangent_base.h +++ b/include/manif/impl/tangent_base.h @@ -363,7 +363,7 @@ struct TangentBase * @tparam _DerivedOther * @param a A Tangent object. * @param b A second Tangent object. - * @return @return LieAlg The Lie bracket [a,b] in vector form. + * @return The Lie bracket [a,b] in vector form. */ template static Tangent Bracket(