From 2133b390ecfff794787f3057fdfa8c4df437a72e Mon Sep 17 00:00:00 2001 From: artivis Date: Sat, 25 Jul 2020 09:48:38 -0400 Subject: [PATCH 1/6] lt::optional explicit base constructor call --- external/lt/lt/optional.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/external/lt/lt/optional.hpp b/external/lt/lt/optional.hpp index b1d26a67..34e4f47c 100644 --- a/external/lt/lt/optional.hpp +++ b/external/lt/lt/optional.hpp @@ -391,7 +391,8 @@ struct optional_copy_base : optional_operations_base { using optional_operations_base::optional_operations_base; optional_copy_base() = default; - optional_copy_base(const optional_copy_base &rhs) { + optional_copy_base(const optional_copy_base &rhs) + : optional_operations_base() { if (rhs.has_value()) { this->construct(rhs.get()); } else { From a6a0e00da6b8e1f9a8b8ff271457d5159dcbc08c Mon Sep 17 00:00:00 2001 From: artivis Date: Sat, 25 Jul 2020 10:01:28 -0400 Subject: [PATCH 2/6] review copy constr/assign --- include/manif/impl/assignment_assert.h | 24 +++++ include/manif/impl/lie_group_base.h | 103 +++++++++++++------- include/manif/impl/macro.h | 72 ++++++++++++++ include/manif/impl/rn/Rn.h | 40 ++------ include/manif/impl/rn/RnTangent.h | 42 ++------ include/manif/impl/rn/RnTangent_base.h | 11 ++- include/manif/impl/rn/RnTangent_map.h | 2 + include/manif/impl/rn/Rn_base.h | 26 ++--- include/manif/impl/rn/Rn_map.h | 3 +- include/manif/impl/se2/SE2.h | 58 ++++------- include/manif/impl/se2/SE2Tangent.h | 42 ++------ include/manif/impl/se2/SE2Tangent_base.h | 14 ++- include/manif/impl/se2/SE2Tangent_map.h | 2 + include/manif/impl/se2/SE2_base.h | 26 +++++ include/manif/impl/se2/SE2_map.h | 2 + include/manif/impl/se3/SE3.h | 61 +++--------- include/manif/impl/se3/SE3Tangent.h | 39 ++------ include/manif/impl/se3/SE3Tangent_base.h | 11 ++- include/manif/impl/se3/SE3Tangent_map.h | 2 + include/manif/impl/se3/SE3_base.h | 26 +++++ include/manif/impl/se3/SE3_map.h | 2 + include/manif/impl/so2/SO2.h | 47 ++------- include/manif/impl/so2/SO2Tangent.h | 42 ++------ include/manif/impl/so2/SO2Tangent_base.h | 11 ++- include/manif/impl/so2/SO2Tangent_map.h | 2 + include/manif/impl/so2/SO2_base.h | 25 +++++ include/manif/impl/so2/SO2_map.h | 2 + include/manif/impl/so3/SO3.h | 47 ++------- include/manif/impl/so3/SO3Tangent.h | 42 ++------ include/manif/impl/so3/SO3Tangent_base.h | 11 ++- include/manif/impl/so3/SO3Tangent_map.h | 2 + include/manif/impl/so3/SO3_base.h | 25 +++++ include/manif/impl/so3/SO3_map.h | 2 + include/manif/impl/tangent_base.h | 117 +++++++++++------------ 34 files changed, 495 insertions(+), 488 deletions(-) create mode 100644 include/manif/impl/assignment_assert.h diff --git a/include/manif/impl/assignment_assert.h b/include/manif/impl/assignment_assert.h new file mode 100644 index 00000000..db35dbf8 --- /dev/null +++ b/include/manif/impl/assignment_assert.h @@ -0,0 +1,24 @@ +#ifndef _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_ +#define _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_ + +namespace manif { +namespace internal { + +template +struct AssignmentEvaluatorImpl +{ + template static void run_impl(const T&) { } +}; + +template +struct AssignmentEvaluator : AssignmentEvaluatorImpl +{ + using Base = AssignmentEvaluatorImpl; + + template void run(T&& t) { Base::run_impl(std::forward(t)); } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_ diff --git a/include/manif/impl/lie_group_base.h b/include/manif/impl/lie_group_base.h index 17bbd7b7..a729972b 100644 --- a/include/manif/impl/lie_group_base.h +++ b/include/manif/impl/lie_group_base.h @@ -5,6 +5,7 @@ #include "manif/impl/traits.h" #include "manif/impl/eigen.h" #include "manif/impl/tangent_base.h" +#include "manif/impl/assignment_assert.h" #include "manif/constants.h" @@ -41,6 +42,37 @@ struct LieGroupBase //! @brief Helper for skipping an optional parameter. static const OptJacobianRef _; +protected: + + MANIF_DEFAULT_CONSTRUCTOR(LieGroupBase) + +public: + + /** + * @brief Assignment operator. + * @param[in] An element of the Lie group. + * @return A reference to this. + * @note This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + _Derived& operator =(const LieGroupBase& m); + + /** + * @brief Assignment operator. + * @param[in] An element of the Lie group. + * @return A reference to this. + */ + template + _Derived& operator =(const LieGroupBase<_DerivedOther>& m); + + /** + * @brief Assignment operator given Eigen object. + * @param[in] An element of the Lie group. + * @return A reference to this. + */ + template + _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& data); + //! @brief Access the underlying data by const reference DataType& coeffs(); @@ -234,21 +266,6 @@ struct LieGroupBase // Some operators - /** - * @brief Assignment operator. - * @param[in] An element of the Lie group. - * @return A reference to this. - */ - _Derived& operator =(const LieGroupBase<_Derived>& m); - - /** - * @brief Assignment operator. - * @param[in] An element of the Lie group. - * @return A reference to this. - */ - template - _Derived& operator =(const LieGroupBase<_DerivedOther>& m); - /** * @brief Equality operator. * @param[in] An element of the same Lie group. @@ -301,10 +318,10 @@ struct LieGroupBase //! Static helper to create a random object of the Lie group. static LieGroup Random(); -private: +protected: - _Derived& derived() { return *static_cast< _Derived* >(this); } - const _Derived& derived() const { return *static_cast< const _Derived* >(this); } + inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); } + inline const _Derived& derived() const & noexcept { return *static_cast< const _Derived* >(this); } }; template @@ -318,6 +335,37 @@ template const typename LieGroupBase<_Derived>::OptJacobianRef LieGroupBase<_Derived>::_ = {}; +// Copy + +template +_Derived& +LieGroupBase<_Derived>::operator =(const LieGroupBase& m) +{ + derived().coeffs() = m.coeffs(); + return derived(); +} + +template +template +_Derived& +LieGroupBase<_Derived>::operator =(const LieGroupBase<_DerivedOther>& m) +{ + derived().coeffs() = m.coeffs(); + return derived(); +} + +template +template +_Derived& +LieGroupBase<_Derived>::operator =(const Eigen::MatrixBase<_EigenDerived>& data) +{ + internal::AssignmentEvaluator< + typename internal::traits<_Derived>::Base>().run(data); + + derived().coeffs() = data; + return derived(); +} + template typename LieGroupBase<_Derived>::DataType& LieGroupBase<_Derived>::coeffs() @@ -563,25 +611,6 @@ LieGroupBase<_Derived>::act(const Vector& v, // Operators -template -_Derived& -LieGroupBase<_Derived>::operator =( - const LieGroupBase<_Derived>& m) -{ - derived().coeffs() = m.coeffs(); - return derived(); -} - -template -template -_Derived& -LieGroupBase<_Derived>::operator =( - const LieGroupBase<_DerivedOther>& m) -{ - derived().coeffs() = m.coeffs(); - return derived(); -} - template template bool LieGroupBase<_Derived>::operator ==( diff --git a/include/manif/impl/macro.h b/include/manif/impl/macro.h index 3fc75f06..571a1c63 100644 --- a/include/manif/impl/macro.h +++ b/include/manif/impl/macro.h @@ -116,6 +116,78 @@ raise(Args&&... args) #define MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(X) \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF((Eigen::internal::traits::Alignment>0)) +#define MANIF_DEFAULT_CONSTRUCTOR(X) \ + X() = default; \ + ~X() = default; \ + X(const X&) = default; \ + X(X&&) = delete; + +#define MANIF_GROUP_ML_ASSIGN_OP(X) \ + _Derived& operator =(const X& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + _Derived& operator =(const LieGroupBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); } + +#define MANIF_GROUP_ASSIGN_OP(X) \ + X& operator=(const X& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + X& operator =(const X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + X& operator =(const LieGroupBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + X& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); } + +#define MANIF_GROUP_MAP_ASSIGN_OP(X) \ + Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ + template \ + Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ + template \ + Map& operator =(const manif::LieGroupBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ + template \ + Map& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return *this; } + +#define MANIF_TANGENT_ML_ASSIGN_OP(X) \ + _Derived& operator=(const X& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + _Derived& operator =(const TangentBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); } + +#define MANIF_TANGENT_ASSIGN_OP(X) \ + X& operator=(const X& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + X& operator =(const X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + X& operator =(const TangentBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\ + template \ + X& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); } + +#define MANIF_TANGENT_MAP_ASSIGN_OP(X) \ + Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\ + template \ + Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ + template \ + Map& operator =(const manif::TangentBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\ + template \ + Map& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return *this; } + +/** + * @brief Automatically define: + * - copy constructor + * - copy constructor given Base object + * - copy constructor given Eigen object + */ +#define MANIF_COPY_CONSTRUCTOR(X) \ + X(const X& o) : Base(), data_(o.coeffs()) { } \ + X(const Base& o) : Base(), data_(o.coeffs()) { } \ + template X(const Eigen::MatrixBase& o) : Base(), data_(o) \ + { manif::internal::AssignmentEvaluator().run(o); } + +#define MANIF_COEFFS_FUNCTIONS() \ + DataType& coeffs() & { return data_; } \ + const DataType& coeffs() const & { return data_; } + // LieGroup - related macros #define MANIF_INHERIT_GROUP_AUTO_API \ diff --git a/include/manif/impl/rn/Rn.h b/include/manif/impl/rn/Rn.h index 579a8adb..011fcfb5 100644 --- a/include/manif/impl/rn/Rn.h +++ b/include/manif/impl/rn/Rn.h @@ -56,6 +56,10 @@ struct Rn : RnBase> using Base = RnBase>; using Type = Rn<_Scalar, _N>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -66,22 +70,17 @@ struct Rn : RnBase> Rn() = default; ~Rn() = default; - // Copy constructor given base - Rn(const Base& o); - - template - Rn(const RnBase<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(Rn) + // Copy constructor given base template Rn(const LieGroupBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - Rn(const Eigen::MatrixBase<_EigenDerived>& data); + MANIF_GROUP_ASSIGN_OP(Rn) // LieGroup common API - //! Get a const reference to the underlying DataType. + //! Get a reference to the underlying DataType. DataType& coeffs(); //! Get a const reference to the underlying DataType. @@ -114,29 +113,6 @@ MANIF_EXTRA_GROUP_TYPEDEF(R7) MANIF_EXTRA_GROUP_TYPEDEF(R8) MANIF_EXTRA_GROUP_TYPEDEF(R9) -template -template -Rn<_Scalar, _N>::Rn(const Eigen::MatrixBase<_EigenDerived>& data) - : data_(data) -{ - // -} - -template -Rn<_Scalar, _N>::Rn(const Base& o) - : Rn(o.coeffs()) -{ - // -} - -template -template -Rn<_Scalar, _N>::Rn(const RnBase<_DerivedOther>& o) - : Rn(o.coeffs()) -{ - // -} - template template Rn<_Scalar, _N>::Rn(const LieGroupBase<_DerivedOther>& o) diff --git a/include/manif/impl/rn/RnTangent.h b/include/manif/impl/rn/RnTangent.h index 88f50b47..446cb98e 100644 --- a/include/manif/impl/rn/RnTangent.h +++ b/include/manif/impl/rn/RnTangent.h @@ -51,6 +51,10 @@ struct RnTangent : RnTangentBase> using Base = RnTangentBase>; using Type = RnTangent<_Scalar, _N>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -62,17 +66,13 @@ struct RnTangent : RnTangentBase> RnTangent() = default; ~RnTangent() = default; - // Copy constructor given base - RnTangent(const Base& o); - template - RnTangent(const RnTangentBase<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(RnTangent) + // Copy constructor given base template RnTangent(const TangentBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - RnTangent(const Eigen::MatrixBase<_EigenDerived>& theta); + MANIF_TANGENT_ASSIGN_OP(RnTangent) // Tangent common API @@ -104,40 +104,14 @@ MANIF_EXTRA_GROUP_TYPEDEF(R7Tangent) MANIF_EXTRA_GROUP_TYPEDEF(R8Tangent) MANIF_EXTRA_GROUP_TYPEDEF(R9Tangent) -template -RnTangent<_Scalar, _N>::RnTangent(const Base& o) - : data_(o.coeffs()) -{ - // -} - template template -RnTangent<_Scalar, _N>::RnTangent( - const RnTangentBase<_DerivedOther>& o) +RnTangent<_Scalar, _N>::RnTangent(const TangentBase<_DerivedOther>& o) : data_(o.coeffs()) { // } -template -template -RnTangent<_Scalar, _N>::RnTangent( - const TangentBase<_DerivedOther>& o) - : data_(o.coeffs()) -{ - // -} - -template -template -RnTangent<_Scalar, _N>::RnTangent( - const Eigen::MatrixBase<_EigenDerived>& theta) - : data_(theta) -{ - // -} - template typename RnTangent<_Scalar, _N>::DataType& RnTangent<_Scalar, _N>::coeffs() diff --git a/include/manif/impl/rn/RnTangent_base.h b/include/manif/impl/rn/RnTangent_base.h index a3ef66b5..325bd886 100644 --- a/include/manif/impl/rn/RnTangent_base.h +++ b/include/manif/impl/rn/RnTangent_base.h @@ -28,8 +28,15 @@ struct RnTangentBase : TangentBase<_Derived> MANIF_INHERIT_TANGENT_OPERATOR using Base::coeffs; - RnTangentBase() = default; - ~RnTangentBase() = default; +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(RnTangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(RnTangentBase) // Tangent common API diff --git a/include/manif/impl/rn/RnTangent_map.h b/include/manif/impl/rn/RnTangent_map.h index 2887c662..4193630a 100644 --- a/include/manif/impl/rn/RnTangent_map.h +++ b/include/manif/impl/rn/RnTangent_map.h @@ -48,6 +48,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_TANGENT_MAP_ASSIGN_OP(RnTangent) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/rn/Rn_base.h b/include/manif/impl/rn/Rn_base.h index 2b9d6095..0c55dea5 100644 --- a/include/manif/impl/rn/Rn_base.h +++ b/include/manif/impl/rn/Rn_base.h @@ -35,6 +35,15 @@ struct RnBase : LieGroupBase<_Derived> using Transformation = typename internal::traits<_Derived>::Transformation; // LieGroup common API +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(RnBase) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(RnBase) /** * @brief Get the inverse of this. @@ -103,14 +112,6 @@ struct RnBase : LieGroupBase<_Derived> * | 0 1 | */ Transformation transform() const; - - /** - * @brief Assignment operator. - * @param[in] v An Eigen MatrixBase (vector expected). - * @return A reference to this. - */ - template - _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& v); }; template @@ -122,15 +123,6 @@ RnBase<_Derived>::transform() const return T; } -template -template -_Derived& RnBase<_Derived>::operator =( - const Eigen::MatrixBase<_EigenDerived>& v) -{ - coeffs() = v; - return *static_cast< _Derived* >(this); -} - template typename RnBase<_Derived>::LieGroup RnBase<_Derived>::inverse(OptJacobianRef J_minv_m) const diff --git a/include/manif/impl/rn/Rn_map.h b/include/manif/impl/rn/Rn_map.h index 7710eeea..1168e14b 100644 --- a/include/manif/impl/rn/Rn_map.h +++ b/include/manif/impl/rn/Rn_map.h @@ -49,8 +49,9 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } - DataType& coeffs() { return data_; } + MANIF_GROUP_MAP_ASSIGN_OP(Rn) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } protected: diff --git a/include/manif/impl/se2/SE2.h b/include/manif/impl/se2/SE2.h index d3a867e6..ce453399 100644 --- a/include/manif/impl/se2/SE2.h +++ b/include/manif/impl/se2/SE2.h @@ -56,6 +56,10 @@ struct SE2 : SE2Base> using Base = SE2Base>; using Type = SE2<_Scalar>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -70,18 +74,13 @@ struct SE2 : SE2Base> SE2() = default; ~SE2() = default; - // Copy constructor given base - SE2(const Base& o); - - template - SE2(const SE2Base<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SE2) + // Copy constructor template SE2(const LieGroupBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SE2(const Eigen::MatrixBase<_EigenDerived>& data); + MANIF_GROUP_ASSIGN_OP(SE2) /** * @brief Constructor given a translation and a unit complex number. @@ -133,7 +132,16 @@ struct SE2 : SE2Base> // LieGroup common API + /** + * @brief Access the underlying data + * @param[out] a reference to the underlying Eigen vector + */ DataType& coeffs(); + + /** + * @brief Access the underlying data + * @param[out] a const reference to the underlying Eigen vector + */ const DataType& coeffs() const; // SE2 specific API @@ -146,43 +154,15 @@ struct SE2 : SE2Base> protected: + //! Underlying data (Eigen) vector DataType data_; }; MANIF_EXTRA_GROUP_TYPEDEF(SE2) -template -template -SE2<_Scalar>::SE2(const Eigen::MatrixBase<_EigenDerived>& data) - : data_(data) -{ - using std::abs; - MANIF_ASSERT(abs(data_.template tail<2>().norm()-Scalar(1)) < - Constants::eps_s, - "SE2 constructor argument not normalized !", - invalid_argument); -} - -template -SE2<_Scalar>::SE2(const Base& o) - : SE2(o.coeffs()) -{ - // -} - -template -template -SE2<_Scalar>::SE2( - const SE2Base<_DerivedOther>& o) - : SE2(o.coeffs()) -{ - // -} - template template -SE2<_Scalar>::SE2( - const LieGroupBase<_DerivedOther>& o) +SE2<_Scalar>::SE2(const LieGroupBase<_DerivedOther>& o) : SE2(o.coeffs()) { // @@ -220,7 +200,7 @@ SE2<_Scalar>::SE2(const Scalar x, const Scalar y, const std::complex& c) } template -SE2<_Scalar>::SE2(const Eigen::Transform<_Scalar,2,Eigen::Isometry>& h) +SE2<_Scalar>::SE2(const Eigen::Transform<_Scalar, 2, Eigen::Isometry>& h) : SE2(h.translation().x(), h.translation().y(), Eigen::Rotation2D(h.rotation()).angle()) { // diff --git a/include/manif/impl/se2/SE2Tangent.h b/include/manif/impl/se2/SE2Tangent.h index 787cc272..d2fadf0b 100644 --- a/include/manif/impl/se2/SE2Tangent.h +++ b/include/manif/impl/se2/SE2Tangent.h @@ -49,6 +49,10 @@ struct SE2Tangent : SE2TangentBase> using Base = SE2TangentBase>; using Type = SE2Tangent<_Scalar>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -60,17 +64,13 @@ struct SE2Tangent : SE2TangentBase> SE2Tangent() = default; ~SE2Tangent() = default; - // Copy constructor given base - SE2Tangent(const Base& o); - template - SE2Tangent(const SE2TangentBase<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SE2Tangent) + // Copy constructor given base template SE2Tangent(const TangentBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SE2Tangent(const Eigen::MatrixBase<_EigenDerived>& v); + MANIF_TANGENT_ASSIGN_OP(SE2Tangent) SE2Tangent(const Scalar x, const Scalar y, const Scalar theta); @@ -90,40 +90,14 @@ struct SE2Tangent : SE2TangentBase> MANIF_EXTRA_TANGENT_TYPEDEF(SE2Tangent); -template -SE2Tangent<_Scalar>::SE2Tangent(const Base& o) - : data_(o.coeffs()) -{ - // -} - template template -SE2Tangent<_Scalar>::SE2Tangent( - const SE2TangentBase<_DerivedOther>& o) +SE2Tangent<_Scalar>::SE2Tangent(const TangentBase<_DerivedOther>& o) : data_(o.coeffs()) { // } -template -template -SE2Tangent<_Scalar>::SE2Tangent( - const TangentBase<_DerivedOther>& o) - : data_(o.coeffs()) -{ - // -} - -template -template -SE2Tangent<_Scalar>::SE2Tangent( - const Eigen::MatrixBase<_EigenDerived>& v) - : data_(v) -{ - // -} - template SE2Tangent<_Scalar>::SE2Tangent(const Scalar x, const Scalar y, diff --git a/include/manif/impl/se2/SE2Tangent_base.h b/include/manif/impl/se2/SE2Tangent_base.h index f87a840f..ffbf551a 100644 --- a/include/manif/impl/se2/SE2Tangent_base.h +++ b/include/manif/impl/se2/SE2Tangent_base.h @@ -24,15 +24,23 @@ struct SE2TangentBase : TangentBase<_Derived> public: - SE2TangentBase() = default; - ~SE2TangentBase() = default; - MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API MANIF_INHERIT_TANGENT_OPERATOR using Base::data; using Base::coeffs; +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SE2TangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(SE2TangentBase) + // Tangent common API /** diff --git a/include/manif/impl/se2/SE2Tangent_map.h b/include/manif/impl/se2/SE2Tangent_map.h index b3d0ab41..0f6a7839 100644 --- a/include/manif/impl/se2/SE2Tangent_map.h +++ b/include/manif/impl/se2/SE2Tangent_map.h @@ -50,6 +50,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_TANGENT_MAP_ASSIGN_OP(SE2Tangent) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/se2/SE2_base.h b/include/manif/impl/se2/SE2_base.h index a94b315c..5084ce2e 100644 --- a/include/manif/impl/se2/SE2_base.h +++ b/include/manif/impl/se2/SE2_base.h @@ -38,6 +38,16 @@ struct SE2Base : LieGroupBase<_Derived> // LieGroup common API +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SE2Base) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(SE2Base) + /** * @brief Get the inverse of this. * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. @@ -396,6 +406,22 @@ struct RandomEvaluatorImpl> } }; +//! @brief Assignment assert specialization for SE2Base objects +template +struct AssignmentEvaluatorImpl> +{ + template + static void run_impl(const T& data) + { + using std::abs; + using Scalar = typename SE2Base::Scalar; + MANIF_CHECK(abs(data.template tail<2>().norm()-Scalar(1)) < + Constants::eps_s, + "SE2 assigned data not normalized !", + invalid_argument); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se2/SE2_map.h b/include/manif/impl/se2/SE2_map.h index c540f26d..b5577c75 100644 --- a/include/manif/impl/se2/SE2_map.h +++ b/include/manif/impl/se2/SE2_map.h @@ -51,6 +51,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_GROUP_MAP_ASSIGN_OP(SE2) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/se3/SE3.h b/include/manif/impl/se3/SE3.h index 5cd95dd2..eba9f4b6 100644 --- a/include/manif/impl/se3/SE3.h +++ b/include/manif/impl/se3/SE3.h @@ -59,6 +59,10 @@ struct SE3 : SE3Base> using Base = SE3Base>; using Type = SE3<_Scalar>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -75,18 +79,12 @@ struct SE3 : SE3Base> SE3() = default; ~SE3() = default; - // Copy constructor given base - SE3(const Base& o); - - template - SE3(const SE3Base<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SE3) template SE3(const LieGroupBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SE3(const Eigen::MatrixBase<_EigenDerived>& data); + MANIF_GROUP_ASSIGN_OP(SE3) /** * @brief Constructor given a translation and a unit quaternion. @@ -149,54 +147,23 @@ struct SE3 : SE3Base> MANIF_EXTRA_GROUP_TYPEDEF(SE3) -template -template -SE3<_Scalar>::SE3(const Eigen::MatrixBase<_EigenDerived>& data) - : data_(data) -{ - using std::abs; - MANIF_ASSERT(abs(data_.template tail<4>().norm()-Scalar(1)) < - Constants::eps_s, - "SE3 constructor argument not normalized !", - invalid_argument); -} - -template -SE3<_Scalar>::SE3(const Base& o) - : SE3(o.coeffs()) -{ - // -} - template template -SE3<_Scalar>::SE3( - const SE3Base<_DerivedOther>& o) +SE3<_Scalar>::SE3(const LieGroupBase<_DerivedOther>& o) : SE3(o.coeffs()) { // } template -template -SE3<_Scalar>::SE3( - const LieGroupBase<_DerivedOther>& o) - : SE3(o.coeffs()) -{ - // -} - -template -SE3<_Scalar>::SE3(const Translation& t, - const Eigen::Quaternion& q) +SE3<_Scalar>::SE3(const Translation& t, const Eigen::Quaternion& q) : SE3((DataType() << t, q.coeffs() ).finished()) { // } template -SE3<_Scalar>::SE3(const Translation& t, - const Eigen::AngleAxis& a) +SE3<_Scalar>::SE3(const Translation& t, const Eigen::AngleAxis& a) : SE3(t, Quaternion(a)) { // @@ -206,16 +173,15 @@ template SE3<_Scalar>::SE3(const Scalar x, const Scalar y, const Scalar z, const Scalar roll, const Scalar pitch, const Scalar yaw) : SE3(Translation(x,y,z), Eigen::Quaternion( - Eigen::AngleAxis(yaw, Eigen::Matrix::UnitZ()) * - Eigen::AngleAxis(pitch, Eigen::Matrix::UnitY()) * - Eigen::AngleAxis(roll, Eigen::Matrix::UnitX()) )) + Eigen::AngleAxis(yaw, Eigen::Matrix::UnitZ()) * + Eigen::AngleAxis(pitch, Eigen::Matrix::UnitY()) * + Eigen::AngleAxis(roll, Eigen::Matrix::UnitX()) )) { // } template -SE3<_Scalar>::SE3(const Translation& t, - const SO3& so3) +SE3<_Scalar>::SE3(const Translation& t, const SO3& so3) : SE3(t, so3.quat()) { // @@ -228,7 +194,6 @@ SE3<_Scalar>::SE3(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h) // } - template typename SE3<_Scalar>::DataType& SE3<_Scalar>::coeffs() diff --git a/include/manif/impl/se3/SE3Tangent.h b/include/manif/impl/se3/SE3Tangent.h index d76b7966..d6a2210b 100644 --- a/include/manif/impl/se3/SE3Tangent.h +++ b/include/manif/impl/se3/SE3Tangent.h @@ -49,6 +49,10 @@ struct SE3Tangent : SE3TangentBase> using Base = SE3TangentBase>; using Type = SE3Tangent<_Scalar>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -60,17 +64,13 @@ struct SE3Tangent : SE3TangentBase> SE3Tangent() = default; ~SE3Tangent() = default; - // Copy constructor given base - SE3Tangent(const Base& o); - template - SE3Tangent(const SE3TangentBase<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SE3Tangent) + // Copy constructor given base template SE3Tangent(const TangentBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SE3Tangent(const Eigen::MatrixBase<_EigenDerived>& v); + MANIF_TANGENT_ASSIGN_OP(SE3Tangent) // Tangent common API @@ -86,22 +86,6 @@ struct SE3Tangent : SE3TangentBase> MANIF_EXTRA_TANGENT_TYPEDEF(SE3Tangent); -template -SE3Tangent<_Scalar>::SE3Tangent(const Base& o) - : data_(o.coeffs()) -{ - // -} - -template -template -SE3Tangent<_Scalar>::SE3Tangent( - const SE3TangentBase<_DerivedOther>& o) - : data_(o.coeffs()) -{ - // -} - template template SE3Tangent<_Scalar>::SE3Tangent( @@ -111,15 +95,6 @@ SE3Tangent<_Scalar>::SE3Tangent( // } -template -template -SE3Tangent<_Scalar>::SE3Tangent( - const Eigen::MatrixBase<_EigenDerived>& v) - : data_(v) -{ - // -} - template typename SE3Tangent<_Scalar>::DataType& SE3Tangent<_Scalar>::coeffs() diff --git a/include/manif/impl/se3/SE3Tangent_base.h b/include/manif/impl/se3/SE3Tangent_base.h index 09c28bf7..bfcdb2d0 100644 --- a/include/manif/impl/se3/SE3Tangent_base.h +++ b/include/manif/impl/se3/SE3Tangent_base.h @@ -36,8 +36,15 @@ struct SE3TangentBase : TangentBase<_Derived> using Base::data; using Base::coeffs; - SE3TangentBase() = default; - ~SE3TangentBase() = default; +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SE3TangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(SE3TangentBase) // Tangent common API diff --git a/include/manif/impl/se3/SE3Tangent_map.h b/include/manif/impl/se3/SE3Tangent_map.h index 4e8aeab6..f38c1211 100644 --- a/include/manif/impl/se3/SE3Tangent_map.h +++ b/include/manif/impl/se3/SE3Tangent_map.h @@ -50,6 +50,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_TANGENT_MAP_ASSIGN_OP(SE3Tangent) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/se3/SE3_base.h b/include/manif/impl/se3/SE3_base.h index d0d943c6..834c043d 100644 --- a/include/manif/impl/se3/SE3_base.h +++ b/include/manif/impl/se3/SE3_base.h @@ -42,6 +42,16 @@ struct SE3Base : LieGroupBase<_Derived> // LieGroup common API +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SE3Base) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(SE3Base) + /** * @brief Get the inverse. * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. @@ -396,6 +406,22 @@ struct RandomEvaluatorImpl> } }; +//! @brief Assignment assert specialization for SE2Base objects +template +struct AssignmentEvaluatorImpl> +{ + template + static void run_impl(const T& data) + { + using std::abs; + using Scalar = typename SE3Base::Scalar; + MANIF_CHECK(abs(data.template tail<4>().norm()-Scalar(1)) < + Constants::eps_s, + "SE3 assigned data not normalized !", + invalid_argument); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/se3/SE3_map.h b/include/manif/impl/se3/SE3_map.h index a4e8478f..2888b020 100644 --- a/include/manif/impl/se3/SE3_map.h +++ b/include/manif/impl/se3/SE3_map.h @@ -51,6 +51,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_GROUP_MAP_ASSIGN_OP(SE3) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/so2/SO2.h b/include/manif/impl/so2/SO2.h index d6cd3dee..7e6c2deb 100644 --- a/include/manif/impl/so2/SO2.h +++ b/include/manif/impl/so2/SO2.h @@ -63,21 +63,22 @@ struct SO2 : SO2Base> using Base::rotation; using Base::normalize; +protected: + + using Base::derived; + +public: + SO2() = default; ~SO2() = default; - // Copy constructor given base - SO2(const Base& o); - - template - SO2(const SO2Base<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SO2) + // Copy constructor given base template SO2(const LieGroupBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SO2(const Eigen::MatrixBase<_EigenDerived>& data); + MANIF_GROUP_ASSIGN_OP(SO2) /** * @brief Constructor given the real and imaginary part @@ -108,37 +109,9 @@ struct SO2 : SO2Base> MANIF_EXTRA_GROUP_TYPEDEF(SO2) -template -template -SO2<_Scalar>::SO2(const Eigen::MatrixBase<_EigenDerived>& data) - : data_(data) -{ - using std::abs; - MANIF_ASSERT(abs(data_.norm()-Scalar(1)) < Constants::eps_s, - "SO2 constructor argument not normalized !", - invalid_argument); -} - -template -SO2<_Scalar>::SO2(const Base& o) - : SO2(o.coeffs()) -{ - // -} - -template -template -SO2<_Scalar>::SO2( - const SO2Base<_DerivedOther>& o) - : SO2(o.coeffs()) -{ - // -} - template template -SO2<_Scalar>::SO2( - const LieGroupBase<_DerivedOther>& o) +SO2<_Scalar>::SO2(const LieGroupBase<_DerivedOther>& o) : SO2(o.coeffs()) { // diff --git a/include/manif/impl/so2/SO2Tangent.h b/include/manif/impl/so2/SO2Tangent.h index a0d66626..5945f620 100644 --- a/include/manif/impl/so2/SO2Tangent.h +++ b/include/manif/impl/so2/SO2Tangent.h @@ -49,6 +49,10 @@ struct SO2Tangent : SO2TangentBase> using Base = SO2TangentBase>; using Type = SO2Tangent<_Scalar>; +protected: + + using Base::derived; + public: MANIF_TANGENT_TYPEDEF @@ -58,17 +62,13 @@ struct SO2Tangent : SO2TangentBase> SO2Tangent() = default; ~SO2Tangent() = default; - // Copy constructor given base - SO2Tangent(const Base& o); - template - SO2Tangent(const SO2TangentBase<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SO2Tangent) + // Copy constructor given base template SO2Tangent(const TangentBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SO2Tangent(const Eigen::MatrixBase<_EigenDerived>& theta); + MANIF_TANGENT_ASSIGN_OP(SO2Tangent) //! @brief Constructor given an angle (rad.). SO2Tangent(const Scalar theta); @@ -89,26 +89,9 @@ struct SO2Tangent : SO2TangentBase> MANIF_EXTRA_TANGENT_TYPEDEF(SO2Tangent); -template -SO2Tangent<_Scalar>::SO2Tangent(const Base& o) - : data_(o.coeffs()) -{ - // -} - template template -SO2Tangent<_Scalar>::SO2Tangent( - const SO2TangentBase<_DerivedOther>& o) - : data_(o.coeffs()) -{ - // -} - -template -template -SO2Tangent<_Scalar>::SO2Tangent( - const TangentBase<_DerivedOther>& o) +SO2Tangent<_Scalar>::SO2Tangent(const TangentBase<_DerivedOther>& o) : data_(o.coeffs()) { // @@ -121,15 +104,6 @@ SO2Tangent<_Scalar>::SO2Tangent(const Scalar theta) // } -template -template -SO2Tangent<_Scalar>::SO2Tangent( - const Eigen::MatrixBase<_EigenDerived>& theta) - : data_(theta) -{ - // -} - template typename SO2Tangent<_Scalar>::DataType& SO2Tangent<_Scalar>::coeffs() diff --git a/include/manif/impl/so2/SO2Tangent_base.h b/include/manif/impl/so2/SO2Tangent_base.h index da0ae799..2e2ccf2d 100644 --- a/include/manif/impl/so2/SO2Tangent_base.h +++ b/include/manif/impl/so2/SO2Tangent_base.h @@ -29,8 +29,15 @@ struct SO2TangentBase : TangentBase<_Derived> using Base::coeffs; - SO2TangentBase() = default; - ~SO2TangentBase() = default; +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SO2TangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(SO2TangentBase) // Tangent common API diff --git a/include/manif/impl/so2/SO2Tangent_map.h b/include/manif/impl/so2/SO2Tangent_map.h index 00c0d3ec..d3105a01 100644 --- a/include/manif/impl/so2/SO2Tangent_map.h +++ b/include/manif/impl/so2/SO2Tangent_map.h @@ -48,6 +48,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_TANGENT_MAP_ASSIGN_OP(SO2Tangent) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/so2/SO2_base.h b/include/manif/impl/so2/SO2_base.h index b2a93285..8f669101 100644 --- a/include/manif/impl/so2/SO2_base.h +++ b/include/manif/impl/so2/SO2_base.h @@ -35,6 +35,16 @@ struct SO2Base : LieGroupBase<_Derived> // LieGroup common API +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SO2Base) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(SO2Base) + /** * @brief Get the inverse of this. * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. @@ -312,6 +322,21 @@ struct RandomEvaluatorImpl> } }; +//! @brief Assignment assert specialization for SO2Base objects +template +struct AssignmentEvaluatorImpl> +{ + template + static void run_impl(const T& data) + { + using std::abs; + using Scalar = typename SO2Base::Scalar; + MANIF_CHECK(abs(data.norm()-Scalar(1)) < Constants::eps_s, + "SO2 assigned data not normalized !", + invalid_argument); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/so2/SO2_map.h b/include/manif/impl/so2/SO2_map.h index f0ef7447..9ad5fc9c 100644 --- a/include/manif/impl/so2/SO2_map.h +++ b/include/manif/impl/so2/SO2_map.h @@ -51,6 +51,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_GROUP_MAP_ASSIGN_OP(SO2) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/so3/SO3.h b/include/manif/impl/so3/SO3.h index fc13a63b..01410325 100644 --- a/include/manif/impl/so3/SO3.h +++ b/include/manif/impl/so3/SO3.h @@ -57,6 +57,10 @@ struct SO3 : SO3Base> using QuaternionDataType = Eigen::Quaternion<_Scalar>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -71,18 +75,13 @@ struct SO3 : SO3Base> SO3() = default; ~SO3() = default; - // Copy constructor given base - SO3(const Base& o); - - template - SO3(const SO3Base<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SO3) + // Copy constructor given base template SO3(const LieGroupBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SO3(const Eigen::MatrixBase<_EigenDerived>& data); + MANIF_GROUP_ASSIGN_OP(SO3) /** * @brief Constructor given a unit quaternion. @@ -123,44 +122,14 @@ struct SO3 : SO3Base> MANIF_EXTRA_GROUP_TYPEDEF(SO3) -template -template -SO3<_Scalar>::SO3(const Eigen::MatrixBase<_EigenDerived>& data) - : data_(data) -{ - using std::abs; - MANIF_ASSERT(abs(data_.norm()-Scalar(1)) < Constants::eps_s, - "SO3 constructor argument not normalized !", - invalid_argument); -} - -template -SO3<_Scalar>::SO3(const Base& o) - : SO3(o.coeffs()) -{ - // -} - template template -SO3<_Scalar>::SO3( - const SO3Base<_DerivedOther>& o) +SO3<_Scalar>::SO3(const LieGroupBase<_DerivedOther>& o) : SO3(o.coeffs()) { // } -template -template -SO3<_Scalar>::SO3( - const LieGroupBase<_DerivedOther>& o) - : SO3(o.coeffs()) -{ - // -} - - - template SO3<_Scalar>::SO3(const QuaternionDataType& q) : SO3(q.coeffs()) diff --git a/include/manif/impl/so3/SO3Tangent.h b/include/manif/impl/so3/SO3Tangent.h index 5145c1b1..6721d9b7 100644 --- a/include/manif/impl/so3/SO3Tangent.h +++ b/include/manif/impl/so3/SO3Tangent.h @@ -49,6 +49,10 @@ struct SO3Tangent : SO3TangentBase> using Base = SO3TangentBase>; using Type = SO3Tangent<_Scalar>; +protected: + + using Base::derived; + public: MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND @@ -60,17 +64,13 @@ struct SO3Tangent : SO3TangentBase> SO3Tangent() = default; ~SO3Tangent() = default; - // Copy constructor given base - SO3Tangent(const Base& o); - template - SO3Tangent(const SO3TangentBase<_DerivedOther>& o); + MANIF_COPY_CONSTRUCTOR(SO3Tangent) + // Copy constructor given base template SO3Tangent(const TangentBase<_DerivedOther>& o); - // Copy constructor given Eigen - template - SO3Tangent(const Eigen::MatrixBase<_EigenDerived>& v); + MANIF_TANGENT_ASSIGN_OP(SO3Tangent) // Tangent common API @@ -86,40 +86,14 @@ struct SO3Tangent : SO3TangentBase> MANIF_EXTRA_TANGENT_TYPEDEF(SO3Tangent); -template -SO3Tangent<_Scalar>::SO3Tangent(const Base& o) - : data_(o.coeffs()) -{ - // -} - template template -SO3Tangent<_Scalar>::SO3Tangent( - const SO3TangentBase<_DerivedOther>& o) +SO3Tangent<_Scalar>::SO3Tangent(const TangentBase<_DerivedOther>& o) : data_(o.coeffs()) { // } -template -template -SO3Tangent<_Scalar>::SO3Tangent( - const TangentBase<_DerivedOther>& o) - : data_(o.coeffs()) -{ - // -} - -template -template -SO3Tangent<_Scalar>::SO3Tangent( - const Eigen::MatrixBase<_EigenDerived>& v) - : data_(v) -{ - // -} - template typename SO3Tangent<_Scalar>::DataType& SO3Tangent<_Scalar>::coeffs() diff --git a/include/manif/impl/so3/SO3Tangent_base.h b/include/manif/impl/so3/SO3Tangent_base.h index 8272064f..b259a6ea 100644 --- a/include/manif/impl/so3/SO3Tangent_base.h +++ b/include/manif/impl/so3/SO3Tangent_base.h @@ -31,8 +31,15 @@ struct SO3TangentBase : TangentBase<_Derived> using Base::coeffs; - SO3TangentBase() = default; - ~SO3TangentBase() = default; +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SO3TangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(SO3TangentBase) /** * @brief Hat operator of SO3. diff --git a/include/manif/impl/so3/SO3Tangent_map.h b/include/manif/impl/so3/SO3Tangent_map.h index afe6db82..b1666c28 100644 --- a/include/manif/impl/so3/SO3Tangent_map.h +++ b/include/manif/impl/so3/SO3Tangent_map.h @@ -50,6 +50,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_TANGENT_MAP_ASSIGN_OP(SO3Tangent) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/so3/SO3_base.h b/include/manif/impl/so3/SO3_base.h index b5a71882..621fcb0c 100644 --- a/include/manif/impl/so3/SO3_base.h +++ b/include/manif/impl/so3/SO3_base.h @@ -35,6 +35,16 @@ struct SO3Base : LieGroupBase<_Derived> using Transformation = typename internal::traits<_Derived>::Transformation; using QuaternionDataType = Eigen::Quaternion; +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SO3Base) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(SO3Base) + // LieGroup common API /** @@ -381,6 +391,21 @@ struct RandomEvaluatorImpl> } }; +//! @brief Assignment assert specialization for SE2Base objects +template +struct AssignmentEvaluatorImpl> +{ + template + static void run_impl(const T& data) + { + using std::abs; + using Scalar = typename SO3Base::Scalar; + MANIF_CHECK(abs(data.norm()-Scalar(1)) < Constants::eps_s, + "SO3 assigned data not normalized !", + invalid_argument); + } +}; + } /* namespace internal */ } /* namespace manif */ diff --git a/include/manif/impl/so3/SO3_map.h b/include/manif/impl/so3/SO3_map.h index d9f42e2b..13e77d05 100644 --- a/include/manif/impl/so3/SO3_map.h +++ b/include/manif/impl/so3/SO3_map.h @@ -51,6 +51,8 @@ class Map, 0> Map(Scalar* coeffs) : data_(coeffs) { } + MANIF_GROUP_MAP_ASSIGN_OP(SO3) + DataType& coeffs() { return data_; } const DataType& coeffs() const { return data_; } diff --git a/include/manif/impl/tangent_base.h b/include/manif/impl/tangent_base.h index 00fbd18f..a00e88ce 100644 --- a/include/manif/impl/tangent_base.h +++ b/include/manif/impl/tangent_base.h @@ -39,10 +39,35 @@ struct TangentBase template using TangentTemplate = typename internal::traitscast::cast; +protected: + + MANIF_DEFAULT_CONSTRUCTOR(TangentBase) + public: - TangentBase() = default; - ~TangentBase() = default; + /** + * @brief Assignment operator. + * @param[in] t An element of the same Tangent group. + * @return A reference to this. + */ + _Derived& operator =(const TangentBase& t); + + /** + * @brief Assignment operator. + * @param[in] t An element of the same Tangent group. + * @return A reference to this. + */ + template + _Derived& operator =(const TangentBase<_DerivedOther>& t); + + /** + * @brief Assignment operator. + * @param[in] t A DataType object. + * @return A reference to this. + * @see DataType. + */ + template + _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& v); //! @brief Access the underlying data by reference DataType& coeffs(); @@ -258,30 +283,6 @@ struct TangentBase // Copy assignment - /** - * @brief Assignment operator. - * @param[in] t An element of the same Tangent group. - * @return A reference to this. - */ - _Derived& operator =(const TangentBase<_Derived>& t); - - /** - * @brief Assignment operator. - * @param[in] t An element of the same Tangent group. - * @return A reference to this. - */ - template - _Derived& operator =(const TangentBase<_DerivedOther>& t); - - /** - * @brief Assignment operator. - * @param[in] t A DataType object. - * @return A reference to this. - * @see DataType. - */ - template - _Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& v); - template auto operator <<(T&& v) ->decltype( std::declval().operator<<(std::forward(v)) ); @@ -332,12 +333,40 @@ struct TangentBase //! Static helper to get a Basis of the Lie group. static InnerWeight W(); -private: +protected: - _Derived& derived() { return *static_cast< _Derived* >(this); } - const _Derived& derived() const { return *static_cast< const _Derived* >(this); } + inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); } + inline const _Derived& derived() const & noexcept { return *static_cast< const _Derived* >(this); } }; +// Copy + +template +_Derived& +TangentBase<_Derived>::operator =(const TangentBase& t) +{ + coeffs() = t.coeffs(); + return derived(); +} + +template +template +_Derived& +TangentBase<_Derived>::operator =(const TangentBase<_DerivedOther>& t) +{ + coeffs() = t.coeffs(); + return derived(); +} + +template +template +_Derived& +TangentBase<_Derived>::operator =(const Eigen::MatrixBase<_EigenDerived>& v) +{ + coeffs() = v; + return derived(); +} + template typename TangentBase<_Derived>::DataType& TangentBase<_Derived>::coeffs() @@ -605,36 +634,6 @@ bool TangentBase<_Derived>::isApprox( // Operators -// Copy assignment - -template -_Derived& -TangentBase<_Derived>::operator =( - const TangentBase<_Derived>& t) -{ - coeffs() = t.coeffs(); - return derived(); -} - -template -template -_Derived& -TangentBase<_Derived>::operator =( - const TangentBase<_DerivedOther>& t) -{ - coeffs() = t.coeffs(); - return derived(); -} - -template -template -_Derived& TangentBase<_Derived>::operator =( - const Eigen::MatrixBase<_EigenDerived>& v) -{ - coeffs() = v; - return derived(); -} - template template auto TangentBase<_Derived>::operator <<(T&& v) From a33bc53bc11962cfb61bfc0847ac9a5e8fbb0c85 Mon Sep 17 00:00:00 2001 From: artivis Date: Mon, 27 Jul 2020 08:28:12 -0400 Subject: [PATCH 3/6] cleanup eigen includes --- examples/se2_localization.cpp | 2 -- examples/se2_sam.cpp | 3 --- examples/se3_localization.cpp | 2 -- examples/se3_sam.cpp | 3 --- examples/se3_sam_selfcalib.cpp | 3 --- include/manif/impl/eigen.h | 2 ++ include/manif/impl/rn/RnTangent.h | 2 -- include/manif/impl/se2/SE2Tangent.h | 2 -- include/manif/impl/se3/SE3.h | 2 -- include/manif/impl/se3/SE3Tangent.h | 2 -- include/manif/impl/se3/SE3_base.h | 2 -- include/manif/impl/so2/SO2Tangent.h | 2 -- include/manif/impl/so3/SO3Tangent.h | 2 -- test/eigen_gtest.h | 1 - test/se3/gtest_se3.cpp | 4 +--- 15 files changed, 3 insertions(+), 31 deletions(-) diff --git a/examples/se2_localization.cpp b/examples/se2_localization.cpp index 4e6fd61f..96816f0a 100644 --- a/examples/se2_localization.cpp +++ b/examples/se2_localization.cpp @@ -103,8 +103,6 @@ #include "manif/SE2.h" -#include - #include #include diff --git a/examples/se2_sam.cpp b/examples/se2_sam.cpp index 2f54b030..9054c4ce 100644 --- a/examples/se2_sam.cpp +++ b/examples/se2_sam.cpp @@ -180,9 +180,6 @@ // manif #include "manif/SE2.h" -// Eigen -#include - // Std #include #include diff --git a/examples/se3_localization.cpp b/examples/se3_localization.cpp index abf0528b..73a01958 100644 --- a/examples/se3_localization.cpp +++ b/examples/se3_localization.cpp @@ -103,8 +103,6 @@ #include "manif/SE3.h" -#include - #include #include diff --git a/examples/se3_sam.cpp b/examples/se3_sam.cpp index b57241be..5146671d 100644 --- a/examples/se3_sam.cpp +++ b/examples/se3_sam.cpp @@ -180,9 +180,6 @@ // manif #include "manif/SE3.h" -// Eigen -#include - // Std #include #include diff --git a/examples/se3_sam_selfcalib.cpp b/examples/se3_sam_selfcalib.cpp index bf8cc341..f37361ba 100644 --- a/examples/se3_sam_selfcalib.cpp +++ b/examples/se3_sam_selfcalib.cpp @@ -182,9 +182,6 @@ // manif #include "manif/SE3.h" -// Eigen -#include - // Std #include #include diff --git a/include/manif/impl/eigen.h b/include/manif/impl/eigen.h index 39798c79..d0cfe9b9 100644 --- a/include/manif/impl/eigen.h +++ b/include/manif/impl/eigen.h @@ -2,6 +2,8 @@ #define _MANIF_MANIF_EIGEN_H_ #include +#include // for mat.inverse() +#include /** * @note static_cast to avoid -Wno-enum-compare diff --git a/include/manif/impl/rn/RnTangent.h b/include/manif/impl/rn/RnTangent.h index 446cb98e..5e3e1eef 100644 --- a/include/manif/impl/rn/RnTangent.h +++ b/include/manif/impl/rn/RnTangent.h @@ -3,8 +3,6 @@ #include "manif/impl/rn/RnTangent_base.h" -#include - namespace manif { namespace internal { diff --git a/include/manif/impl/se2/SE2Tangent.h b/include/manif/impl/se2/SE2Tangent.h index d2fadf0b..9c0da2c2 100644 --- a/include/manif/impl/se2/SE2Tangent.h +++ b/include/manif/impl/se2/SE2Tangent.h @@ -3,8 +3,6 @@ #include "manif/impl/se2/SE2Tangent_base.h" -#include - namespace manif { namespace internal { diff --git a/include/manif/impl/se3/SE3.h b/include/manif/impl/se3/SE3.h index eba9f4b6..a497bb72 100644 --- a/include/manif/impl/se3/SE3.h +++ b/include/manif/impl/se3/SE3.h @@ -3,8 +3,6 @@ #include "manif/impl/se3/SE3_base.h" -#include - namespace manif { // Forward declare for type traits specialization diff --git a/include/manif/impl/se3/SE3Tangent.h b/include/manif/impl/se3/SE3Tangent.h index d6a2210b..1f7f74f4 100644 --- a/include/manif/impl/se3/SE3Tangent.h +++ b/include/manif/impl/se3/SE3Tangent.h @@ -3,8 +3,6 @@ #include "manif/impl/se3/SE3Tangent_base.h" -#include - namespace manif { namespace internal { diff --git a/include/manif/impl/se3/SE3_base.h b/include/manif/impl/se3/SE3_base.h index 834c043d..b9c8dc40 100644 --- a/include/manif/impl/se3/SE3_base.h +++ b/include/manif/impl/se3/SE3_base.h @@ -5,8 +5,6 @@ #include "manif/impl/lie_group_base.h" #include "manif/impl/so3/SO3_map.h" -#include - namespace manif { // diff --git a/include/manif/impl/so2/SO2Tangent.h b/include/manif/impl/so2/SO2Tangent.h index 5945f620..c53ffa84 100644 --- a/include/manif/impl/so2/SO2Tangent.h +++ b/include/manif/impl/so2/SO2Tangent.h @@ -3,8 +3,6 @@ #include "manif/impl/so2/SO2Tangent_base.h" -#include - namespace manif { namespace internal { diff --git a/include/manif/impl/so3/SO3Tangent.h b/include/manif/impl/so3/SO3Tangent.h index 6721d9b7..dc20d799 100644 --- a/include/manif/impl/so3/SO3Tangent.h +++ b/include/manif/impl/so3/SO3Tangent.h @@ -3,8 +3,6 @@ #include "manif/impl/so3/SO3Tangent_base.h" -#include - namespace manif { namespace internal { diff --git a/test/eigen_gtest.h b/test/eigen_gtest.h index 26889829..b2093219 100644 --- a/test/eigen_gtest.h +++ b/test/eigen_gtest.h @@ -2,7 +2,6 @@ #define _MANIF_TEST_EIGEN_GTEST_H_ #include -#include namespace manif { namespace detail { diff --git a/test/se3/gtest_se3.cpp b/test/se3/gtest_se3.cpp index fdc5dd59..ea84e59e 100644 --- a/test/se3/gtest_se3.cpp +++ b/test/se3/gtest_se3.cpp @@ -3,8 +3,6 @@ #include "manif/SE3.h" #include "../common_tester.h" -#include - using namespace manif; TEST(TEST_SE3, TEST_SE3_CONSTRUCTOR_DATATYPE) @@ -234,7 +232,7 @@ TEST(TEST_SE3, TEST_SE3_INVERSE) se3 = SE3d::Random(); - Eigen::Isometry3d iso(se3.asSO3().quat()); + Eigen::Isometry3d iso(se3.quat()); iso.translation() = se3.translation(); EXPECT_EIGEN_NEAR(iso.matrix(), se3.transform()); From 4b697095e331455b6f0143f12b15c12874b5c795 Mon Sep 17 00:00:00 2001 From: artivis Date: Tue, 16 Jun 2020 19:48:23 -0400 Subject: [PATCH 4/6] add g++9 in CI --- .travis.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/.travis.yml b/.travis.yml index 3cf5100e..82c9d2ac 100644 --- a/.travis.yml +++ b/.travis.yml @@ -188,6 +188,24 @@ matrix: - make - make test + # + # G++ 9 + # + - os: linux + env: + - TEST="G++ 9" + addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - gcc-9 + - g++-9 + script: + - cmake -DCMAKE_CXX_COMPILER="g++-9" -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON .. + - make + - make test + # # Clang 3.8 # From 23072069901af06c2cc5567682d4cf13a070630b Mon Sep 17 00:00:00 2001 From: artivis Date: Sat, 25 Jul 2020 10:06:34 -0400 Subject: [PATCH 5/6] add g++8 in CI --- .travis.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/.travis.yml b/.travis.yml index 82c9d2ac..e23823de 100644 --- a/.travis.yml +++ b/.travis.yml @@ -188,6 +188,24 @@ matrix: - make - make test + # + # G++ 8 + # + - os: linux + env: + - TEST="G++ 8" + addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - gcc-8 + - g++-8 + script: + - cmake -DCMAKE_CXX_COMPILER="g++-8" -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON .. + - make + - make test + # # G++ 9 # From b9f046007476b3ca72679445c942d2aca99f9e9c Mon Sep 17 00:00:00 2001 From: artivis Date: Mon, 17 Aug 2020 22:08:20 -0400 Subject: [PATCH 6/6] add unit dual quaternion --- include/manif/DHu.h | 16 + include/manif/impl/dhu/DHu.h | 227 +++++++++++++ include/manif/impl/dhu/DHuTangent.h | 112 ++++++ include/manif/impl/dhu/DHuTangent_base.h | 197 +++++++++++ include/manif/impl/dhu/DHuTangent_map.h | 89 +++++ include/manif/impl/dhu/DHu_base.h | 411 +++++++++++++++++++++++ include/manif/impl/dhu/DHu_map.h | 91 +++++ include/manif/impl/dhu/DHu_properties.h | 33 ++ test/CMakeLists.txt | 3 + test/dhu/CMakeLists.txt | 13 + test/dhu/gtest_dhu.cpp | 168 +++++++++ 11 files changed, 1360 insertions(+) create mode 100644 include/manif/DHu.h create mode 100644 include/manif/impl/dhu/DHu.h create mode 100644 include/manif/impl/dhu/DHuTangent.h create mode 100644 include/manif/impl/dhu/DHuTangent_base.h create mode 100644 include/manif/impl/dhu/DHuTangent_map.h create mode 100644 include/manif/impl/dhu/DHu_base.h create mode 100644 include/manif/impl/dhu/DHu_map.h create mode 100644 include/manif/impl/dhu/DHu_properties.h create mode 100644 test/dhu/CMakeLists.txt create mode 100644 test/dhu/gtest_dhu.cpp diff --git a/include/manif/DHu.h b/include/manif/DHu.h new file mode 100644 index 00000000..6f8682c7 --- /dev/null +++ b/include/manif/DHu.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_DHU_H_ +#define _MANIF_DHU_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/dhu/DHu_properties.h" +#include "manif/impl/dhu/DHu_base.h" +#include "manif/impl/dhu/DHuTangent_base.h" +#include "manif/impl/dhu/DHu.h" +#include "manif/impl/dhu/DHuTangent.h" +#include "manif/impl/dhu/DHu_map.h" +#include "manif/impl/dhu/DHuTangent_map.h" + +#endif // _MANIF_DHU_H_ diff --git a/include/manif/impl/dhu/DHu.h b/include/manif/impl/dhu/DHu.h new file mode 100644 index 00000000..a5853a4a --- /dev/null +++ b/include/manif/impl/dhu/DHu.h @@ -0,0 +1,227 @@ +#ifndef _MANIF_MANIF_DHU_H_ +#define _MANIF_MANIF_DHU_H_ + +#include "manif/impl/dhu/DHu_base.h" + +namespace manif { + +// Forward declare for type traits specialization + +template struct DHu; +template struct DHuTangent; + +namespace internal { + +//! Traits specialization +template +struct traits> +{ + using Scalar = _Scalar; + + using LieGroup = DHu<_Scalar>; + using Tangent = DHuTangent<_Scalar>; + + using Base = DHuBase>; + + static constexpr int Dim = LieGroupProperties::Dim; + static constexpr int DoF = LieGroupProperties::DoF; + static constexpr int RepSize = 8; + + using DataType = Eigen::Matrix; + + using Jacobian = Eigen::Matrix; + using Transformation = Eigen::Matrix; + using Rotation = Eigen::Matrix; + using Translation = Eigen::Matrix; + using Vector = Eigen::Matrix; +}; + +} // namespace internal +} // namespace manif + +namespace manif { + +// +// LieGroup +// + +/** + * @brief Represent an element of DHu. + */ +template +struct DHu : DHuBase> +{ +private: + + using Base = DHuBase>; + using Type = DHu<_Scalar>; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_COMPLETE_GROUP_TYPEDEF + using Translation = typename Base::Translation; + using Quaternion = Eigen::Quaternion; + using Real = Quaternion; + using Dual = Quaternion; + + MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; + using Base::normalize; + + DHu() = default; + ~DHu() = default; + + MANIF_COPY_CONSTRUCTOR(DHu) + + template + DHu(const LieGroupBase<_DerivedOther>& o); + + MANIF_GROUP_ASSIGN_OP(DHu) + + /** + * @brief Constructor given a translation and a unit quaternion. + * @param[in] t A translation vector. + * @param[in] q A unit quaternion. + * @throws manif::invalid_argument on un-normalized complex number. + */ + DHu(const Real& real, const Dual& dual); + + /** + * @brief Constructor given a translation and a unit quaternion. + * @param[in] t A translation vector. + * @param[in] q A unit quaternion. + * @throws manif::invalid_argument on un-normalized complex number. + */ + // DHu(const Translation& t, + // const Eigen::Quaternion& q); + + /** + * @brief Constructor given a translation and an angle axis. + * @param[in] t A translation vector. + * @param[in] angle_axis An angle-axis. + */ + // DHu(const Translation& t, + // const Eigen::AngleAxis& angle_axis); + + /** + * @brief Constructor given a translation and SO3 element. + * @param[in] t A translation vector. + * @param[in] SO3 An element of SO3. + */ + // DHu(const Translation& t, + // const SO3& SO3); + + /** + * @brief Constructor given translation components and + * roll-pitch-yaw angles. + * @param[in] x The x component of the translation. + * @param[in] y The y component of the translation. + * @param[in] z The z component of the translation. + * @param[in] roll The roll angle. + * @param[in] pitch The pitch angle. + * @param[in] yaw The yaw angle. + */ + // DHu(const Scalar x, const Scalar y, const Scalar z, + // const Scalar roll, const Scalar pitch, const Scalar yaw); + + /** + * @brief Constructor from a 3D Eigen::Isometry + * @param[in] h an isometry object from Eigen + * + * Isometry is a typedef from Eigen::Transform, in which the linear part is assumed a rotation matrix. + * This is used to speed up certain methods of Transform, especially inverse(). + */ + DHu(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h); + + // LieGroup common API + + DataType& coeffs(); + const DataType& coeffs() const; + + // DHu specific API + +protected: + + DataType data_; +}; + +MANIF_EXTRA_GROUP_TYPEDEF(DHu) + +template +template +DHu<_Scalar>::DHu(const LieGroupBase<_DerivedOther>& o) + : DHu(o.coeffs()) +{ + // +} + +template +DHu<_Scalar>::DHu(const Real& real, const Dual& dual) + : DHu((DataType() << real.coeffs(), dual.coeffs() ).finished()) +{ + // +} + +// template +// DHu<_Scalar>::DHu(const Translation& t, const Eigen::Quaternion& q) +// : DHu((DataType() << t, q.coeffs() ).finished()) +// { +// // +// } + +// template +// DHu<_Scalar>::DHu(const Translation& t, const Eigen::AngleAxis& a) +// : DHu(t, Quaternion(a)) +// { +// // +// } + +// template +// DHu<_Scalar>::DHu(const Scalar x, const Scalar y, const Scalar z, +// const Scalar roll, const Scalar pitch, const Scalar yaw) +// : DHu(Translation(x,y,z), Eigen::Quaternion( +// Eigen::AngleAxis(yaw, Eigen::Matrix::UnitZ()) * +// Eigen::AngleAxis(pitch, Eigen::Matrix::UnitY()) * +// Eigen::AngleAxis(roll, Eigen::Matrix::UnitX()) )) +// { +// // +// } + +// template +// DHu<_Scalar>::DHu(const Translation& t, const SO3& so3) +// : DHu(t, so3.quat()) +// { +// // +// } + +// template +// DHu<_Scalar>::DHu(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h) +// : DHu(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation())) +// { +// // +// } + +template +typename DHu<_Scalar>::DataType& +DHu<_Scalar>::coeffs() +{ + return data_; +} + +template +const typename DHu<_Scalar>::DataType& +DHu<_Scalar>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_DHU_H_ diff --git a/include/manif/impl/dhu/DHuTangent.h b/include/manif/impl/dhu/DHuTangent.h new file mode 100644 index 00000000..935e9f49 --- /dev/null +++ b/include/manif/impl/dhu/DHuTangent.h @@ -0,0 +1,112 @@ +#ifndef _MANIF_MANIF_DHUTANGENT_H_ +#define _MANIF_MANIF_DHUTANGENT_H_ + +#include "manif/impl/dhu/DHuTangent_base.h" + +namespace manif { +namespace internal { + +//! Traits specialization +template +struct traits> +{ + using Scalar = _Scalar; + + using LieGroup = DHu<_Scalar>; + using Tangent = DHuTangent<_Scalar>; + + using Base = DHuTangentBase; + + static constexpr int Dim = LieGroupProperties::Dim; + static constexpr int DoF = LieGroupProperties::DoF; + static constexpr int RepSize = DoF; + + using DataType = Eigen::Matrix; + + using Jacobian = Eigen::Matrix; + using LieAlg = Eigen::Matrix; +}; + +} // namespace internal +} // namespace manif + +namespace manif { + +// +// Tangent +// + +/** + * @brief Represents an element of tangent space of DHu. + */ +template +struct DHuTangent : DHuTangentBase> +{ +private: + + using Base = DHuTangentBase>; + using Type = DHuTangent<_Scalar>; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + DHuTangent() = default; + ~DHuTangent() = default; + + MANIF_COPY_CONSTRUCTOR(DHuTangent) + + // Copy constructor given base + template + DHuTangent(const TangentBase<_DerivedOther>& o); + + MANIF_TANGENT_ASSIGN_OP(DHuTangent) + + // Tangent common API + + DataType& coeffs(); + const DataType& coeffs() const; + + // DHuTangent specific API + +protected: + + DataType data_; +}; + +MANIF_EXTRA_TANGENT_TYPEDEF(DHuTangent); + +template +template +DHuTangent<_Scalar>::DHuTangent( + const TangentBase<_DerivedOther>& o) + : data_(o.coeffs()) +{ + // +} + +template +typename DHuTangent<_Scalar>::DataType& +DHuTangent<_Scalar>::coeffs() +{ + return data_; +} + +template +const typename DHuTangent<_Scalar>::DataType& +DHuTangent<_Scalar>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif //_MANIF_MANIF_DHUTANGENT_H_ diff --git a/include/manif/impl/dhu/DHuTangent_base.h b/include/manif/impl/dhu/DHuTangent_base.h new file mode 100644 index 00000000..f8be251c --- /dev/null +++ b/include/manif/impl/dhu/DHuTangent_base.h @@ -0,0 +1,197 @@ +#ifndef _MANIF_MANIF_DHUTANGENT_BASE_H_ +#define _MANIF_MANIF_DHUTANGENT_BASE_H_ + +#include "manif/impl/dhu/DHu_properties.h" +#include "manif/impl/tangent_base.h" + +namespace manif { + +// +// Tangent +// + +/** + * @brief The base class of the DHu tangent. + */ +template +struct DHuTangentBase : TangentBase<_Derived> +{ +private: + + using Base = TangentBase<_Derived>; + using Type = DHuTangentBase<_Derived>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_OPERATOR + + using Base::data; + using Base::coeffs; + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(DHuTangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(DHuTangentBase) + + // Tangent common API + + /** + * @brief Hat operator of DHu. + * @return An element of the Lie algebra dhu. + */ + LieAlg hat() const; + + /** + * @brief Get the DHu element. + * @param[out] -optional- J_m_t Jacobian of the DHu element wrt this. + * @return The DHu element. + * @note This is the exp() map with the argument in vector form. + */ + 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 of DHu. + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian of DHu. + */ + Jacobian ljac() const; + + /** + * @brief Get the inverse right Jacobian of DHu. + */ + Jacobian rjacinv() const; + + /** + * @brief Get the inverse left Jacobian of DHu. + */ + Jacobian ljacinv() const; + + /** + * @brief + * @return + */ + Jacobian smallAdj() const; + + // DHuTangent specific API +}; + +template +typename DHuTangentBase<_Derived>::LieGroup +DHuTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const +{ + if (J_m_t) + { + *J_m_t = rjac(); + } + + // @todo + return LieGroup(); +} + +template +typename DHuTangentBase<_Derived>::LieGroup +DHuTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const +{ + return exp(J_m_t); +} + +template +typename DHuTangentBase<_Derived>::LieAlg +DHuTangentBase<_Derived>::hat() const +{ + // @todo + return LieAlg(); +} + +template +typename DHuTangentBase<_Derived>::Jacobian +DHuTangentBase<_Derived>::rjac() const +{ + // @todo + return Jacobian(); +} + +template +typename DHuTangentBase<_Derived>::Jacobian +DHuTangentBase<_Derived>::ljac() const +{ + // @todo + return Jacobian(); +} + +template +typename DHuTangentBase<_Derived>::Jacobian +DHuTangentBase<_Derived>::rjacinv() const +{ + // @todo + return Jacobian(); +} + +template +typename DHuTangentBase<_Derived>::Jacobian +DHuTangentBase<_Derived>::ljacinv() const +{ + // @todo + return Jacobian(); +} + +template +typename DHuTangentBase<_Derived>::Jacobian +DHuTangentBase<_Derived>::smallAdj() const +{ + // @todo + return Jacobian(); +} + +// DHuTangent specific API + +namespace internal { + +//! @brief Generator specialization for DHuTangentBase objects. +template +struct GeneratorEvaluator> +{ + static typename DHuTangentBase::LieAlg + run(const unsigned int i) + { + using LieAlg = typename DHuTangentBase::LieAlg; + + // @todo + + return LieAlg{}; + } +}; + +//! @brief Random specialization for DHuTangentBase objects. +template +struct RandomEvaluatorImpl> +{ + static void run(DHuTangentBase& m) + { + // @todo check + m.coeffs().setRandom(); // in [-1,1] + m.coeffs() *= MANIF_PI; // in [-PI,PI] + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_DHUTANGENT_BASE_H_ diff --git a/include/manif/impl/dhu/DHuTangent_map.h b/include/manif/impl/dhu/DHuTangent_map.h new file mode 100644 index 00000000..879b54a6 --- /dev/null +++ b/include/manif/impl/dhu/DHuTangent_map.h @@ -0,0 +1,89 @@ +#ifndef _MANIF_MANIF_DHUTANGENT_MAP_H_ +#define _MANIF_MANIF_DHUTANGENT_MAP_H_ + +#include "manif/impl/dhu/DHuTangent.h" + +namespace manif { +namespace internal { + +//! @brief traits specialization for Eigen Map +template +struct traits< Eigen::Map,0> > + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using DataType = Eigen::Map, 0>; + using Base = DHuTangentBase, 0>>; +}; + +//! @brief traits specialization for Eigen Map +template +struct traits< Eigen::Map,0> > + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using DataType = Eigen::Map, 0>; + using Base = DHuTangentBase, 0>>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::DHu + */ +template +class Map, 0> + : public manif::DHuTangentBase, 0> > +{ + using Base = manif::DHuTangentBase, 0> >; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + Map(Scalar* coeffs) : data_(coeffs) { } + + MANIF_TANGENT_MAP_ASSIGN_OP(DHuTangent) + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::DHu + */ +template +class Map, 0> + : public manif::DHuTangentBase, 0> > +{ + using Base = manif::DHuTangentBase, 0> >; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + Map(const Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_DHUTANGENT_MAP_H_ diff --git a/include/manif/impl/dhu/DHu_base.h b/include/manif/impl/dhu/DHu_base.h new file mode 100644 index 00000000..12b3d85b --- /dev/null +++ b/include/manif/impl/dhu/DHu_base.h @@ -0,0 +1,411 @@ +#ifndef _MANIF_MANIF_DHU_BASE_H_ +#define _MANIF_MANIF_DHU_BASE_H_ + +#include "manif/impl/dhu/DHu_properties.h" +#include "manif/impl/lie_group_base.h" + +namespace manif { + +// +// LieGroup +// + +/** + * @brief The base class of the DHu group (Unit Dual Quaternion). + * @note s = [r,d] = [r0,r1,r2,r3, d0,d1,d2,d3] + * with r (real) is a quaternion q = [x,y,z,w] and + * with d (dual) is a quaternion q = [x,y,z,w]. + * A dual quaternion is unit if s . s* = 1, that is if + * |r| = 1 + * and + * r dot d = 0 + */ +template +struct DHuBase : LieGroupBase<_Derived> +{ +private: + + using Base = LieGroupBase<_Derived>; + using Type = DHuBase<_Derived>; + +public: + + MANIF_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_AUTO_API + MANIF_INHERIT_GROUP_OPERATOR + + using Base::coeffs; + + using Rotation = typename internal::traits<_Derived>::Rotation; + using Translation = typename internal::traits<_Derived>::Translation; + using Transformation = typename internal::traits<_Derived>::Transformation; + using Isometry = Eigen::Transform; + using Quaternion = Eigen::Quaternion; + using Real = Quaternion; + using Dual = Quaternion; + + // LieGroup common API + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(DHuBase) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(DHuBase) + + /** + * @brief Get the inverse. + * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. + */ + LieGroup inverse(OptJacobianRef J_minv_m = {}) const; + + /** + * @brief Get the DHu corresponding Lie algebra element in vector form. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The DHu tangent of this. + * @note This is the log() map in vector form. + * @see DHuTangent. + */ + Tangent log(OptJacobianRef J_t_m = {}) const; + + /** + * @brief This function is deprecated. + * Please considere using + * @ref log instead. + */ + MANIF_DEPRECATED + Tangent lift(OptJacobianRef J_t_m = {}) const; + + /** + * @brief Composition of this and another DHu element. + * @param[in] m Another DHu 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 Rigid motion action on a 3D point. + * @param v A 3D point. + * @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 transformed 3D point. + */ + template + Eigen::Matrix + act(const Eigen::MatrixBase<_EigenDerived> &v, + tl::optional>> J_vout_m = {}, + tl::optional>> J_vout_v = {}) const; + + /** + * @brief Get the adjoint matrix of DHu at this. + */ + Jacobian adj() const; + + // DHu specific functions + + LieGroup conjugate() const; + LieGroup conjugatedual() const; + + /** + * Get the transformation matrix (3D isometry). + * @note T = | R t | + * | 0 1 | + */ + Transformation transform() const; + + /** + * Get the isometry object (Eigen 3D isometry). + * @note T = | R t | + * | 0 1 | + */ + Isometry isometry() const; + + /** + * @brief Get the rotational part of this as a rotation matrix. + */ + Rotation rotation() const; + + /** + * @brief Get the real part of this as a quaternion. + */ + Real real() const; + + /** + * @brief Get the dual part of this as a quaternion. + */ + Dual dual() const; + + /** + * @brief Get the translational part in vector form. + */ + Translation translation() const; + + /** + * @brief Normalize the underlying dual quaternion. + */ + void normalize(); +}; + +template +typename DHuBase<_Derived>::LieGroup +DHuBase<_Derived>::inverse(OptJacobianRef J_minv_m) const +{ + if (J_minv_m) + { + (*J_minv_m) = -adj(); + } + + return conjugate(); +} + +template +typename DHuBase<_Derived>::Tangent +DHuBase<_Derived>::log(OptJacobianRef J_t_m) const +{ + // @todo + Tangent tan; + + if (J_t_m) + { + // Jr^-1 + (*J_t_m) = tan.rjacinv(); + } + + return tan; +} + +template +typename DHuBase<_Derived>::Tangent +DHuBase<_Derived>::lift(OptJacobianRef J_t_m) const +{ + return log(J_t_m); +} + +template +template +typename DHuBase<_Derived>::LieGroup +DHuBase<_Derived>::compose( + const LieGroupBase<_DerivedOther>& m, + OptJacobianRef J_mc_ma, + OptJacobianRef J_mc_mb) const +{ + static_assert( + std::is_base_of, _DerivedOther>::value, + "Argument does not inherit from DHuBase !"); + + const auto& m_DHu = static_cast&>(m); + + if (J_mc_ma) + { + (*J_mc_ma) = m_DHu.inverse().adj(); + } + + if (J_mc_mb) + { + J_mc_mb->setIdentity(); + } + + return LieGroup(real()*m_DHu.real(), + Quaternion((real()*m_DHu.dual()).coeffs()+(dual()*m_DHu.real()).coeffs())); +} + +template +template +Eigen::Matrix::Scalar, 3, 1> +DHuBase<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v) const +{ + assert_vector_dim(v, 3); + const Rotation R(rotation()); + + if (J_vout_m) + { + // @todo + } + + if (J_vout_v) + { + // @todo + } + + // @todo do it in a dual quat fashion + return translation() + R * v; +} + + +template +typename DHuBase<_Derived>::Jacobian +DHuBase<_Derived>::adj() const +{ + // @todo + + return Jacobian(); +} + +// DHu specific function + +template +typename DHuBase<_Derived>::Transformation +DHuBase<_Derived>::transform() const +{ + Transformation T = Transformation::Identity(); + T.template topLeftCorner<3,3>() = rotation(); + T.template topRightCorner<3,1>() = translation(); + return T; +} + +template +typename DHuBase<_Derived>::Isometry +DHuBase<_Derived>::isometry() const +{ + return Isometry(transform()); +} + +template +typename DHuBase<_Derived>::Rotation +DHuBase<_Derived>::rotation() const +{ + return real().matrix(); +} + +template +typename DHuBase<_Derived>::Real +DHuBase<_Derived>::real() const +{ + return Real(coeffs().template head<4>()); +} + +template +typename DHuBase<_Derived>::Dual +DHuBase<_Derived>::dual() const +{ + return Real(coeffs().template tail<4>()); +} + +template +typename DHuBase<_Derived>::Translation +DHuBase<_Derived>::translation() const +{ + return dual()*real().conjugate()*Scalar(2); +} + +template +typename DHuBase<_Derived>::LieGroup +DHuBase<_Derived>::conjugate() const +{ + return LieGroup(real().conjugate(), dual().conjugate()); +} + +template +typename DHuBase<_Derived>::LieGroup +DHuBase<_Derived>::conjugatedual() const +{ + return LieGroup((DataType() << coeffs().template head<4>(), + -coeffs().template tail<4>()).finished()); +} + +template +void DHuBase<_Derived>::normalize() +{ + const Scalar n = coeffs().template head<4>().norm(); + coeffs() /= n; + // real and dual parts are orthogonal + coeffs().template tail<4>() -= (coeffs().template head<4>().dot(coeffs().template tail<4>())) * coeffs().template head<4>(); +} + +namespace internal { + +//! @brief Random specialization for DHuBase objects. +template +struct RandomEvaluatorImpl> +{ + template + static void run(T& m) + { + // @note: + // Quaternion::UnitRandom is not available in Eigen 3.3-beta1 + // which is the default version in Ubuntu 16.04 + // So we copy its implementation here. + + using std::sqrt; + using std::sin; + using std::cos; + + using Scalar = typename DHuBase::Scalar; + using LieGroup = typename DHuBase::LieGroup; + using DataType = typename DHuBase::DataType; + + Scalar u1 = Eigen::internal::random(0, 1), + u2 = Eigen::internal::random(0, 2*EIGEN_PI), + u3 = Eigen::internal::random(0, 2*EIGEN_PI); + Scalar a = sqrt(Scalar(1) - u1), + b = sqrt(u1); + + Eigen::Matrix real(a * cos(u2),b * sin(u3),b * cos(u3),a * sin(u2)); + + u1 = Eigen::internal::random(0, 1); + u2 = Eigen::internal::random(0, 2*EIGEN_PI); + u3 = Eigen::internal::random(0, 2*EIGEN_PI); + a = sqrt(Scalar(1) - u1); + b = sqrt(u1); + + Eigen::Matrix dual(a * cos(u2),b * sin(u3),b * cos(u3),a * sin(u2)); + + dual -= real.dot(dual) * real; + + m = LieGroup((DataType() << real, dual).finished()); + + // @note see https://scicomp.stackexchange.com/a/27843 + + // Scalar x = a * cos(u2), + // y = b * sin(u3), + // z = b * cos(u3), + // w = a * sin(u2); + + // Eigen::Matrix M; + // M << -x, -y, -z, + // w, z, -y, + // -z, w, x, + // y, -x, w; + + // const Eigen::Matrix v = M * Eigen::Matrix::Random().normalized(); + + // m = LieGroup((DataType() << x, y, z, w, v(1), v(2), v(3), v(0)).finished()); + } +}; + +//! @brief Assignment assert specialization for SE2Base objects +template +struct AssignmentEvaluatorImpl> +{ + template + static void run_impl(const T& data) + { + using std::abs; + using Scalar = typename DHuBase::Scalar; + // |r| = 1 + MANIF_CHECK(abs(data.template head<4>().norm()-Scalar(1)) < + Constants::eps_s, + "DHu real part not normalized !", + invalid_argument); + // r dot d = 0 + MANIF_CHECK(abs(data.template head<4>().dot(data.template tail<4>())) < + Constants::eps_s, + "DHu dual part not normalized !", + invalid_argument); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_DHU_BASE_H_ diff --git a/include/manif/impl/dhu/DHu_map.h b/include/manif/impl/dhu/DHu_map.h new file mode 100644 index 00000000..d54ed3b6 --- /dev/null +++ b/include/manif/impl/dhu/DHu_map.h @@ -0,0 +1,91 @@ +#ifndef _MANIF_MANIF_DHU_MAP_H_ +#define _MANIF_MANIF_DHU_MAP_H_ + +#include "manif/impl/dhu/DHu.h" + +namespace manif { +namespace internal { + +//! @brief traits specialization for Eigen Map +template +struct traits< Eigen::Map,0> > + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = DHuBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +//! @brief traits specialization for Eigen Map const +template +struct traits< Eigen::Map,0> > + : public traits> +{ + using typename traits>::Scalar; + using traits>::RepSize; + using Base = DHuBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::DHu + */ +template +class Map, 0> + : public manif::DHuBase, 0> > +{ + using Base = manif::DHuBase, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; + + Map(Scalar* coeffs) : data_(coeffs) { } + + MANIF_GROUP_MAP_ASSIGN_OP(DHu) + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::DHu + */ +template +class Map, 0> + : public manif::DHuBase, 0> > +{ + using Base = manif::DHuBase, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; + + Map(const Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_DHU_MAP_H_ diff --git a/include/manif/impl/dhu/DHu_properties.h b/include/manif/impl/dhu/DHu_properties.h new file mode 100644 index 00000000..9b8b2d10 --- /dev/null +++ b/include/manif/impl/dhu/DHu_properties.h @@ -0,0 +1,33 @@ +#ifndef _MANIF_MANIF_DHU_PROPERTIES_H_ +#define _MANIF_MANIF_DHU_PROPERTIES_H_ + +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declaration +template struct DHuBase; +template struct DHuTangentBase; + +namespace internal { + +//! traits specialization +template +struct LieGroupProperties> +{ + static constexpr int Dim = 3; /// @brief Space dimension + static constexpr int DoF = 6; /// @brief Degrees of freedom +}; + +//! traits specialization +template +struct LieGroupProperties> +{ + static constexpr int Dim = 3; /// @brief Space dimension + static constexpr int DoF = 6; /// @brief Degrees of freedom +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_DHU_PROPERTIES_H_ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 47ebf570..6df4437b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -64,6 +64,9 @@ add_subdirectory(se2) # SE3 tests add_subdirectory(se3) +# DHu tests +add_subdirectory(dhu) + find_package(Ceres QUIET) if(CERES_FOUND) diff --git a/test/dhu/CMakeLists.txt b/test/dhu/CMakeLists.txt new file mode 100644 index 00000000..62056bda --- /dev/null +++ b/test/dhu/CMakeLists.txt @@ -0,0 +1,13 @@ +# DHu tests + +manif_add_gtest(gtest_dhu gtest_dhu.cpp) + +set(CXX_11_TEST_TARGETS + + ${CXX_11_TEST_TARGETS} + + # DHu + gtest_dhu + + PARENT_SCOPE +) diff --git a/test/dhu/gtest_dhu.cpp b/test/dhu/gtest_dhu.cpp new file mode 100644 index 00000000..9685fd9a --- /dev/null +++ b/test/dhu/gtest_dhu.cpp @@ -0,0 +1,168 @@ +#include + +#include "manif/DHu.h" + +#include "../common_tester.h" + +using namespace manif; + +TEST(TEST_DHU, TEST_DHU_CONSTRUCTOR_DATATYPE) +{ + DHud dhu((DHud::DataType() << 0,0,0,1,0,0,0,0).finished()); + + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(2)); + EXPECT_DOUBLE_EQ(1, dhu.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(5)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(7)); +} + +TEST(TEST_DHU, TEST_DHU_CONSTRUCTOR_QUAT) +{ + Eigen::Quaterniond real(1,0,0,0), dual(0,0,0,0); + DHud dhu(real, dual); + + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(2)); + EXPECT_DOUBLE_EQ(1, dhu.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(5)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, dhu.coeffs()(7)); + + EXPECT_EIGEN_NEAR(dhu.real().coeffs(), real.coeffs()); + EXPECT_EIGEN_NEAR(dhu.dual().coeffs(), dual.coeffs()); +} + +// TEST(TEST_DHU, TEST_DHU_IDENTITY) +// { +// DHud dhu; + +// dhu.setIdentity(); + +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(0)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(1)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(2)); +// EXPECT_DOUBLE_EQ(1, dhu.coeffs()(3)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(4)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(5)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(6)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(7)); +// } + +// TEST(TEST_DHU, TEST_DHU_IDENTITY2) +// { +// const DHud dhu = DHud::Identity(); + +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(0)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(1)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(2)); +// EXPECT_DOUBLE_EQ(1, dhu.coeffs()(3)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(4)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(5)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(6)); +// EXPECT_DOUBLE_EQ(0, dhu.coeffs()(7)); +// } + +TEST(TEST_DHU, TEST_DHU_INVERSE) +{ + // inverse of identity is identity + // DHud dhu = DHud::Identity(); + DHud dhu((DHud::DataType() << 0,0,0,1,0,0,0,0).finished()); + + auto dhu_inv = dhu.inverse(); + + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(2)); + EXPECT_DOUBLE_EQ(1, dhu_inv.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(5)); + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, dhu_inv.coeffs()(7)); + + // inverse of random in quaternion form is conjugate + EXPECT_NO_THROW( + dhu = DHud::Random(); + ); + + EXPECT_NO_THROW( + dhu_inv = dhu.inverse(); + ); + + DHud I((DHud::DataType() << 0,0,0,1,0,0,0,0).finished()); + + EXPECT_MANIF_NEAR(I, dhu*dhu_inv); + EXPECT_MANIF_NEAR(I, dhu_inv*dhu); +} + +#ifndef MANIF_NO_DEBUG + +TEST(TEST_DHU, TEST_DHU_CONSTRUCTOR_NOT_NORMALIZED_ARGS) +{ + EXPECT_THROW( + DHud dhu((DHud::DataType() << 1,1,1,1,0,0,0,0).finished()), + manif::invalid_argument + ); + + EXPECT_THROW( + DHud dhu((DHud::DataType() << 0,0,0,1,1,1,1,1).finished()), + manif::invalid_argument + ); + + try { + DHud dhu((DHud::DataType() << 1,1,1,1,0,0,0,0).finished()); + } catch (manif::invalid_argument& e) { + EXPECT_FALSE(std::string(e.what()).empty()); + } +} + +TEST(TEST_DHU, TEST_DHU_CONSTRUCTOR_UNNORMALIZED) +{ + using DataType = typename DHud::DataType; + EXPECT_THROW( + DHud(DataType::Random()*10.), manif::invalid_argument + ); +} + +TEST(TEST_DHU, TEST_DHU_NORMALIZE) +{ + using DataType = DHud::DataType; + DataType data = DataType::Random() * 100.; + + EXPECT_THROW( + DHud a(data), manif::invalid_argument + ); + + Eigen::Map map(data.data()); + map.normalize(); + + EXPECT_NO_THROW( + DHud b = map + ); + + try { + DHud b = map; + } catch (manif::invalid_argument& e) { + EXPECT_FALSE(true) << e.what(); + } +} + +#endif + +// MANIF_TEST(DHud); + +// MANIF_TEST_MAP(DHud); + +// MANIF_TEST_JACOBIANS(DHud); + +int main(int argc, char** argv) +{ + std::srand((unsigned int) time(0)); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}