diff --git a/include/manif/impl/bundle/BundleTangent_base.h b/include/manif/impl/bundle/BundleTangent_base.h index 14ae3346..0cb5d9c0 100644 --- a/include/manif/impl/bundle/BundleTangent_base.h +++ b/include/manif/impl/bundle/BundleTangent_base.h @@ -408,6 +408,31 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for BundleTangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + return vee_impl( + t, v, internal::make_intseq_t::BundleSize>{} + ); + } +}; + +template +void vee_impl(TL& t, const TR& v, internal::intseq<_Idx...>) { + // c++11 "fold expression" + auto l = {((t.template element<_Idx>().setVee( + v.template block< + TL::template Element<_Idx>::LieAlg::RowsAtCompileTime, + TL::template Element<_Idx>::LieAlg::RowsAtCompileTime + >( + std::get<_Idx>(internal::traits::AlgIdx), + std::get<_Idx>(internal::traits::AlgIdx) + ))), 0) ...}; + static_cast(l); // compiler warning +} + } // namespace internal } // namespace manif diff --git a/include/manif/impl/rn/RnTangent_base.h b/include/manif/impl/rn/RnTangent_base.h index d8d66734..a4f7b7c8 100644 --- a/include/manif/impl/rn/RnTangent_base.h +++ b/include/manif/impl/rn/RnTangent_base.h @@ -202,6 +202,7 @@ struct RandomEvaluatorImpl> } }; +//! @brief Bracket specialization for RnTangentBase objects. template struct BracketEvaluatorImpl> { template @@ -210,6 +211,15 @@ struct BracketEvaluatorImpl> { } }; +//! @brief Vee specialization for RnTangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() = v.template topRightCorner(); + } +}; + } // namespace internal } // namespace manif diff --git a/include/manif/impl/se2/SE2Tangent_base.h b/include/manif/impl/se2/SE2Tangent_base.h index 50149db0..05ac86ab 100644 --- a/include/manif/impl/se2/SE2Tangent_base.h +++ b/include/manif/impl/se2/SE2Tangent_base.h @@ -480,6 +480,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SE2TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 2), v(1, 2), v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se3/SE3Tangent_base.h b/include/manif/impl/se3/SE3Tangent_base.h index ebb8f635..a4860c90 100644 --- a/include/manif/impl/se3/SE3Tangent_base.h +++ b/include/manif/impl/se3/SE3Tangent_base.h @@ -458,6 +458,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SE3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 3), v(1, 3), v(2, 3), v(2, 1), v(0, 2), v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se_2_3/SE_2_3Tangent_base.h b/include/manif/impl/se_2_3/SE_2_3Tangent_base.h index 781b3769..2d44b9c3 100644 --- a/include/manif/impl/se_2_3/SE_2_3Tangent_base.h +++ b/include/manif/impl/se_2_3/SE_2_3Tangent_base.h @@ -478,6 +478,17 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SE_2_3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 3), v(1, 3), v(2, 3), + v(2, 1), v(0, 2), v(1, 0), + v(0, 4), v(1, 4), v(2, 4); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/sgal3/SGal3Tangent_base.h b/include/manif/impl/sgal3/SGal3Tangent_base.h index ad705827..f756ab9d 100644 --- a/include/manif/impl/sgal3/SGal3Tangent_base.h +++ b/include/manif/impl/sgal3/SGal3Tangent_base.h @@ -617,6 +617,18 @@ struct RandomEvaluatorImpl> { } }; +//! @brief Vee specialization for SGal3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(0, 4), v(1, 4), v(2, 4), + v(0, 3), v(1, 3), v(2, 3), + v(2, 1), v(0, 2), v(1, 0), + v(3, 4); + } +}; + } // namespace internal } // namespace manif diff --git a/include/manif/impl/so2/SO2Tangent_base.h b/include/manif/impl/so2/SO2Tangent_base.h index 8811c401..8d03c605 100644 --- a/include/manif/impl/so2/SO2Tangent_base.h +++ b/include/manif/impl/so2/SO2Tangent_base.h @@ -225,6 +225,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SO2TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/so3/SO3Tangent_base.h b/include/manif/impl/so3/SO3Tangent_base.h index c14939d1..de9f7a3c 100644 --- a/include/manif/impl/so3/SO3Tangent_base.h +++ b/include/manif/impl/so3/SO3Tangent_base.h @@ -327,6 +327,15 @@ struct RandomEvaluatorImpl> } }; +//! @brief Vee specialization for SO3TangentBase objects. +template +struct VeeEvaluatorImpl> { + template + static void run(TL& t, const TR& v) { + t.coeffs() << v(2, 1), v(0, 2), v(1, 0); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/tangent_base.h b/include/manif/impl/tangent_base.h index 014397f6..6d5178a4 100644 --- a/include/manif/impl/tangent_base.h +++ b/include/manif/impl/tangent_base.h @@ -6,6 +6,7 @@ #include "manif/impl/generator.h" #include "manif/impl/random.h" #include "manif/impl/bracket.h" +#include "manif/impl/vee.h" #include "manif/impl/eigen.h" #include "manif/constants.h" @@ -99,6 +100,14 @@ struct TangentBase */ _Derived& setRandom(); + /** + * @brief Set the Tangent object this from an object in the Lie algebra. + * @param[in] a An object in the Lie algebra. + * @return A reference to this. + */ + template + _Derived& setVee(const Eigen::MatrixBase<_EigenDerived>& a); + // Minimum API // Those functions must be implemented in the Derived class ! @@ -370,6 +379,17 @@ struct TangentBase const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b ); + /** + * @brief Instantiate a Tangent from a Lie algebra object. + * + * @tparam _EigenDerived + * @param alg A tangent object expressed in the Lie algebra. + * @return a Tangent object. + * @see hat + */ + template + static Tangent Vee(const Eigen::MatrixBase<_EigenDerived>& alg); + protected: inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); } @@ -464,6 +484,13 @@ _Derived& TangentBase<_Derived>::setRandom() return derived(); } +template +template +_Derived& TangentBase<_Derived>::setVee(const Eigen::MatrixBase<_EigenDerived>& a) { + internal::VeeEvaluator::Base>(derived()).run(a); + return derived(); +} + template typename TangentBase<_Derived>::LieGroup TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const @@ -738,6 +765,14 @@ typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket( return a.bracket(b); } +template +template +typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Vee( + const Eigen::MatrixBase<_EigenDerived>& alg +) { + return Tangent().setVee(alg); +} + // Math template diff --git a/include/manif/impl/vee.h b/include/manif/impl/vee.h new file mode 100644 index 00000000..19cbf0e6 --- /dev/null +++ b/include/manif/impl/vee.h @@ -0,0 +1,37 @@ +#ifndef _MANIF_MANIF_IMPL_VEE_H_ +#define _MANIF_MANIF_IMPL_VEE_H_ + +namespace manif { +namespace internal { + +template +struct VeeEvaluatorImpl { + template + static void run(TL& t, const TR&) { + static_assert( + constexpr_false(), "VeeEvaluator not overloaded for Derived type!" + ); + // t.setRandom(); + } +}; + +template +struct VeeEvaluator : VeeEvaluatorImpl { + using Base = VeeEvaluatorImpl; + + VeeEvaluator(Derived& xptr) : xptr_(xptr) {} + + template + void run(const T& t) { + Base::run(xptr_, t); + } + +protected: + + Derived& xptr_; +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_IMPL_VEE_H_ diff --git a/test/common_tester.h b/test/common_tester.h index 5ad7698c..bc7ce126 100644 --- a/test/common_tester.h +++ b/test/common_tester.h @@ -110,7 +110,9 @@ TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_INVERSE) \ { evalInverse(); } \ TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_BRACKET) \ - { evalBracket(); } + { evalBracket(); } \ + TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_HAT_VEE) \ + { evalHatVee(); } #define MANIF_TEST_JACOBIANS(manifold) \ using manifold##JacobiansTester = JacobianTester; \ @@ -771,6 +773,10 @@ class CommonTester EXPECT_MANIF_NEAR(Tangent::Bracket(getDelta(), getDelta()), Tangent::Zero()); } + void evalHatVee() { + EXPECT_MANIF_NEAR(getDelta(), Tangent::Vee(getDelta().hat())); + } + protected: // relax eps for float type