diff --git a/include/manif/Bundle.h b/include/manif/Bundle.h new file mode 100644 index 00000000..8cf7873c --- /dev/null +++ b/include/manif/Bundle.h @@ -0,0 +1,17 @@ +#ifndef _MANIF_BUNDLE_H_ +#define _MANIF_BUNDLE_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/bundle/Bundle_properties.h" +#include "manif/impl/bundle/Bundle_base.h" +#include "manif/impl/bundle/Bundle_map.h" +#include "manif/impl/bundle/Bundle.h" +#include "manif/impl/bundle/BundleTangent_base.h" +#include "manif/impl/bundle/BundleTangent.h" +#include "manif/impl/bundle/BundleTangent_map.h" + +#endif // _MANIF_BUNDLE_H_ diff --git a/include/manif/impl/bundle/Bundle.h b/include/manif/impl/bundle/Bundle.h new file mode 100644 index 00000000..df3b9368 --- /dev/null +++ b/include/manif/impl/bundle/Bundle.h @@ -0,0 +1,172 @@ +#ifndef _MANIF_MANIF_BUNDLE_H_ +#define _MANIF_MANIF_BUNDLE_H_ + +#include "manif/impl/bundle/Bundle_base.h" + +#include +#include + +namespace manif +{ +namespace internal +{ + +//! Traits specialization +template class ... _T> +struct traits> +{ + // Bundle-specific traits + using IdxList = make_intseq_t; + + using LenDim = intseq<_T<_Scalar>::Dim ...>; + using BegDim = bundle::intseq_psum_t; + + using LenDoF = intseq<_T<_Scalar>::DoF ...>; + using BegDoF = bundle::intseq_psum_t; + + using LenTra = intseq<_T<_Scalar>::Transformation::RowsAtCompileTime ...>; + using BegTra = bundle::intseq_psum_t; + + using LenRep = intseq<_T<_Scalar>::RepSize ...>; + using BegRep = bundle::intseq_psum_t; + + template + using PartType = typename bundle::bundle_element<_Idx, _T<_Scalar>...>::type; + + // Regular traits + using Scalar = _Scalar; + + using LieGroup = Bundle<_Scalar, _T ...>; + using Tangent = BundleTangent<_Scalar, _T ...>; + + using Base = BundleBase>; + + static constexpr int Dim = bundle::intseq_sum::value; + static constexpr int DoF = bundle::intseq_sum::value; + static constexpr int RepSize = bundle::intseq_sum::value; + + using DataType = Eigen::Matrix<_Scalar, RepSize, 1>; + using Jacobian = Eigen::Matrix<_Scalar, DoF, DoF>; + using Transformation = Eigen::Matrix< + _Scalar, bundle::intseq_sum::value, bundle::intseq_sum::value + >; + using Vector = Eigen::Matrix<_Scalar, Dim, 1>; +}; + +} // namespace internal + +// +// Bundle LieGroup +// + +/** + * @brief Represents a Bundle element. + */ +template class ... _T> +struct Bundle : BundleBase> +{ +private: + static_assert(sizeof...(_T) > 0, "Must have at least one element in Bundle !"); + + using Base = BundleBase>; + using Type = Bundle<_Scalar, _T...>; + + using BegRep = typename internal::traits::BegRep; + using LenRep = typename internal::traits::LenRep; + +protected: + + using Base::derived; + +public: + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Bundle() = default; + ~Bundle() = default; + + MANIF_COPY_CONSTRUCTOR(Bundle); + MANIF_MOVE_CONSTRUCTOR(Bundle); + + // Copy constructor + template + Bundle(const LieGroupBase<_DerivedOther> & o); + + MANIF_GROUP_ASSIGN_OP(Bundle); + + // LieGroup common API + + /** + * @brief Get a reference to the underlying DataType. + * @param[out] a reference to the underlying Eigen vector + */ + DataType & coeffs(); + + /** + * @brief Get a const reference to the underlying DataType. + * @param[out] a const reference to the underlying Eigen vector + */ + const DataType & coeffs() const; + + + // Bundle specific API + + /** + * @brief Construct from Bundle parts + */ + Bundle(const _T<_Scalar> & ... parts); + +protected: + // Helper for the parts constructor + template + Bundle(intseq<_BegRep...>, intseq<_LenRep...>, const _T<_Scalar> & ... parts); + +protected: + + //! Underlying data (Eigen) vector + DataType data_; +}; + + +template class ... _T> +template +Bundle<_Scalar, _T...>::Bundle(const LieGroupBase<_DerivedOther> & o) +: Bundle(o.coeffs()) +{} + +template class ... _T> +Bundle<_Scalar, _T...>::Bundle(const _T<_Scalar> & ... parts) +: Bundle(BegRep{}, LenRep{}, parts ...) +{} + +template class ... _T> +template +Bundle<_Scalar, _T...>::Bundle( + intseq<_BegRep...>, intseq<_LenRep...>, const _T<_Scalar> & ... parts) +{ + // c++11 "fold expression" + auto l = {((data_.template segment<_LenRep>(_BegRep) = parts.coeffs()), 0) ...}; + static_cast(l); // compiler warning +} + +template class ... _T> +typename Bundle<_Scalar, _T...>::DataType & +Bundle<_Scalar, _T...>::coeffs() +{ + return data_; +} + +template class ... _T> +const typename Bundle<_Scalar, _T...>::DataType & +Bundle<_Scalar, _T...>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_H_ diff --git a/include/manif/impl/bundle/BundleTangent.h b/include/manif/impl/bundle/BundleTangent.h new file mode 100644 index 00000000..3c3ca785 --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent.h @@ -0,0 +1,170 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_H_ +#define _MANIF_MANIF_BUNDLETANGENT_H_ + +#include "manif/impl/bundle/BundleTangent_base.h" + +#include +#include + +namespace manif +{ +namespace internal +{ + +//! Traits specialization +template class ... _T> +struct traits> +{ + // BundleTangent-specific traits + using IdxList = make_intseq_t; + + using LenDim = intseq<_T<_Scalar>::Tangent::Dim ...>; + using BegDim = bundle::intseq_psum_t; + + using LenDoF = intseq<_T<_Scalar>::Tangent::DoF ...>; + using BegDoF = bundle::intseq_psum_t; + + using LenRep = intseq<_T<_Scalar>::Tangent::RepSize ...>; + using BegRep = bundle::intseq_psum_t; + + using LenAlg = intseq<_T<_Scalar>::Tangent::LieAlg::RowsAtCompileTime ...>; + using BegAlg = bundle::intseq_psum_t; + + template + using PartType = typename bundle::bundle_element<_Idx, _T<_Scalar>...>::type::Tangent; + + // Regular traits + using Scalar = _Scalar; + + using LieGroup = Bundle<_Scalar, _T...>; + using Tangent = BundleTangent<_Scalar, _T...>; + + using Base = BundleTangentBase; + + static constexpr int Dim = bundle::intseq_sum::value; + static constexpr int DoF = bundle::intseq_sum::value; + static constexpr int RepSize = bundle::intseq_sum::value; + + using DataType = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + using LieAlg = Eigen::Matrix::value, + bundle::intseq_sum::value>; +}; + +} // namespace internal + +// +// BundleTangent +// + +/** + * @brief Represents a BundleTangent element. + */ +template class ... _T> +struct BundleTangent : BundleTangentBase> +{ +private: + static_assert(sizeof...(_T) > 0, "Must have at least one element in BundleTangent !"); + + using Base = BundleTangentBase>; + using Type = BundleTangent<_Scalar, _T...>; + + using BegRep = typename internal::traits::BegRep; + using LenRep = typename internal::traits::LenRep; + +protected: + + using Base::derived; + +public: + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + BundleTangent() = default; + ~BundleTangent() = default; + + MANIF_COPY_CONSTRUCTOR(BundleTangent) + MANIF_MOVE_CONSTRUCTOR(BundleTangent) + + // Copy constructors given base + template + BundleTangent(const TangentBase<_DerivedOther> & o); + + MANIF_TANGENT_ASSIGN_OP(BundleTangent) + + // Tangent common API + + /** + * @brief Get a reference to the underlying DataType. + */ + DataType & coeffs(); + + /** + * @brief Get a const reference to the underlying DataType. + */ + const DataType & coeffs() const; + + + // BundleTangent specific API + + /** + * @brief Construct from BundleTangent parts + */ + BundleTangent(const typename _T<_Scalar>::Tangent & ... parts); + +protected: + // Helper for the parts constructor + template + BundleTangent( + intseq<_BegRep...>, intseq<_LenRep...>, + const typename _T<_Scalar>::Tangent & ... parts); + +protected: + DataType data_; +}; + + +template class ... _T> +template +BundleTangent<_Scalar, _T...>::BundleTangent(const TangentBase<_DerivedOther> & o) +: data_(o.coeffs()) +{} + +template class ... _T> +BundleTangent<_Scalar, _T...>::BundleTangent(const typename _T<_Scalar>::Tangent & ... parts) +: BundleTangent(BegRep{}, LenRep{}, parts ...) +{} + +template class ... _T> +template +BundleTangent<_Scalar, _T...>::BundleTangent( + intseq<_BegRep...>, intseq<_LenRep...>, + const typename _T<_Scalar>::Tangent & ... parts) +{ + // c++11 "fold expression" + auto l = {((data_.template segment<_LenRep>(_BegRep) = parts.coeffs()), 0) ...}; + static_cast(l); // compiler warning +} + +template class ... _T> +typename BundleTangent<_Scalar, _T...>::DataType & +BundleTangent<_Scalar, _T...>::coeffs() +{ + return data_; +} + +template class ... _T> +const typename BundleTangent<_Scalar, _T...>::DataType & +BundleTangent<_Scalar, _T...>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLETANGENT_H_ diff --git a/include/manif/impl/bundle/BundleTangent_base.h b/include/manif/impl/bundle/BundleTangent_base.h new file mode 100644 index 00000000..f15d45f3 --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent_base.h @@ -0,0 +1,398 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_BASE_H_ +#define _MANIF_MANIF_BUNDLETANGENT_BASE_H_ + +#include "manif/impl/bundle/Bundle_properties.h" +#include "manif/impl/tangent_base.h" + +namespace manif +{ + +/** + * @brief The base class of the Bundle tangent. + */ +template +struct BundleTangentBase : TangentBase<_Derived> +{ +private: + using Base = TangentBase<_Derived>; + using Type = BundleTangentBase<_Derived>; + + using IdxList = typename internal::traits<_Derived>::IdxList; + + using BegDoF = typename internal::traits<_Derived>::BegDoF; + using LenDoF = typename internal::traits<_Derived>::LenDoF; + + using BegRep = typename internal::traits<_Derived>::BegRep; + using LenRep = typename internal::traits<_Derived>::LenRep; + + using BegAlg = typename internal::traits<_Derived>::BegAlg; + using LenAlg = typename internal::traits<_Derived>::LenAlg; + + template + using PartType = typename internal::traits<_Derived>::template PartType<_Idx>; + +public: + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::data; + using Base::coeffs; + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(BundleTangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(BundleTangentBase) + + // Tangent common API + + /** + * @brief Hat operator. + * @return An element of the Lie algebra. + */ + LieAlg hat() const; + + /** + * @brief Exponential operator. + * @return An element of the Lie Group. + */ + LieGroup exp(OptJacobianRef J_m_t = {}) const; + + /** + * @brief This function is deprecated. + * Please considere using + * @ref exp instead. + */ + MANIF_DEPRECATED + LieGroup retract(OptJacobianRef J_m_t = {}) const; + + /** + * @brief Get the right Jacobian. + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian. + */ + Jacobian ljac() const; + + /** + * @brief Get the inverse of the right Jacobian. + */ + Jacobian rjacinv() const; + + /** + * @brief Get the inverse of the right Jacobian. + */ + Jacobian ljacinv() const; + + /** + * @brief + */ + Jacobian smallAdj() const; + + + // BundleTangent specific API + + /** + * @brief Number of parts in the BundleTangent + */ + static constexpr std::size_t BundleSize = IdxList::size(); + + /** + * @brief Access BundleTangent part as Map + * @tparam _Idx part index + */ + template + Eigen::Map> get(); + + /** + * @brief Access BundleTangent part as Map to const + * @tparam _Idx part index + */ + template + Eigen::Map> get() const; + +protected: + template + LieAlg + hat_impl(intseq<_Idx...>, intseq<_BegAlg...>, intseq<_LenAlg...>) const; + + template + LieGroup + exp_impl(OptJacobianRef J_m_t, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + rjac_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + ljac_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + rjacinv_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + ljacinv_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian + smallAdj_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + friend internal::GeneratorEvaluator>; + friend internal::RandomEvaluatorImpl>; +}; + + +template +typename BundleTangentBase<_Derived>::LieAlg +BundleTangentBase<_Derived>::hat() const +{ + return hat_impl(IdxList{}, BegAlg{}, LenAlg{}); +} + +template +template +typename BundleTangentBase<_Derived>::LieAlg +BundleTangentBase<_Derived>::hat_impl( + intseq<_Idx...>, intseq<_BegAlg...>, intseq<_LenAlg...>) const +{ + LieAlg ret = LieAlg::Zero(); + // c++11 "fold expression" + auto l = {((ret.template block<_LenAlg, _LenAlg>(_BegAlg, _BegAlg) = get<_Idx>().hat()), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const +{ + if (J_m_t) { + J_m_t->setZero(); + } + return exp_impl(J_m_t, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::exp_impl( + OptJacobianRef J_m_t, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + if (J_m_t) { + return LieGroup(get<_Idx>().exp(J_m_t->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF)) ...); + } + return LieGroup(get<_Idx>().exp() ...); +} + +template +typename BundleTangentBase<_Derived>::LieGroup +BundleTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const +{ + return exp(J_m_t); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjac() const +{ + return rjac_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljac() const +{ + return ljac_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjacinv() const +{ + return rjacinv_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljacinv() const +{ + return ljacinv_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::smallAdj() const +{ + return smallAdj_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjac_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = {((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = get<_Idx>().rjac() ), 0) ...}; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljac_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = {((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = get<_Idx>().ljac()), 0) ...}; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::rjacinv_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = get<_Idx>().rjacinv()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::ljacinv_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = get<_Idx>().ljacinv()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +typename BundleTangentBase<_Derived>::Jacobian +BundleTangentBase<_Derived>::smallAdj_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian Jr = Jacobian::Zero(); + // c++11 "fold expression" + auto l = { + ((Jr.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = get<_Idx>().smallAdj()), 0) ... + }; + static_cast(l); // compiler warning + return Jr; +} + +template +template +Eigen::Map::template PartType<_Idx>> +BundleTangentBase<_Derived>::get() +{ + return Eigen::Map>( + static_cast<_Derived &>(*this).coeffs().data() + + internal::bundle::intseq_element<_Idx, BegRep>::value); +} + +template +template +Eigen::Map::template PartType<_Idx>> +BundleTangentBase<_Derived>::get() const +{ + return Eigen::Map>( + static_cast(*this).coeffs().data() + + internal::bundle::intseq_element<_Idx, BegRep>::value); +} + +namespace internal +{ + +/** + * @brief Generator specialization for BundleTangentBase objects. + */ +template +struct GeneratorEvaluator> +{ + static typename BundleTangentBase::LieAlg + run(const unsigned int i) + { + MANIF_CHECK( + i < BundleTangentBase::DoF, + "Index i must less than DoF!", + invalid_argument); + + return run( + i, + typename BundleTangentBase::IdxList{}, + typename BundleTangentBase::BegDoF{}, + typename BundleTangentBase::LenDoF{}, + typename BundleTangentBase::BegAlg{}, + typename BundleTangentBase::LenAlg{}); + } + + template + static typename BundleTangentBase::LieAlg + run( + const unsigned int i, intseq<_Idx...>, + intseq<_BegDoF...>, intseq<_LenDoF...>, + intseq<_BegAlg...>, intseq<_LenAlg...>) + { + using LieAlg = typename BundleTangentBase::LieAlg; + LieAlg Ei = LieAlg::Constant(0); + // c++11 "fold expression" + auto l = {((Ei.template block<_LenAlg, _LenAlg>(_BegAlg, _BegAlg) = + (i >= _BegDoF && i < _BegDoF + _LenDoF) ? + BundleTangentBase::template PartType<_Idx>::Generator(i - _BegDoF) : + BundleTangentBase::template PartType<_Idx>::LieAlg::Zero() + ), 0) ...}; + static_cast(l); // compiler warning + return Ei; + } +}; + +/** + * @brief Random specialization for BundleTangent objects. + */ +template +struct RandomEvaluatorImpl> +{ + static void run(BundleTangentBase & m) + { + run(m, typename BundleTangentBase::IdxList{}); + } + + template + static void run(BundleTangentBase & m, intseq<_Idx...>) + { + m = typename BundleTangentBase::Tangent( + BundleTangentBase::template PartType<_Idx>::Random() ...); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLETANGENT_BASE_H_ diff --git a/include/manif/impl/bundle/BundleTangent_map.h b/include/manif/impl/bundle/BundleTangent_map.h new file mode 100644 index 00000000..f64f57c6 --- /dev/null +++ b/include/manif/impl/bundle/BundleTangent_map.h @@ -0,0 +1,98 @@ +#ifndef _MANIF_MANIF_BUNDLETANGENT_MAP_H_ +#define _MANIF_MANIF_BUNDLETANGENT_MAP_H_ + +#include "manif/impl/bundle/BundleTangent.h" + +namespace manif +{ +namespace internal +{ + +/** + * @brief traits specialization for Eigen Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using Base = BundleTangentBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +/** + * @brief traits specialization for Eigen const Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using Base = BundleTangentBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + + +namespace Eigen +{ + +/** + * @brief Specialization of Map for manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleTangentBase, 0>> +{ + using Base = manif::BundleTangentBase, 0>>; + +public: + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + Map(Scalar * coeffs) : data_(coeffs) { } + + MANIF_TANGENT_MAP_ASSIGN_OP(BundleTangent) + + DataType & coeffs() {return data_;} + + const DataType & coeffs() const {return data_;} + +protected: + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::BundleTangent + */ +template class ... T> +class Map, 0> + : public manif::BundleTangentBase, 0>> +{ + using Base = manif::BundleTangentBase, 0>>; + +public: + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::BundleSize; + + Map(const Scalar * coeffs) : data_(coeffs) { } + + const DataType & coeffs() const {return data_;} + +protected: + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_BUNDLETANGENT_MAP_H_ diff --git a/include/manif/impl/bundle/Bundle_base.h b/include/manif/impl/bundle/Bundle_base.h new file mode 100644 index 00000000..d61ccd81 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_base.h @@ -0,0 +1,390 @@ +#ifndef _MANIF_MANIF_BUNDLE_BASE_H_ +#define _MANIF_MANIF_BUNDLE_BASE_H_ + +#include "manif/impl/bundle/Bundle_properties.h" +#include "manif/impl/lie_group_base.h" + +namespace manif +{ + +/** + * @brief The base class of the Bundle group. + */ +template +struct BundleBase : LieGroupBase<_Derived> +{ +private: + using Base = LieGroupBase<_Derived>; + using Type = BundleBase<_Derived>; + + using IdxList = typename internal::traits<_Derived>::IdxList; + + using BegDoF = typename internal::traits<_Derived>::BegDoF; + using LenDoF = typename internal::traits<_Derived>::LenDoF; + + using BegDim = typename internal::traits<_Derived>::BegDim; + using LenDim = typename internal::traits<_Derived>::LenDim; + + using BegRep = typename internal::traits<_Derived>::BegRep; + using LenRep = typename internal::traits<_Derived>::LenRep; + + using BegTra = typename internal::traits<_Derived>::BegTra; + using LenTra = typename internal::traits<_Derived>::LenTra; + + template + using PartType = typename internal::traits<_Derived>::template PartType; + +public: + MANIF_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_AUTO_API + MANIF_INHERIT_GROUP_OPERATOR + + using Base::coeffs; + + using Transformation = typename internal::traits<_Derived>::Transformation; + + // LieGroup common API + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(BundleBase) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(BundleBase) + + /** + * @brief Get the inverse of this. + * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. + * @note r^-1 = -r + */ + LieGroup inverse(OptJacobianRef J_minv_m = {}) const; + + /** + * @brief Get the corresponding Lie algebra element. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The tangent of this. + */ + Tangent log(OptJacobianRef J_t_m = {}) const; + + /** + * @brief This function is deprecated. + * Please consider using + * @ref log instead. + */ + MANIF_DEPRECATED + Tangent lift(OptJacobianRef J_t_m = {}) const; + + /** + * @brief Composition of this and another Bundle element. + * @param[in] m Another Bundle element. + * @param[out] -optional- J_mc_ma Jacobian of the composition wrt this. + * @param[out] -optional- J_mc_mb Jacobian of the composition wrt m. + * @return The composition of 'this . m'. + */ + template + LieGroup compose( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma = {}, + OptJacobianRef J_mc_mb = {}) const; + + /** + * @brief Bundle group action + * @param v vector. + * @param[out] -optional- J_vout_m The Jacobian of the new object wrt this. + * @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object. + * @return The translated vector. + */ + Vector act( + const Vector & v, + tl::optional>> J_vout_m = {}, + tl::optional>> J_vout_v = {}) const; + + /** + * @brief Get the adjoint matrix at this. + */ + Jacobian adj() const; + + + // Bundle-specific API + + /** + * @brief Number of parts in bundle + */ + static constexpr std::size_t BundleSize = IdxList::size(); + + /** + * @brief Get the block-diagonal transformation matrix + */ + Transformation transform() const; + + /** + * @brief Access Bundle part as Map + * @tparam _Idx part index + */ + template + Eigen::Map> get(); + + /** + * @brief Access Bundle part as Map to const + * @tparam _Idx part index + */ + template + Eigen::Map> get() const; + +protected: + template + LieGroup + inverse_impl(OptJacobianRef, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Tangent + log_impl(OptJacobianRef, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + LieGroup + compose_impl( + const LieGroupBase<_DerivedOther> & m, + OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb, + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Vector act_impl( + const Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v, + intseq<_Idx...>, intseq<_BegDim...>, intseq<_LenDim...>, + intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Jacobian adj_impl(intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const; + + template + Transformation + transform_impl(intseq<_Idx...>, intseq<_BegTra...>, intseq<_LenTra...>) const; + + friend internal::RandomEvaluatorImpl>; +}; + + +template +typename BundleBase<_Derived>::Transformation +BundleBase<_Derived>::transform() const +{ + return transform_impl(IdxList{}, BegTra{}, LenTra{}); +} + +template +template +typename BundleBase<_Derived>::Transformation +BundleBase<_Derived>::transform_impl( + intseq<_Idx...>, intseq<_BegTra...>, intseq<_LenTra...>) const +{ + Transformation ret = Transformation::Zero(); + // cxx11 "fold expression" + auto l = + {((ret.template block<_LenTra, _LenTra>(_BegTra, _BegTra) = get<_Idx>().transform()), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::inverse(OptJacobianRef J_minv_m) const +{ + if (J_minv_m) { + J_minv_m->setZero(); + } + return inverse_impl(J_minv_m, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::inverse_impl( + OptJacobianRef J_minv_m, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + if (J_minv_m) { + return LieGroup( + get<_Idx>().inverse(J_minv_m->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF)) ... + ); + } + return LieGroup(get<_Idx>().inverse() ...); +} + +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::log(OptJacobianRef J_t_m) const +{ + if (J_t_m) { + J_t_m->setZero(); + } + return log_impl(J_t_m, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::log_impl( + OptJacobianRef J_minv_m, intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + if (J_minv_m) { + return Tangent( + get<_Idx>().log(J_minv_m->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF))... + ); + } + return Tangent(get<_Idx>().log() ...); +} + +template +typename BundleBase<_Derived>::Tangent +BundleBase<_Derived>::lift(OptJacobianRef J_t_m) const +{ + return log(J_t_m); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::compose( + const LieGroupBase<_DerivedOther> & m, OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb) const +{ + if (J_mc_ma) { + J_mc_ma->setZero(); + } + if (J_mc_mb) { + J_mc_mb->setZero(); + } + return compose_impl(m, J_mc_ma, J_mc_mb, IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::LieGroup +BundleBase<_Derived>::compose_impl( + const LieGroupBase<_DerivedOther> & m, OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb, + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + return LieGroup( + get<_Idx>().compose( + static_cast(m).template get<_Idx>(), + J_mc_ma ? + J_mc_ma->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) : + tl::optional>>{}, + J_mc_mb ? + J_mc_mb->template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) : + tl::optional>>{} + ) ... + ); +} + +template +typename BundleBase<_Derived>::Vector +BundleBase<_Derived>::act( + const typename BundleBase<_Derived>::Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v) const +{ + if (J_vout_m) { + J_vout_m->setZero(); + } + if (J_vout_v) { + J_vout_v->setZero(); + } + + return act_impl(v, J_vout_m, J_vout_v, IdxList{}, BegDim{}, LenDim{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::Vector +BundleBase<_Derived>::act_impl( + const typename BundleBase<_Derived>::Vector & v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v, + intseq<_Idx...>, intseq<_BegDim...>, intseq<_LenDim...>, + intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Vector ret; + // cxx11 "fold expression" + auto l = {((ret.template segment<_LenDim>(_BegDim) = get<_Idx>().act( + v.template segment<_LenDim>(_BegDim), + J_vout_m ? + J_vout_m->template block<_LenDim, _LenDoF>(_BegDim, _BegDoF) : + tl::optional>>{}, + J_vout_v ? + J_vout_v->template block<_LenDim, _LenDim>(_BegDim, _BegDim) : + tl::optional>>{} + )), 0) ...}; + static_cast(l); // compiler warning + return ret; +} + +template +typename BundleBase<_Derived>::Jacobian +BundleBase<_Derived>::adj() const +{ + return adj_impl(IdxList{}, BegDoF{}, LenDoF{}); +} + +template +template +typename BundleBase<_Derived>::Jacobian +BundleBase<_Derived>::adj_impl( + intseq<_Idx...>, intseq<_BegDoF...>, intseq<_LenDoF...>) const +{ + Jacobian adj = Jacobian::Zero(); + // cxx11 "fold expression" + auto l = {((adj.template block<_LenDoF, _LenDoF>(_BegDoF, _BegDoF) = get<_Idx>().adj()), 0) ...}; + static_cast(l); // compiler warning + return adj; +} + +template +template +Eigen::Map::template PartType<_Idx>> +BundleBase<_Derived>::get() +{ + return Eigen::Map>( + static_cast<_Derived &>(*this).coeffs().data() + + internal::bundle::intseq_element<_Idx, BegRep>::value); +} + +template +template +Eigen::Map::template PartType<_Idx>> +BundleBase<_Derived>::get() const +{ + return Eigen::Map>( + static_cast(*this).coeffs().data() + internal::bundle::intseq_element<_Idx, + BegRep>::value); +} + +namespace internal +{ + +/** + * @brief Random specialization for Bundle objects. + */ +template +struct RandomEvaluatorImpl> +{ + static void run(BundleBase & m) + { + run(m, typename BundleBase::IdxList{}); + } + + template + static void run(BundleBase & m, intseq<_Idx...>) + { + m = typename BundleBase::LieGroup( + BundleBase::template PartType<_Idx>::Random() ...); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_BASE_H_ diff --git a/include/manif/impl/bundle/Bundle_map.h b/include/manif/impl/bundle/Bundle_map.h new file mode 100644 index 00000000..7519b865 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_map.h @@ -0,0 +1,96 @@ +#ifndef _MANIF_MANIF_BUNDLE_MAP_H_ +#define _MANIF_MANIF_BUNDLE_MAP_H_ + +#include "manif/impl/bundle/Bundle.h" + +namespace manif +{ +namespace internal +{ + +/** + * @brief traits specialization for Eigen Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = BundleBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +/** + * @brief traits specialization for Eigen const Map + */ +template class ... T> +struct traits, 0>> + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = BundleBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + + +namespace Eigen +{ + +/** + * @brief Specialization of Map for manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleBase, 0>> +{ + using Base = manif::BundleBase, 0>>; + +public: + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Map(Scalar * coeffs) : data_(coeffs) { } + + MANIF_GROUP_MAP_ASSIGN_OP(Bundle) + + DataType & coeffs() {return data_;} + + const DataType & coeffs() const {return data_;} + +protected: + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::Bundle + */ +template class ... T> +class Map, 0> + : public manif::BundleBase, 0>> +{ + using Base = manif::BundleBase, 0>>; + +public: + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + using Base::BundleSize; + + Map(const Scalar * coeffs) : data_(coeffs) { } + + const DataType & coeffs() const {return data_;} + +protected: + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_BUNDLE_MAP_H_ diff --git a/include/manif/impl/bundle/Bundle_properties.h b/include/manif/impl/bundle/Bundle_properties.h new file mode 100644 index 00000000..1e6d0757 --- /dev/null +++ b/include/manif/impl/bundle/Bundle_properties.h @@ -0,0 +1,160 @@ +#ifndef _MANIF_MANIF_BUNDLE_PROPERTIES_H_ +#define _MANIF_MANIF_BUNDLE_PROPERTIES_H_ + +#include "manif/impl/traits.h" + +namespace manif +{ + +// Forward declaration for type traits specialization +template +struct BundleBase; +template +struct BundleTangentBase; + +template class ... _T> +struct Bundle; +template class ... _T> +struct BundleTangent; + +/** + * @brief intseq: std::integer_sequence-equivalent + */ +template +struct intseq +{ + using value_type = int; + static constexpr std::size_t size() noexcept {return sizeof...(_I);} +}; + +namespace internal +{ +namespace bundle +{ + +/** + * @brief join two intseqs + */ +template +struct intseq_join; + +template class _IntSeq, int ... _I1, int ... _I2> +struct intseq_join<_IntSeq<_I1...>, _IntSeq<_I2...>> +{ + using type = _IntSeq<_I1..., _I2...>; +}; + + +/** + * @brief generate intseq 0, 1, ..., N-1 + */ +template +struct make_intseq +{ + using type = typename intseq_join::type, intseq<_N - 1>>::type; +}; + +template<> +struct make_intseq<0> +{ + using type = intseq<>; +}; + + +/** + * @brief extract intseq element + */ +template +struct intseq_element; + +template class _IntSeq, int Head, int ... Tail> +struct intseq_element<_Idx, _IntSeq> + : public intseq_element<_Idx - 1, _IntSeq> +{}; + +template class _IntSeq, int Head, int ... Tail> +struct intseq_element<0, _IntSeq> +{ + static constexpr int value = Head; +}; + + +/** + * @brief sum an intseq + */ +template +struct intseq_sum; + +template class _IntSeq, int I, int ... Is> +struct intseq_sum<_IntSeq> +{ + static constexpr int value = I + intseq_sum<_IntSeq>::value; +}; + +template class _IntSeq> +struct intseq_sum<_IntSeq<>> +{ + static constexpr int value = 0; +}; + + +/** + * @brief prefix-sum an intseq + */ +template +struct intseq_psum; + +template class _IntSeq, int... _Cur, int _Sum> +struct intseq_psum<_IntSeq<_Cur...>, _IntSeq<>, _Sum> +{ + using type = _IntSeq<_Cur...>; +}; + +template class _IntSeq, int _First, int _Sum, int... _Cur, int... _Rem> +struct intseq_psum<_IntSeq<_Cur...>, _IntSeq<_First, _Rem...>, _Sum> + : intseq_psum<_IntSeq<_Cur..., _Sum>, _IntSeq<_Rem...>, _Sum + _First> +{}; + +/** + * @brief prefix-sum an intseq + */ +template +using intseq_psum_t = typename intseq_psum, _Seq, 0>::type; + +/** + * @brief extract element of typelist + */ +template +struct bundle_element; + +// recursive case +template +struct bundle_element + : public bundle_element { }; + +// base case +template +struct bundle_element<0, Head, Tail...> +{ + using type = Head; +}; + +} // namespace bundle +} // namespace internal + + +/** + * @brief generate intseq 0, 1, ..., N-1 + */ +template +using make_intseq = internal::bundle::make_intseq<_N>; + +/** + * @brief generate intseq 0, 1, ..., N-1 + */ +template +using make_intseq_t = typename internal::bundle::make_intseq<_N>::type; + +} // namespace manif + +#endif // _MANIF_MANIF_BUNDLE_PROPERTIES_H_ diff --git a/include/manif/impl/macro.h b/include/manif/impl/macro.h index c01ca735..268fca11 100644 --- a/include/manif/impl/macro.h +++ b/include/manif/impl/macro.h @@ -154,6 +154,7 @@ raise(Args&&... args) X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) { coeffs() = std::move(o); return derived(); } #define MANIF_GROUP_MAP_ASSIGN_OP(X) \ + Map(const Map & o) : Base(), data_(o.coeffs()) { }\ Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ template \ Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ @@ -198,6 +199,7 @@ raise(Args&&... args) X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o); return derived(); } #define MANIF_TANGENT_MAP_ASSIGN_OP(X) \ + Map(const Map & o) : Base(), data_(o.coeffs()) { }\ Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ template \ Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ diff --git a/include/manif/impl/rn/RnTangent.h b/include/manif/impl/rn/RnTangent.h index 094ac921..09cf5a39 100644 --- a/include/manif/impl/rn/RnTangent.h +++ b/include/manif/impl/rn/RnTangent.h @@ -93,15 +93,15 @@ template using R7Tangent = RnTangent<_Scalar, 7>; template using R8Tangent = RnTangent<_Scalar, 8>; template using R9Tangent = RnTangent<_Scalar, 9>; -MANIF_EXTRA_GROUP_TYPEDEF(R1Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R2Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R3Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R4Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R5Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R6Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R7Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R8Tangent) -MANIF_EXTRA_GROUP_TYPEDEF(R9Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R1Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R2Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R3Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R4Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R5Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R6Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R7Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R8Tangent) +MANIF_EXTRA_TANGENT_TYPEDEF(R9Tangent) template template diff --git a/include/manif/impl/rn/Rn_base.h b/include/manif/impl/rn/Rn_base.h index 0c55dea5..6587ae3c 100644 --- a/include/manif/impl/rn/Rn_base.h +++ b/include/manif/impl/rn/Rn_base.h @@ -158,8 +158,6 @@ RnBase<_Derived>::compose( OptJacobianRef J_mc_ma, OptJacobianRef J_mc_mb) const { - using std::abs; - static_assert( std::is_base_of, _DerivedOther>::value, "Argument does not inherit from RnBase !"); diff --git a/include/manif/impl/traits.h b/include/manif/impl/traits.h index f87e17b0..4eb7d9f1 100644 --- a/include/manif/impl/traits.h +++ b/include/manif/impl/traits.h @@ -39,13 +39,17 @@ struct traitscast; /** * @brief Traits to change the scalar type of a template class - * @note given using FooDouble = Foo + * @note given using FooDouble = Foo * using FooFloat = typename traitscast::cast; */ -template