-
Notifications
You must be signed in to change notification settings - Fork 249
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
84 changed files
with
4,304 additions
and
2,986 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,28 @@ | ||
| #ifndef _MANIF_MANIF_IMPL_CRTP_H_ | ||
| #define _MANIF_MANIF_IMPL_CRTP_H_ | ||
|
|
||
| namespace manif { | ||
| namespace internal { | ||
|
|
||
| template <typename Derived> | ||
| struct crtp | ||
| { | ||
| protected: | ||
| /** Return reference to this as derived object */ | ||
| inline Derived &derived() & noexcept { | ||
| return *static_cast<Derived *>(this); | ||
| } | ||
| /** Return reference to this as derived object */ | ||
| inline const Derived &derived() const & noexcept { | ||
| return *static_cast<Derived const *>(this); | ||
| } | ||
| /** Return reference to this as derived object, when this is rvalue */ | ||
| inline Derived &&derived() && noexcept { | ||
| return std::move(*static_cast<Derived *>(this)); | ||
| } | ||
| }; | ||
|
|
||
| } // namespace internal | ||
| } // namespace manif | ||
|
|
||
| #endif // _MANIF_MANIF_IMPL_CRTP_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,33 @@ | ||
| #ifndef _MANIF_MANIF_IMPL_CORE_UNARY_STORAGE_H_ | ||
| #define _MANIF_MANIF_IMPL_CORE_UNARY_STORAGE_H_ | ||
|
|
||
| namespace manif { | ||
| namespace internal { | ||
|
|
||
| template <typename T> | ||
| struct storage_selector { | ||
| using type = T; | ||
| }; | ||
|
|
||
| template <typename T> | ||
| struct storage_selector<T&> { | ||
| using type = T&; | ||
| }; | ||
|
|
||
| template <typename T> | ||
| struct storage_selector<const T&> { | ||
| using type = const T&; | ||
| }; | ||
|
|
||
| template <typename T> | ||
| struct storage_selector<T&&> { | ||
| using type = T; | ||
| }; | ||
|
|
||
| template <typename T> | ||
| using storage_t = typename storage_selector<T>::type; | ||
|
|
||
| } // namespace internal | ||
| } // namespace manif | ||
|
|
||
| #endif // _MANIF_MANIF_IMPL_CORE_UNARY_STORAGE_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,71 @@ | ||
| #ifndef _MANIF_MANIF_IMPL_CORE_UNARY_STORAGE_H_ | ||
| #define _MANIF_MANIF_IMPL_CORE_UNARY_STORAGE_H_ | ||
|
|
||
|
|
||
| namespace manif { | ||
| namespace internal { | ||
|
|
||
| template <typename Derived, typename RhsDerived_> | ||
| struct UnaryStorage { | ||
| private: | ||
| // Hold a reference to leaf expressions to avoid copies, but a copy of other | ||
| // expressions to avoid references to temporaries. | ||
| using RhsStore = internal::storage_t<RhsDerived_>; | ||
|
|
||
| // Helper to lazily instantiate is_constructible check in forwarding constructor | ||
| // We use this inside std::conjunction. | ||
| template <typename...> | ||
| struct copy_not_applicable : std::true_type {}; | ||
|
|
||
| // Case for exactly one argument: need to check whether copy constructor applies | ||
| template <typename Arg> | ||
| struct copy_not_applicable<Arg> | ||
| : tmp::bool_constant<!std::is_same<std::decay_t<Arg>, Derived>{}> {}; | ||
|
|
||
| public: | ||
| // Forward args to rhs (version for one argument) | ||
| // Disable if copy ctor would apply - https://stackoverflow.com/a/39646176 | ||
| template <typename Arg, | ||
| std::enable_if_t<tmp::conjunction<copy_not_applicable<Arg>, | ||
| std::is_constructible<RhsStore, Arg>>{}, | ||
| bool> = true> | ||
| explicit UnaryStorage(Arg &&arg) : rhs_{std::forward<Arg>(arg)} {} | ||
|
|
||
| // Forward args to rhs (version for 0 or more than 1 argument) | ||
| template < | ||
| typename... Args, | ||
| std::enable_if_t<tmp::conjunction<copy_not_applicable<Args...>, | ||
| std::is_constructible<RhsStore, Args...>>{}, | ||
| bool> = true> | ||
| explicit UnaryStorage(Args &&... args) : rhs_{std::forward<Args>(args)...} {} | ||
|
|
||
| UnaryStorage() = delete; | ||
| /** Copy constructor */ | ||
| UnaryStorage(const UnaryStorage &) = default; | ||
| /** Move constructor */ | ||
| UnaryStorage(UnaryStorage &&) = default; | ||
| /** Copy assignment operator */ | ||
| UnaryStorage &operator=(const UnaryStorage &) = default; | ||
| /** Move assignment operator */ | ||
| UnaryStorage &operator=(UnaryStorage &&) = default; | ||
|
|
||
| const RhsStore &rhs() const & { | ||
| return rhs_; | ||
| } | ||
|
|
||
| RhsStore &rhs() & { | ||
| return rhs_; | ||
| } | ||
|
|
||
| RhsStore rhs() && { | ||
| return rhs_; | ||
| } | ||
|
|
||
| protected: | ||
| RhsStore rhs_; | ||
| }; | ||
|
|
||
| } // namespace internal | ||
| } // namespace manif | ||
|
|
||
| #endif // _MANIF_MANIF_IMPL_CORE_UNARY_STORAGE_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,62 @@ | ||
| #ifndef _MANIF_MANIF_IMPL_EXPR_ACT_H_ | ||
| #define _MANIF_MANIF_IMPL_EXPR_ACT_H_ | ||
|
|
||
| namespace manif { | ||
|
|
||
| // MANIF_DECLARE_BINARY_EXPR(Act); | ||
|
|
||
| template <typename _Lhs, typename _Rhs> | ||
| struct ActExpr | ||
| : internal::ExprBase<ActExpr<_Lhs, _Rhs>> | ||
| { | ||
| using ExprDerived = ActExpr<_Lhs, _Rhs>; | ||
| using Base = internal::ExprBase<ExprDerived>; | ||
|
|
||
| using LieGroup = typename _Lhs::LieGroup; | ||
| using Scalar = typename internal::traits<LieGroup>::Scalar; | ||
|
|
||
| using LhsOptJacobianRef = ::tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, internal::traits<LieGroup>::Dim, internal::traits<LieGroup>::DoF>>>; | ||
| using RhsOptJacobianRef = ::tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, internal::traits<LieGroup>::Dim, internal::traits<LieGroup>::Dim>>>; | ||
|
|
||
| using Ret = ReturnType<ExprDerived>; | ||
|
|
||
| using Base::derived; | ||
| using Base::eval; | ||
|
|
||
| ActExpr(const _Lhs& lhsptr, | ||
| const _Rhs& rhsptr, | ||
| LhsOptJacobianRef& J_ret_lhs = {}, | ||
| RhsOptJacobianRef& J_ret_rhs = {}) | ||
| : lhsptr_(lhsptr) | ||
| , rhsptr_(rhsptr) | ||
| , J_ret_lhs_(J_ret_lhs) | ||
| , J_ret_rhs_(J_ret_rhs) { } | ||
|
|
||
| protected: | ||
|
|
||
| friend Base; | ||
| Ret evalImpl() const | ||
| { | ||
| return internal::ExprEvaluator<ExprDerived>::run( | ||
| lhsptr_, rhsptr_, J_ret_lhs_, J_ret_rhs_); | ||
| } | ||
|
|
||
| const _Lhs& lhsptr_; | ||
| const _Rhs& rhsptr_; | ||
| mutable LhsOptJacobianRef J_ret_lhs_; | ||
| mutable RhsOptJacobianRef J_ret_rhs_; | ||
| }; | ||
|
|
||
| namespace internal { | ||
|
|
||
| template <template <typename _Derived> class _LieGroupBase, | ||
| typename _Derived, typename _DerivedOther> | ||
| struct ReturnTypeHelper<ActExpr<_LieGroupBase<_Derived>, _DerivedOther>> | ||
| { | ||
| using type = typename traits<_Derived>::Vector; | ||
| }; | ||
|
|
||
| } // namespace internal | ||
| } // namespace manif | ||
|
|
||
| #endif // _MANIF_MANIF_IMPL_EXPR_ACT_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,21 @@ | ||
| #ifndef _MANIF_MANIF_IMPL_EXPR_ADJ_H_ | ||
| #define _MANIF_MANIF_IMPL_EXPR_ADJ_H_ | ||
|
|
||
| namespace manif { | ||
|
|
||
| MANIF_DECLARE_UNARY_EXPR(Adj); | ||
|
|
||
| namespace internal { | ||
|
|
||
| // Default expression return type | ||
|
|
||
| template <template <typename _LieGroupDerived> class _LieGroupDerivedBase, typename _LieGroupDerived> | ||
| struct ReturnTypeHelper<AdjExpr<_LieGroupDerivedBase<_LieGroupDerived>>> | ||
| { | ||
| using type = typename internal::traits<_LieGroupDerived>::Jacobian; | ||
| }; | ||
|
|
||
| } // namespace internal | ||
| } // namespace manif | ||
|
|
||
| #endif // _MANIF_MANIF_IMPL_EXPR_ADJ_H_ |
Oops, something went wrong.