diff --git a/.travis.yml b/.travis.yml index 0053d58e..fbc88abe 100644 --- a/.travis.yml +++ b/.travis.yml @@ -126,7 +126,7 @@ matrix: - g++-6 script: - cmake -DENABLE_COVERAGE=ON -DCMAKE_CXX_COMPILER="g++-6" -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON .. - - make + - travis_wait make - make test - cd .. - bash <(curl -s https://codecov.io/bash) diff --git a/CMakeLists.txt b/CMakeLists.txt index c156142f..d31ada97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,8 @@ elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") -Wno-unused-parameter -Wno-missing-field-initializers ) +elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") + target_compile_options(${PROJECT_NAME} INTERFACE /bigobj) endif() ############# @@ -176,6 +178,7 @@ endif(BUILD_EXAMPLES) # ------------------------------------------------------------------------------ if(ENABLE_COVERAGE) + add_definitions(-DMANIF_COVERAGE_ENABLED) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g ") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fprofile-arcs") diff --git a/README.md b/README.md index 8b82c8ea..422ede8c 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,7 @@ targeted at robotics applications. At the moment, it provides the groups: + - R(n): Euclidean space with addition. - SO(2): rotations in the plane. - SE(2): rigid motion (rotation and translation) in the plane. - SO(3): rotations in 3D space. diff --git a/include/manif/Rn.h b/include/manif/Rn.h new file mode 100644 index 00000000..1affac10 --- /dev/null +++ b/include/manif/Rn.h @@ -0,0 +1,17 @@ +#ifndef _MANIF_RN_H_ +#define _MANIF_RN_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/rn/Rn_properties.h" +#include "manif/impl/rn/Rn_base.h" +#include "manif/impl/rn/RnTangent_base.h" +#include "manif/impl/rn/Rn.h" +#include "manif/impl/rn/RnTangent.h" +#include "manif/impl/rn/Rn_map.h" +#include "manif/impl/rn/RnTangent_map.h" + +#endif // _MANIF_RN_H_ diff --git a/include/manif/impl/generator.h b/include/manif/impl/generator.h index 8d87c5a7..0d11bc2f 100644 --- a/include/manif/impl/generator.h +++ b/include/manif/impl/generator.h @@ -8,7 +8,7 @@ template struct GeneratorEvaluator { static typename Derived::LieAlg - run(const int) + run(const unsigned int) { /// @todo print actual Derived type static_assert(constexpr_false(), diff --git a/include/manif/impl/macro.h b/include/manif/impl/macro.h index 0891434d..ddc79fec 100644 --- a/include/manif/impl/macro.h +++ b/include/manif/impl/macro.h @@ -88,11 +88,14 @@ raise(Args&&... args) #define MANIF_DEPRECATED #endif -// LieGroup - related macros +// Common macros + +#define MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF((Eigen::internal::traits::Alignment>0)) +#define MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(X) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF((Eigen::internal::traits::Alignment>0)) -#define MANIF_GROUP_PROPERTIES \ - static constexpr int Dim = internal::LieGroupProperties::Dim; \ - static constexpr int DoF = internal::LieGroupProperties::DoF; +// LieGroup - related macros #define MANIF_INHERIT_GROUP_AUTO_API \ using Base::setRandom; \ @@ -104,8 +107,6 @@ raise(Args&&... args) #define MANIF_INHERIT_GROUP_API \ MANIF_INHERIT_GROUP_AUTO_API \ - using Base::transform; \ - using Base::rotation; \ using Base::setIdentity; \ using Base::inverse; \ using Base::lift; \ @@ -120,7 +121,12 @@ raise(Args&&... args) using Base::operator *=; \ using Base::operator =; +#define MANIF_GROUP_PROPERTIES \ + using Base::Dim; \ + using Base::DoF; + #define MANIF_GROUP_TYPEDEF \ + MANIF_GROUP_PROPERTIES \ using Scalar = typename Base::Scalar; \ using LieGroup = typename Base::LieGroup; \ using Tangent = typename Base::Tangent; \ @@ -139,10 +145,6 @@ raise(Args&&... args) // Tangent - related macros -#define MANIF_TANGENT_PROPERTIES \ - static constexpr int Dim = internal::LieGroupProperties::Dim; \ - static constexpr int DoF = internal::LieGroupProperties::DoF; - #define MANIF_INHERIT_TANGENT_API \ using Base::setZero; \ using Base::setRandom; \ @@ -161,7 +163,12 @@ raise(Args&&... args) using Base::operator =; \ using Base::operator <<; +#define MANIF_TANGENT_PROPERTIES \ +using Base::Dim; \ +using Base::DoF; + #define MANIF_TANGENT_TYPEDEF \ + MANIF_TANGENT_PROPERTIES \ using Scalar = typename Base::Scalar; \ using LieGroup = typename Base::LieGroup; \ using Tangent = typename Base::Tangent; \ diff --git a/include/manif/impl/rn/Rn.h b/include/manif/impl/rn/Rn.h new file mode 100644 index 00000000..a2a09c2c --- /dev/null +++ b/include/manif/impl/rn/Rn.h @@ -0,0 +1,164 @@ +#ifndef _MANIF_MANIF_RN_H_ +#define _MANIF_MANIF_RN_H_ + +#include "manif/impl/rn/Rn_base.h" + +namespace manif { + +// Forward declare for type traits specialization + +template struct Rn; +template struct RnTangent; + +namespace internal { + +//! Traits specialization +template +struct traits> +{ + using Scalar = _Scalar; + + using LieGroup = Rn<_Scalar, _N>; + using Tangent = RnTangent<_Scalar, _N>; + + using Base = RnBase>; + + static constexpr int Dim = _N; + static constexpr int DoF = _N; + static constexpr int RepSize = _N; + + using DataType = Eigen::Matrix; + + using Jacobian = Eigen::Matrix; + using Transformation = Eigen::Matrix; + using Vector = Eigen::Matrix; +}; + +} // namespace internal +} // namespace manif + +namespace manif { + +// +// LieGroup +// + +/** + * @brief Represents an element of Rn. + */ +template +struct Rn : RnBase> +{ +private: + + static_assert(_N > 0, "N must be greater than 0 !"); + + using Base = RnBase>; + using Type = Rn<_Scalar, _N>; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + Rn() = default; + ~Rn() = default; + + // Copy constructor given base + Rn(const Base& o); + + template + Rn(const RnBase<_DerivedOther>& o); + + template + Rn(const LieGroupBase<_DerivedOther>& o); + + // Copy constructor given Eigen + template + Rn(const Eigen::MatrixBase<_EigenDerived>& data); + + // LieGroup common API + + //! Get a const reference to the underlying DataType. + const DataType& coeffs() const; + + // Rn specific API + +protected: + + friend struct LieGroupBase>; + DataType& coeffs_nonconst(); + + DataType data_; +}; + +template using R1 = Rn<_Scalar, 1>; +template using R2 = Rn<_Scalar, 2>; +template using R3 = Rn<_Scalar, 3>; +template using R4 = Rn<_Scalar, 4>; +template using R5 = Rn<_Scalar, 5>; +template using R6 = Rn<_Scalar, 6>; +template using R7 = Rn<_Scalar, 7>; +template using R8 = Rn<_Scalar, 8>; +template using R9 = Rn<_Scalar, 9>; + +MANIF_EXTRA_GROUP_TYPEDEF(R1) +MANIF_EXTRA_GROUP_TYPEDEF(R2) +MANIF_EXTRA_GROUP_TYPEDEF(R3) +MANIF_EXTRA_GROUP_TYPEDEF(R4) +MANIF_EXTRA_GROUP_TYPEDEF(R5) +MANIF_EXTRA_GROUP_TYPEDEF(R6) +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) + : Rn(o.coeffs()) +{ + // +} + +template +typename Rn<_Scalar, _N>::DataType& +Rn<_Scalar, _N>::coeffs_nonconst() +{ + return data_; +} + +template +const typename Rn<_Scalar, _N>::DataType& +Rn<_Scalar, _N>::coeffs() const +{ + return data_; +} + +} /* namespace manif */ + +#endif /* _MANIF_MANIF_RN_H_ */ diff --git a/include/manif/impl/rn/RnTangent.h b/include/manif/impl/rn/RnTangent.h new file mode 100644 index 00000000..88f50b47 --- /dev/null +++ b/include/manif/impl/rn/RnTangent.h @@ -0,0 +1,157 @@ +#ifndef _MANIF_MANIF_RNTANGENT_H_ +#define _MANIF_MANIF_RNTANGENT_H_ + +#include "manif/impl/rn/RnTangent_base.h" + +#include + +namespace manif { +namespace internal { + +//! Traits specialization +template +struct traits> +{ + using Scalar = _Scalar; + + using LieGroup = Rn<_Scalar, _N>; + using Tangent = RnTangent<_Scalar, _N>; + + using Base = RnTangentBase; + + static constexpr int Dim = _N; + static constexpr int DoF = _N; + static constexpr int RepSize = _N; + + 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 Rn. + */ +template +struct RnTangent : RnTangentBase> +{ +private: + + static_assert(_N > 0, "N must be greater than 0 !"); + + using Base = RnTangentBase>; + using Type = RnTangent<_Scalar, _N>; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + RnTangent() = default; + ~RnTangent() = default; + + // Copy constructor given base + RnTangent(const Base& o); + template + RnTangent(const RnTangentBase<_DerivedOther>& o); + + template + RnTangent(const TangentBase<_DerivedOther>& o); + + // Copy constructor given Eigen + template + RnTangent(const Eigen::MatrixBase<_EigenDerived>& theta); + + // Tangent common API + + DataType& coeffs(); + const DataType& coeffs() const; + +protected: + + DataType data_; +}; + +template using R1Tangent = RnTangent<_Scalar, 1>; +template using R2Tangent = RnTangent<_Scalar, 2>; +template using R3Tangent = RnTangent<_Scalar, 3>; +template using R4Tangent = RnTangent<_Scalar, 4>; +template using R5Tangent = RnTangent<_Scalar, 5>; +template using R6Tangent = RnTangent<_Scalar, 6>; +template using R7Tangent = RnTangent<_Scalar, 7>; +template using R8Tangent = RnTangent<_Scalar, 8>; +template using R9Tangent = RnTangent<_Scalar, 9>; + +MANIF_EXTRA_GROUP_TYPEDEF(R1Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R2Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R3Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R4Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R5Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R6Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R7Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R8Tangent) +MANIF_EXTRA_GROUP_TYPEDEF(R9Tangent) + +template +RnTangent<_Scalar, _N>::RnTangent(const Base& o) + : data_(o.coeffs()) +{ + // +} + +template +template +RnTangent<_Scalar, _N>::RnTangent( + const RnTangentBase<_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() +{ + return data_; +} + +template +const typename RnTangent<_Scalar, _N>::DataType& +RnTangent<_Scalar, _N>::coeffs() const +{ + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_RNTANGENT_H_ diff --git a/include/manif/impl/rn/RnTangent_base.h b/include/manif/impl/rn/RnTangent_base.h new file mode 100644 index 00000000..a3ef66b5 --- /dev/null +++ b/include/manif/impl/rn/RnTangent_base.h @@ -0,0 +1,201 @@ +#ifndef _MANIF_MANIF_RNTANGENT_BASE_H_ +#define _MANIF_MANIF_RNTANGENT_BASE_H_ + +#include "manif/impl/rn/Rn_properties.h" +#include "manif/impl/tangent_base.h" + +namespace manif { + +// +// Tangent +// + +/** + * @brief The base class of the R^n tangent. + * @note See Appendix E. + */ +template +struct RnTangentBase : TangentBase<_Derived> +{ +private: + + using Base = TangentBase<_Derived>; + using Type = RnTangentBase<_Derived>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_OPERATOR + using Base::coeffs; + + RnTangentBase() = default; + ~RnTangentBase() = default; + + // Tangent common API + + /** + * @brief Hat operator of Rn. + * @return An element of the Lie algebra rn. + * @note See Appendix E. + */ + LieAlg hat() const; + + /** + * @brief Get the Rn element. + * @param[out] -optional- J_m_t Jacobian of the Rn element wrt this. + * @return The Rn element. + * @note This is the exp() map with the argument in vector form. + * @note See Eqs. (184) and Eq. (191). + */ + 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 Rn. + * @note See Eq. (191). + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian of Rn. + * @note See Eq. (191). + */ + Jacobian ljac() const; + + /** + * @brief Get the inverse of the right Jacobian of Rn. + * @note See Eq. (191). + * @see rjac. + */ + Jacobian rjacinv() const; + + /** + * @brief Get the inverse of the right Jacobian of Rn. + * @note See Eq. (191). + * @see ljac. + */ + Jacobian ljacinv() const; + + /** + * @brief + * @return + */ + Jacobian smallAdj() const; + + // RnTangent specific API +}; + +template +typename RnTangentBase<_Derived>::LieGroup +RnTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const +{ + if (J_m_t) + { + J_m_t->setIdentity(); + } + + return LieGroup(coeffs()); +} + +template +typename RnTangentBase<_Derived>::LieGroup +RnTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const +{ + return exp(J_m_t); +} + +template +typename RnTangentBase<_Derived>::LieAlg +RnTangentBase<_Derived>::hat() const +{ + LieAlg t_hat = LieAlg::Constant(0); + t_hat.template topRightCorner() = coeffs(); + return t_hat; +} + +template +typename RnTangentBase<_Derived>::Jacobian +RnTangentBase<_Derived>::rjac() const +{ + static const Jacobian Jr = Jacobian::Identity(); + return Jr; +} + +template +typename RnTangentBase<_Derived>::Jacobian +RnTangentBase<_Derived>::ljac() const +{ + static const Jacobian Jl = Jacobian::Identity(); + return Jl; +} + +template +typename RnTangentBase<_Derived>::Jacobian +RnTangentBase<_Derived>::rjacinv() const +{ + return rjac(); +} + +template +typename RnTangentBase<_Derived>::Jacobian +RnTangentBase<_Derived>::ljacinv() const +{ + return ljac(); +} + +template +typename RnTangentBase<_Derived>::Jacobian +RnTangentBase<_Derived>::smallAdj() const +{ + static const Jacobian smallAdj = Jacobian::Constant(0); + return smallAdj; +} + +// RnTangent specific API + +namespace internal { + +/** + * @brief Generator specialization for RnTangentBase objects. + */ +template +struct GeneratorEvaluator> +{ + static typename RnTangentBase::LieAlg + run(const unsigned int i) + { + MANIF_CHECK(i::DoF, + "Index i must less than DoF!", + invalid_argument); + + using LieAlg = typename RnTangentBase::LieAlg; + + LieAlg Ei = LieAlg::Constant(0); + + Ei(i, RnTangentBase::DoF) = 1; + + return Ei; + } +}; + +//! @brief Random specialization for RnTangentBase objects. +template +struct RandomEvaluatorImpl> +{ + static void run(RnTangentBase& m) + { + m.coeffs().setRandom(); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_RNTANGENT_BASE_H_ diff --git a/include/manif/impl/rn/RnTangent_map.h b/include/manif/impl/rn/RnTangent_map.h new file mode 100644 index 00000000..3d15b895 --- /dev/null +++ b/include/manif/impl/rn/RnTangent_map.h @@ -0,0 +1,81 @@ +#ifndef _MANIF_MANIF_RNTANGENT_MAP_H_ +#define _MANIF_MANIF_RNTANGENT_MAP_H_ + +#include "manif/impl/rn/RnTangent.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>; +}; + +//! @brief traits specialization for Eigen Map const +template +struct traits< Eigen::Map,0> > + : public traits> +{ + using typename traits>::Scalar; + using traits>::DoF; + using DataType = ::Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +//! @brief Specialization of Map for manif::RnTangent +template +class Map, 0> + : public manif::RnTangentBase, 0> > +{ + using Base = manif::RnTangentBase, 0> >; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + Map(Scalar* coeffs) : data_(coeffs) { } + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +//! @brief Specialization of Map for const manif::RnTangent +template +class Map, 0> + : public manif::RnTangentBase, 0> > +{ + using Base = manif::RnTangentBase, 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_RNTANGENT_MAP_H_ diff --git a/include/manif/impl/rn/Rn_base.h b/include/manif/impl/rn/Rn_base.h new file mode 100644 index 00000000..0a9fee68 --- /dev/null +++ b/include/manif/impl/rn/Rn_base.h @@ -0,0 +1,240 @@ +#ifndef _MANIF_MANIF_RN_BASE_H_ +#define _MANIF_MANIF_RN_BASE_H_ + +#include "manif/impl/so2/SO2_properties.h" +#include "manif/impl/lie_group_base.h" + +#include + +namespace manif { + +// +// LieGroup +// + +/** + * @brief The base class of the Rn group. + * @note See Appendix E. + */ +template +struct RnBase : LieGroupBase<_Derived> +{ +private: + + using Base = LieGroupBase<_Derived>; + using Type = RnBase<_Derived>; + +public: + + MANIF_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_AUTO_API + MANIF_INHERIT_GROUP_OPERATOR + + using Base::coeffs; + + using Transformation = typename internal::traits<_Derived>::Transformation; + + // LieGroup common API + + /** + * @brief Get the inverse of this. + * @param[out] -optional- J_minv_m Jacobian of the inverse wrt this. + * @note r^-1 = -r + * @note See Appendix E and Eq. (189). + */ + LieGroup inverse(OptJacobianRef J_minv_m = {}) const; + + /** + * @brief Get the Rn corresponding Lie algebra element in vector form. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The Rn tangent of this. + * @note This is the log() map in vector form. + * @note See Appendix E. + * @see RnTangent. + */ + 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 Rn element. + * @param[in] m Another Rn 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'. + * @note See Eq. (190). + */ + template + LieGroup compose(const LieGroupBase<_DerivedOther>& m, + OptJacobianRef J_mc_ma = {}, + OptJacobianRef J_mc_mb = {}) const; + + /** + * @brief Translation action on a 2-vector. + * @param v A 2-vector. + * @param[out] -optional- J_vout_m The Jacobian of the new object wrt this. + * @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object. + * @return The translated 2-vector. + */ + template + auto + act(const Eigen::MatrixBase<_EigenDerived> &v, + tl::optional>> J_vout_m = {}, + tl::optional>> J_vout_v = {}) const + -> Eigen::Matrix; + + /** + * @brief Get the ajoint matrix of Rn at this. + * @note See Eqs. (188). + */ + Jacobian adj() const; + + // Rn specific functions + + /** + * @brief Get the transformation matrix (2D isometry). + * @note T = | 0 t | + * | 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); + +protected: + + using Base::coeffs_nonconst; +}; + +template +typename RnBase<_Derived>::Transformation +RnBase<_Derived>::transform() const +{ + Transformation T(Transformation::Identity()); + T.template topRightCorner() = coeffs(); + return T; +} + +template +template +_Derived& RnBase<_Derived>::operator =( + const Eigen::MatrixBase<_EigenDerived>& v) +{ + coeffs_nonconst() = v; + return *static_cast< _Derived* >(this); +} + +template +typename RnBase<_Derived>::LieGroup +RnBase<_Derived>::inverse(OptJacobianRef J_minv_m) const +{ + if (J_minv_m) + J_minv_m->setIdentity() *= Scalar(-1); + + return LieGroup(-coeffs()); +} + +template +typename RnBase<_Derived>::Tangent +RnBase<_Derived>::log(OptJacobianRef J_t_m) const +{ + if (J_t_m) + J_t_m->setIdentity(); + + return Tangent(coeffs()); +} + +template +typename RnBase<_Derived>::Tangent +RnBase<_Derived>::lift(OptJacobianRef J_t_m) const +{ + return log(J_t_m); +} + +template +template +typename RnBase<_Derived>::LieGroup +RnBase<_Derived>::compose( + const LieGroupBase<_DerivedOther>& m, + OptJacobianRef J_mc_ma, + OptJacobianRef J_mc_mb) const +{ + using std::abs; + + static_assert( + std::is_base_of, _DerivedOther>::value, + "Argument does not inherit from RnBase !"); + + static_assert( + RnBase<_DerivedOther>::Dim==_DerivedOther::Dim, "Dimension mismatch !"); + + if (J_mc_ma) + J_mc_ma->setIdentity(); + + if (J_mc_mb) + J_mc_mb->setIdentity(); + + return LieGroup(coeffs() + m.coeffs()); +} + +template +template +// Eigen::Matrix::Scalar, RnBase<_Derived>::Dim, 1> +auto +RnBase<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v, + tl::optional>> J_vout_m, + tl::optional>> J_vout_v) const +-> Eigen::Matrix +{ + assert_vector_dim(v, Dim); + + if (J_vout_m) + { + J_vout_m->setIdentity(); + } + + if (J_vout_v) + { + J_vout_v->setIdentity(); + } + + return coeffs() + v; +} + +template +typename RnBase<_Derived>::Jacobian +RnBase<_Derived>::adj() const +{ + static const Jacobian adj = Jacobian::Identity(); + return adj; +} + +namespace internal { + +//! @brief Random specialization for RnBase objects. +template +struct RandomEvaluatorImpl> +{ + template + static void run(T& m) + { + using Tangent = typename Derived::Tangent; + m = Tangent::Random().exp(); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_RN_BASE_H_ diff --git a/include/manif/impl/rn/Rn_map.h b/include/manif/impl/rn/Rn_map.h new file mode 100644 index 00000000..ccba585f --- /dev/null +++ b/include/manif/impl/rn/Rn_map.h @@ -0,0 +1,87 @@ +#ifndef _MANIF_MANIF_RN_MAP_H_ +#define _MANIF_MANIF_RN_MAP_H_ + +#include "manif/impl/rn/Rn.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 = RnBase, 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 = RnBase, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::Rn + */ +template +class Map, 0> + : public manif::RnBase, 0> > +{ + using Base = manif::RnBase, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + Map(Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + friend struct manif::LieGroupBase, 0>>; + DataType& coeffs_nonconst() { return data_; } + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::Rn + */ +template +class Map, 0> + : public manif::RnBase, 0> > +{ + using Base = manif::RnBase, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + + Map(const Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_RN_MAP_H_ diff --git a/include/manif/impl/rn/Rn_properties.h b/include/manif/impl/rn/Rn_properties.h new file mode 100644 index 00000000..8c4250fe --- /dev/null +++ b/include/manif/impl/rn/Rn_properties.h @@ -0,0 +1,33 @@ +#ifndef _MANIF_MANIF_RN_PROPERTIES_H_ +#define _MANIF_MANIF_RN_PROPERTIES_H_ + +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declaration +template struct RnBase; +template struct RnTangentBase; + +namespace internal { + +//! traits specialization +template +struct LieGroupProperties> +{ + static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension + static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom +}; + +//! traits specialization +template +struct LieGroupProperties> +{ + static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension + static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_RN_PROPERTIES_H_ diff --git a/include/manif/impl/se2/SE2.h b/include/manif/impl/se2/SE2.h index 906e7452..bce172cd 100644 --- a/include/manif/impl/se2/SE2.h +++ b/include/manif/impl/se2/SE2.h @@ -61,6 +61,8 @@ struct SE2 : SE2Base> MANIF_COMPLETE_GROUP_TYPEDEF using Translation = typename Base::Translation; MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; using Base::normalize; SE2() = default; diff --git a/include/manif/impl/se2/SE2Tangent_base.h b/include/manif/impl/se2/SE2Tangent_base.h index f5330fda..f87a840f 100644 --- a/include/manif/impl/se2/SE2Tangent_base.h +++ b/include/manif/impl/se2/SE2Tangent_base.h @@ -27,7 +27,6 @@ struct SE2TangentBase : TangentBase<_Derived> SE2TangentBase() = default; ~SE2TangentBase() = default; - MANIF_TANGENT_PROPERTIES MANIF_TANGENT_TYPEDEF MANIF_INHERIT_TANGENT_OPERATOR @@ -291,7 +290,7 @@ template struct GeneratorEvaluator> { static typename SE2TangentBase::LieAlg - run(const int i) + run(const unsigned int i) { using LieAlg = typename SE2TangentBase::LieAlg; using Scalar = typename SE2TangentBase::Scalar; diff --git a/include/manif/impl/se2/SE2_map.h b/include/manif/impl/se2/SE2_map.h index ea0e9eab..8c655d68 100644 --- a/include/manif/impl/se2/SE2_map.h +++ b/include/manif/impl/se2/SE2_map.h @@ -46,6 +46,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(Scalar* coeffs) : data_(coeffs) { } @@ -78,6 +80,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(const Scalar* coeffs) : data_(coeffs) { } diff --git a/include/manif/impl/se3/SE3.h b/include/manif/impl/se3/SE3.h index 57055fb6..909d1153 100644 --- a/include/manif/impl/se3/SE3.h +++ b/include/manif/impl/se3/SE3.h @@ -69,7 +69,8 @@ struct SE3 : SE3Base> using Quaternion = Eigen::Quaternion; MANIF_INHERIT_GROUP_API - + using Base::transform; + using Base::rotation; using Base::normalize; SE3() = default; diff --git a/include/manif/impl/se3/SE3Tangent_base.h b/include/manif/impl/se3/SE3Tangent_base.h index 440612b4..09c28bf7 100644 --- a/include/manif/impl/se3/SE3Tangent_base.h +++ b/include/manif/impl/se3/SE3Tangent_base.h @@ -25,7 +25,6 @@ struct SE3TangentBase : TangentBase<_Derived> public: - MANIF_TANGENT_PROPERTIES MANIF_TANGENT_TYPEDEF MANIF_INHERIT_TANGENT_OPERATOR @@ -369,7 +368,7 @@ template struct GeneratorEvaluator> { static typename SE3TangentBase::LieAlg - run(const int i) + run(const unsigned int i) { using LieAlg = typename SE3TangentBase::LieAlg; using Scalar = typename SE3TangentBase::Scalar; diff --git a/include/manif/impl/se3/SE3_map.h b/include/manif/impl/se3/SE3_map.h index 2ba38dd5..9b6ceaf8 100644 --- a/include/manif/impl/se3/SE3_map.h +++ b/include/manif/impl/se3/SE3_map.h @@ -46,6 +46,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(Scalar* coeffs) : data_(coeffs) { } @@ -71,8 +73,9 @@ class Map, 0> public: MANIF_COMPLETE_GROUP_TYPEDEF - MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(const Scalar* coeffs) : data_(coeffs) { } diff --git a/include/manif/impl/so2/SO2.h b/include/manif/impl/so2/SO2.h index 4487b08e..b77ae3ce 100644 --- a/include/manif/impl/so2/SO2.h +++ b/include/manif/impl/so2/SO2.h @@ -59,6 +59,8 @@ struct SO2 : SO2Base> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; using Base::normalize; SO2() = default; diff --git a/include/manif/impl/so2/SO2Tangent_base.h b/include/manif/impl/so2/SO2Tangent_base.h index 711063f9..da0ae799 100644 --- a/include/manif/impl/so2/SO2Tangent_base.h +++ b/include/manif/impl/so2/SO2Tangent_base.h @@ -24,9 +24,6 @@ struct SO2TangentBase : TangentBase<_Derived> public: - static constexpr int Dim = internal::LieGroupProperties::Dim; - static constexpr int DoF = internal::LieGroupProperties::DoF; - MANIF_TANGENT_TYPEDEF MANIF_INHERIT_TANGENT_OPERATOR @@ -197,9 +194,9 @@ template struct GeneratorEvaluator> { static typename SO2TangentBase::LieAlg - run(const int i) + run(const unsigned int i) { - MANIF_CHECK(i==0 && i::DoF, + MANIF_CHECK(i==0, "Index i must be 0!", invalid_argument); diff --git a/include/manif/impl/so2/SO2_map.h b/include/manif/impl/so2/SO2_map.h index 16d13511..22e3d174 100644 --- a/include/manif/impl/so2/SO2_map.h +++ b/include/manif/impl/so2/SO2_map.h @@ -46,6 +46,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(Scalar* coeffs) : data_(coeffs) { } @@ -72,6 +74,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(const Scalar* coeffs) : data_(coeffs) { } diff --git a/include/manif/impl/so3/SO3.h b/include/manif/impl/so3/SO3.h index 6afba6b5..76cad3a8 100644 --- a/include/manif/impl/so3/SO3.h +++ b/include/manif/impl/so3/SO3.h @@ -61,7 +61,8 @@ struct SO3 : SO3Base> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API - + using Base::transform; + using Base::rotation; using Base::quat; using Base::normalize; diff --git a/include/manif/impl/so3/SO3Tangent_base.h b/include/manif/impl/so3/SO3Tangent_base.h index 9c476d77..8272064f 100644 --- a/include/manif/impl/so3/SO3Tangent_base.h +++ b/include/manif/impl/so3/SO3Tangent_base.h @@ -24,8 +24,6 @@ struct SO3TangentBase : TangentBase<_Derived> public: - MANIF_TANGENT_PROPERTIES - MANIF_TANGENT_TYPEDEF MANIF_INHERIT_TANGENT_OPERATOR @@ -254,7 +252,7 @@ template struct GeneratorEvaluator> { static typename SO3TangentBase::LieAlg - run(const int i) + run(const unsigned int i) { using LieAlg = typename SO3TangentBase::LieAlg; using Scalar = typename SO3TangentBase::Scalar; diff --git a/include/manif/impl/so3/SO3_map.h b/include/manif/impl/so3/SO3_map.h index d7792f1c..98418c69 100644 --- a/include/manif/impl/so3/SO3_map.h +++ b/include/manif/impl/so3/SO3_map.h @@ -46,6 +46,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(Scalar* coeffs) : data_(coeffs) { } @@ -72,6 +74,8 @@ class Map, 0> MANIF_COMPLETE_GROUP_TYPEDEF MANIF_INHERIT_GROUP_API + using Base::transform; + using Base::rotation; Map(const Scalar* coeffs) : data_(coeffs) { } diff --git a/include/manif/manif.h b/include/manif/manif.h index 64334680..ead2ecca 100644 --- a/include/manif/manif.h +++ b/include/manif/manif.h @@ -8,6 +8,7 @@ #include "manif/SE2.h" #include "manif/SO3.h" #include "manif/SE3.h" +#include "manif/Rn.h" #include "manif/algorithms/average.h" #include "manif/algorithms/decasteljau.h" diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 133a6fa2..47ebf570 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -49,6 +49,9 @@ endfunction() include_directories(${GTEST_INCLUDE_DIRS}) +# R^n tests +add_subdirectory(rn) + # SO2 tests add_subdirectory(so2) diff --git a/test/common_tester.h b/test/common_tester.h index 180719c5..1d8254fa 100644 --- a/test/common_tester.h +++ b/test/common_tester.h @@ -8,9 +8,7 @@ #define MANIF_TEST(manifold) \ using TEST_##manifold##_TESTER = CommonTester; \ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_COPY_CONSTRUCTOR) \ - { evalCopyConstructor(); } \ - TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_UNNORMALIZE_DATA) \ - { evalConstructorUnnormalizedData(); } \ + { evalCopyConstructor(); } \ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_ASSIGNMENT) \ { evalAssignment(); } \ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_DATA_PTR_VALID) \ @@ -67,8 +65,6 @@ { evalInner(); } \ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_NUMERICAL_STABILITY) \ { evalNumericalStability(); } \ - TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_NORMALIZE) \ - { evalNormalize(); } \ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_SMALL_ADJ) \ { evalSmallAdj(); } \ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_IDENTITY_ACT_POINT) \ @@ -125,6 +121,8 @@ class CommonTester : public ::testing::Test public: + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(LieGroup) + CommonTester() = default; ~CommonTester() = default; @@ -144,14 +142,6 @@ class CommonTester : public ::testing::Test EXPECT_MANIF_NEAR(state, state_copy, tol_); } - void evalConstructorUnnormalizedData() - { - using DataType = typename LieGroup::DataType; - EXPECT_THROW( - LieGroup(DataType::Random()*10.), manif::invalid_argument - ); - } - void evalAssignment() { LieGroup state_copy; @@ -333,9 +323,12 @@ class CommonTester : public ::testing::Test EXPECT_THROW(average_biinvariant(std::vector{}), std::runtime_error); - const auto dummy = LieGroup::Random(); - EXPECT_MANIF_NEAR(dummy, - average_biinvariant(std::vector{dummy}), tol_); + { + const auto dummy = LieGroup::Random(); + std::vector tmp; + tmp.push_back(dummy); + EXPECT_MANIF_NEAR(dummy, average_biinvariant(tmp), tol_); + } const LieGroup centroid = LieGroup::Random(); @@ -525,22 +518,6 @@ class CommonTester : public ::testing::Test ) << "+= failed at iteration " << i ; } - void evalNormalize() - { - typename LieGroup::DataType data = LieGroup::DataType::Random() * 100.; - - EXPECT_THROW( - LieGroup a(data), manif::invalid_argument - ); - - Eigen::Map map(data.data()); - map.normalize(); - - EXPECT_NO_THROW( - LieGroup b = map - ); - } - void evalSmallAdj() { const Tangent delta_other = Tangent::Random(); @@ -562,7 +539,8 @@ class CommonTester : public ::testing::Test protected: - Scalar tol_ = Constants::eps; + // relax eps for float type + Scalar tol_ = (std::is_same::value)? 5e-7 : 1e-8; LieGroup state; LieGroup state_other; @@ -595,10 +573,13 @@ template class JacobianTester : public ::testing::Test { using LieGroup = _LieGroup; - using Tangent = typename _LieGroup::Tangent; + using Scalar = typename LieGroup::Scalar; + using Tangent = typename LieGroup::Tangent; public: + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(LieGroup) + JacobianTester() = default; ~JacobianTester() = default; @@ -901,7 +882,7 @@ class JacobianTester : public ::testing::Test Point point_pert = (state+w).act(point); Point point_lin = pointout + (J_pout_s*w.coeffs()); - EXPECT_EIGEN_NEAR(point_pert, point_lin, 1e-7); + EXPECT_EIGEN_NEAR(point_pert, point_lin, tol_); // Jac wrt second element @@ -966,7 +947,9 @@ class JacobianTester : public ::testing::Test protected: double w_order_ = 1e-4; - double tol_ = 1e-8; + + // relax tolerance for float type + Scalar tol_ = (std::is_same::value)? 1e-4 : 1e-7; LieGroup state; LieGroup state_other; diff --git a/test/rn/CMakeLists.txt b/test/rn/CMakeLists.txt new file mode 100644 index 00000000..44f722d5 --- /dev/null +++ b/test/rn/CMakeLists.txt @@ -0,0 +1,13 @@ +# SO3 tests + +manif_add_gtest(gtest_rn gtest_rn.cpp) + +set(CXX_11_TEST_TARGETS + + ${CXX_11_TEST_TARGETS} + + # R^n + gtest_rn + + PARENT_SCOPE +) diff --git a/test/rn/gtest_rn.cpp b/test/rn/gtest_rn.cpp new file mode 100644 index 00000000..7074e5eb --- /dev/null +++ b/test/rn/gtest_rn.cpp @@ -0,0 +1,159 @@ +#include + +#include "manif/Rn.h" + +#include "../common_tester.h" + +#include + +using namespace manif; + +// specialize std::vector for 'fixed-size vectorizable' Eigen object +// that are multiple of 32 bytes +// @todo: investigate why only this alignment is troublesome +// especially, SO3 wasn't an issue despite being Eigen::Vector4d too... +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(R4d) + +#ifdef MANIF_COVERAGE_ENABLED + +MANIF_TEST(R4d); +MANIF_TEST_JACOBIANS(R4d); + +#else + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(R8d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(R8f) + +TEST(TEST_RN, TEST_RN_VEC_ASSIGN_OP) +{ + { + R1d::DataType data = R1d::DataType::Random(); + + R1d r1; + r1 = data; + + EXPECT_EIGEN_NEAR(data, r1.coeffs()); + } + + { + R2d::DataType data = R2d::DataType::Random(); + + R2d r2; + r2 = data; + + EXPECT_EIGEN_NEAR(data, r2.coeffs()); + } + + { + R3d::DataType data = R3d::DataType::Random(); + + R3d r3; + r3 = data; + + EXPECT_EIGEN_NEAR(data, r3.coeffs()); + } + + { + R4d::DataType data = R4d::DataType::Random(); + + R4d r4; + r4 = data; + + EXPECT_EIGEN_NEAR(data, r4.coeffs()); + } + + { + R5d::DataType data = R5d::DataType::Random(); + + R5d r5; + r5 = data; + + EXPECT_EIGEN_NEAR(data, r5.coeffs()); + } + + { + R6d::DataType data = R6d::DataType::Random(); + + R6d r6; + r6 = data; + + EXPECT_EIGEN_NEAR(data, r6.coeffs()); + } + + { + R7d::DataType data = R7d::DataType::Random(); + + R7d r7; + r7 = data; + + EXPECT_EIGEN_NEAR(data, r7.coeffs()); + } + + { + R8d::DataType data = R8d::DataType::Random(); + + R8d r8; + r8 = data; + + EXPECT_EIGEN_NEAR(data, r8.coeffs()); + } + + { + R9d::DataType data = R9d::DataType::Random(); + + R9d r9; + r9 = data; + + EXPECT_EIGEN_NEAR(data, r9.coeffs()); + } +} + +// This is a little too heavy for coverage and not relevant... + +MANIF_TEST(R1f); +MANIF_TEST(R2f); +MANIF_TEST(R3f); +MANIF_TEST(R4f); +MANIF_TEST(R5f); +MANIF_TEST(R6f); +MANIF_TEST(R7f); +MANIF_TEST(R8f); +MANIF_TEST(R9f); + +MANIF_TEST_JACOBIANS(R1f); +MANIF_TEST_JACOBIANS(R2f); +MANIF_TEST_JACOBIANS(R3f); +MANIF_TEST_JACOBIANS(R4f); +MANIF_TEST_JACOBIANS(R5f); +MANIF_TEST_JACOBIANS(R6f); +MANIF_TEST_JACOBIANS(R7f); +MANIF_TEST_JACOBIANS(R8f); +MANIF_TEST_JACOBIANS(R9f); + +MANIF_TEST(R1d); +MANIF_TEST(R2d); +MANIF_TEST(R3d); +MANIF_TEST(R4d); +MANIF_TEST(R5d); +MANIF_TEST(R6d); +MANIF_TEST(R7d); +MANIF_TEST(R8d); +MANIF_TEST(R9d); + +MANIF_TEST_JACOBIANS(R1d); +MANIF_TEST_JACOBIANS(R2d); +MANIF_TEST_JACOBIANS(R3d); +MANIF_TEST_JACOBIANS(R4d); +MANIF_TEST_JACOBIANS(R5d); +MANIF_TEST_JACOBIANS(R6d); +MANIF_TEST_JACOBIANS(R7d); +MANIF_TEST_JACOBIANS(R8d); +MANIF_TEST_JACOBIANS(R9d); + +#endif + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/se2/gtest_se2.cpp b/test/se2/gtest_se2.cpp index e7dd5c6f..fe6b0411 100644 --- a/test/se2/gtest_se2.cpp +++ b/test/se2/gtest_se2.cpp @@ -523,6 +523,31 @@ TEST(TEST_SE2, TEST_SE2_ACT) EXPECT_NEAR(1, transformed_point.y(), 1e-15); } +TEST(TEST_SE2, TEST_SE2_CONSTRUCTOR_UNNORMALIZED) +{ + using DataType = typename SE2d::DataType; + EXPECT_THROW( + SE2d(DataType::Random()*10.), manif::invalid_argument + ); +} + +TEST(TEST_SE2, TEST_SE2_NORMALIZE) +{ + using DataType = SE2d::DataType; + DataType data = DataType::Random() * 100.; + + EXPECT_THROW( + SE2d a(data), manif::invalid_argument + ); + + Eigen::Map map(data.data()); + map.normalize(); + + EXPECT_NO_THROW( + SE2d b = map + ); +} + MANIF_TEST(SE2d); MANIF_TEST_JACOBIANS(SE2d); diff --git a/test/se3/gtest_se3.cpp b/test/se3/gtest_se3.cpp index 3a68bd6c..d1078aea 100644 --- a/test/se3/gtest_se3.cpp +++ b/test/se3/gtest_se3.cpp @@ -353,7 +353,30 @@ TEST(TEST_SE3, TEST_SE3_ISOMETRY) EXPECT_DOUBLE_EQ(3, se3h.matrix()(2,3)); } +TEST(TEST_SE3, TEST_SE3_CONSTRUCTOR_UNNORMALIZED) +{ + using DataType = typename SE3d::DataType; + EXPECT_THROW( + SE3d(DataType::Random()*10.), manif::invalid_argument + ); +} + +TEST(TEST_SE3, TEST_SE3_NORMALIZE) +{ + using DataType = SE3d::DataType; + DataType data = DataType::Random() * 100.; + EXPECT_THROW( + SE3d a(data), manif::invalid_argument + ); + + Eigen::Map map(data.data()); + map.normalize(); + + EXPECT_NO_THROW( + SE3d b = map + ); +} MANIF_TEST(SE3d); diff --git a/test/so2/gtest_so2.cpp b/test/so2/gtest_so2.cpp index f5d033ac..4f9204df 100644 --- a/test/so2/gtest_so2.cpp +++ b/test/so2/gtest_so2.cpp @@ -551,6 +551,31 @@ TEST(TEST_SO2, TEST_SO2_ACT) EXPECT_NEAR(+1, transformed_point.y(), 1e-15); } +TEST(TEST_SO2, TEST_SO2_CONSTRUCTOR_UNNORMALIZED) +{ + using DataType = typename SO2d::DataType; + EXPECT_THROW( + SO2d(DataType::Random()*10.), manif::invalid_argument + ); +} + +TEST(TEST_SO2, TEST_SO2_NORMALIZE) +{ + using DataType = SO2d::DataType; + DataType data = DataType::Random() * 100.; + + EXPECT_THROW( + SO2d a(data), manif::invalid_argument + ); + + Eigen::Map map(data.data()); + map.normalize(); + + EXPECT_NO_THROW( + SO2d b = map + ); +} + MANIF_TEST(SO2d); MANIF_TEST_JACOBIANS(SO2d); diff --git a/test/so3/gtest_so3.cpp b/test/so3/gtest_so3.cpp index 8751cf26..48276a2e 100644 --- a/test/so3/gtest_so3.cpp +++ b/test/so3/gtest_so3.cpp @@ -318,9 +318,9 @@ TEST(TEST_SO3, TEST_SO3_RMINUS) so3c = so3a.rminus(so3b); - EXPECT_DOUBLE_EQ(0, so3c.coeffs()(0)); - EXPECT_DOUBLE_EQ(0, so3c.coeffs()(1)); - EXPECT_DOUBLE_EQ(0, so3c.coeffs()(2)); + EXPECT_NEAR(0, so3c.coeffs()(0), 1e-15); + EXPECT_NEAR(0, so3c.coeffs()(1), 1e-15); + EXPECT_NEAR(0, so3c.coeffs()(2), 1e-15); // todo subtracting something from something } @@ -343,9 +343,9 @@ TEST(TEST_SO3, TEST_SO3_LMINUS) so3c = so3a.rminus(so3b); - EXPECT_DOUBLE_EQ(0, so3c.coeffs()(0)); - EXPECT_DOUBLE_EQ(0, so3c.coeffs()(1)); - EXPECT_DOUBLE_EQ(0, so3c.coeffs()(2)); + EXPECT_NEAR(0, so3c.coeffs()(0), 1e-15); + EXPECT_NEAR(0, so3c.coeffs()(1), 1e-15); + EXPECT_NEAR(0, so3c.coeffs()(2), 1e-15); // todo subtracting something from something } @@ -566,6 +566,31 @@ TEST(TEST_SO3, TEST_SO3_ACT) EXPECT_NEAR( 1, transformed_point.z(), 1e-15); } +TEST(TEST_SO3, TEST_SO3_CONSTRUCTOR_UNNORMALIZED) +{ + using DataType = typename SO3d::DataType; + EXPECT_THROW( + SO3d(DataType::Random()*10.), manif::invalid_argument + ); +} + +TEST(TEST_SO3, TEST_SO3_NORMALIZE) +{ + using DataType = SO3d::DataType; + DataType data = DataType::Random() * 100.; + + EXPECT_THROW( + SO3d a(data), manif::invalid_argument + ); + + Eigen::Map map(data.data()); + map.normalize(); + + EXPECT_NO_THROW( + SO3d b = map + ); +} + MANIF_TEST(SO3d); MANIF_TEST_JACOBIANS(SO3d);