diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e8789628..98e87e2e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,13 +21,15 @@ jobs: matrix: os: [ubuntu-20.04, ubuntu-22.04] build_type: [Release, Debug] - compiler: [{ - "cc": "gcc", - "cxx": "g++" - }, { - "cc": "clang", - "cxx": "clang++" - }] + compiler: [ + { + "cc": "gcc", + "cxx": "g++" + }, { + "cc": "clang", + "cxx": "clang++" + } + ] env: CC: ${{ matrix.compiler.cc }} CXX: ${{ matrix.compiler.cxx }} @@ -88,13 +90,15 @@ jobs: runs-on: ${{ matrix.combinations.os }} strategy: matrix: - combinations: [{ - "os": "windows-2019", - "cmake_generator": "Visual Studio 16 2019" - }, { - "os": "windows-2022", - "cmake_generator": "Visual Studio 17 2022" - }] + combinations: [ + { + "os": "windows-2019", + "cmake_generator": "Visual Studio 16 2019" + }, { + "os": "windows-2022", + "cmake_generator": "Visual Studio 17 2022" + } + ] steps: - name: Checkout uses: actions/checkout@v2 diff --git a/README.md b/README.md index 867c66e7..5a9cf91f 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ At the moment, it provides the groups: introduced (to the best of knowledge) in this [paper][barrau15]. NOTE: The implementation here differs slightly from the developments in the [paper][barrau15]. +- SGal(3): The Special Galilean group (rotation, translation, velocity and time) in 3D space, described in these papers [[1][fourmy19]] & [[2][kelly24]]. - Bundle<>: allows manipulating a manifold bundle as a single Lie group. Referred to as a *composite manifold* in Section IV of the [reference paper](http://arxiv.org/abs/1812.01537). @@ -170,6 +171,8 @@ Want to contribute? Great! Check out our [contribution guidelines](CONTRIBUTING. [jsola18]: http://arxiv.org/abs/1812.01537 [jsola18v]: http://arxiv.org/abs/1812.01537v4 [barrau15]: https://arxiv.org/pdf/1410.1465.pdf +[fourmy19]: https://hal.science/hal-02183498/document +[kelly24]: https://arxiv.org/abs/2312.07555 [deray20]: https://joss.theoj.org/papers/10.21105/joss.01371 [jsola-iri-lecture]: https://www.youtube.com/watch?v=nHOcoIyJj2o diff --git a/include/manif/SGal3.h b/include/manif/SGal3.h new file mode 100644 index 00000000..895e29ea --- /dev/null +++ b/include/manif/SGal3.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SGAL3_H_ +#define _MANIF_SGAL3_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/sgal3/SGal3_base.h" +#include "manif/impl/sgal3/SGal3Tangent_base.h" +#include "manif/impl/sgal3/SGal3.h" +#include "manif/impl/sgal3/SGal3Tangent.h" +#include "manif/impl/sgal3/SGal3_map.h" +#include "manif/impl/sgal3/SGal3Tangent_map.h" + +#endif // _MANIF_SGAL3_H_ diff --git a/include/manif/impl/sgal3/SGal3.h b/include/manif/impl/sgal3/SGal3.h new file mode 100644 index 00000000..66312d08 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3.h @@ -0,0 +1,254 @@ +#ifndef _MANIF_MANIF_SGAL3_H_ +#define _MANIF_MANIF_SGAL3_H_ + +#include "manif/impl/sgal3/SGal3_base.h" + +namespace manif { + +// Forward declare for type traits specialization +template struct SGal3; +template struct SGal3Tangent; + +namespace internal { + +//! Traits specialization +template +struct traits> { + using Scalar = _Scalar; + + using LieGroup = SGal3<_Scalar>; + using Tangent = SGal3Tangent<_Scalar>; + + using Base = SGal3Base>; + + static constexpr int Dim = LieGroupProperties::Dim; + static constexpr int DoF = LieGroupProperties::DoF; + static constexpr int RepSize = 11; + + /// @todo would be nice to concat vec3 + quaternion + vec3 + t + using DataType = Eigen::Matrix; + + using Jacobian = Eigen::Matrix; + using Rotation = Eigen::Matrix; + using Translation = Eigen::Matrix; + using LinearVelocity = Eigen::Matrix; + using Vector = Eigen::Matrix; +}; + +} // namespace internal +} // namespace manif + +namespace manif { + +// +// LieGroup +// + +/** + * @brief Represent an element of SGal3. + */ +template +struct SGal3 : SGal3Base> { +private: + + using Base = SGal3Base>; + using Type = SGal3<_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 LinearVelocity = typename Base::LinearVelocity; + + MANIF_INHERIT_GROUP_API + using Base::rotation; + using Base::normalize; + + SGal3() = default; + ~SGal3() = default; + + MANIF_COPY_CONSTRUCTOR(SGal3) + MANIF_MOVE_CONSTRUCTOR(SGal3) + + template + SGal3(const LieGroupBase<_DerivedOther>& o); + + MANIF_GROUP_ASSIGN_OP(SGal3) + + /** + * @brief Constructor given a translation, a unit quaternion and a linear velocity. + * @param[in] t A translation vector. + * @param[in] q A unit quaternion. + * @param[in] v A linear velocity vector. + * @param[in] time A time. + * @throws manif::invalid_argument on un-normalized complex number. + */ + SGal3( + const Translation& t, + const Eigen::Quaternion& q, + const LinearVelocity& v, + const Scalar time + ); + + /** + * @brief Constructor given a translation, an angle axis and a linear velocity. + * @param[in] t A translation vector. + * @param[in] angle_axis An angle-axis. + * @param[in] v A linear velocity vector. + * @param[in] time A time. + */ + SGal3( + const Translation& t, + const Eigen::AngleAxis& angle_axis, + const LinearVelocity& v, + const Scalar time + ); + + /** + * @brief Constructor given a translation, SO3 element and a linear velocity. + * @param[in] t A translation vector. + * @param[in] SO3 An element of SO3. + * @param[in] v A linear velocity vector. + * @param[in] time A time. + */ + SGal3( + const Translation& t, + const SO3& SO3, + const LinearVelocity& v, + const Scalar time + ); + + /** + * @brief Constructor given translation components, + * roll-pitch-yaw angles and linear velocity components + * @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. + * @param[in] vx The x component of the linear velocity. + * @param[in] vy The y component of the linear velocity. + * @param[in] vz The z component of the linear velocity. + * @param[in] t time. + */ + SGal3( + const Scalar x, const Scalar y, const Scalar z, + const Scalar roll, const Scalar pitch, const Scalar yaw, + const Scalar vx, const Scalar vy, const Scalar vz, + const Scalar t + ); + + /** + * @brief Constructor from a 3D Eigen::Isometry relevant to SE(3) and a linear velocity + * @param[in] h a isometry object from Eigen defined for SE(3) + * @param[in] v a linear velocity vector. + * @note overall, this should be a double direct spatial isometry, + */ + SGal3( + const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h, + const LinearVelocity& v, + const Scalar t + ); + + // LieGroup common API + + DataType& coeffs(); + const DataType& coeffs() const; + + // SGal3 specific API + +protected: + + DataType data_; +}; + +MANIF_EXTRA_GROUP_TYPEDEF(SGal3) + +template +template +SGal3<_Scalar>::SGal3(const LieGroupBase<_DerivedOther>& o) : SGal3(o.coeffs()) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Translation& t, + const Eigen::Quaternion& q, + const LinearVelocity& v, + const Scalar time +) : SGal3((DataType() << t, q.coeffs(), v, time).finished()) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Translation& t, + const Eigen::AngleAxis& a, + const LinearVelocity& v, + const Scalar time +) : SGal3(t, Quaternion(a), v, time) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Scalar x, const Scalar y, const Scalar z, + const Scalar roll, const Scalar pitch, const Scalar yaw, + const Scalar vx, const Scalar vy, const Scalar vz, + const Scalar t +) : SGal3( + Translation(x,y,z), + Eigen::Quaternion( + Eigen::AngleAxis(yaw, Eigen::Matrix::UnitZ()) * + Eigen::AngleAxis(pitch, Eigen::Matrix::UnitY()) * + Eigen::AngleAxis(roll, Eigen::Matrix::UnitX()) + ), + LinearVelocity(vx, vy, vz), + t +) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Translation& t, + const SO3& so3, + const LinearVelocity& v, + const Scalar time +) : SGal3(t, so3.quat(), v, time) { + // +} + +template +SGal3<_Scalar>::SGal3( + const Eigen::Transform<_Scalar, 3, Eigen::Isometry>& h, + const LinearVelocity& v, + const Scalar t +) : SGal3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()), v, t) { + // +} + + +template +typename SGal3<_Scalar>::DataType& +SGal3<_Scalar>::coeffs() { + return data_; +} + +template +const typename SGal3<_Scalar>::DataType& +SGal3<_Scalar>::coeffs() const { + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3_H_ diff --git a/include/manif/impl/sgal3/SGal3Tangent.h b/include/manif/impl/sgal3/SGal3Tangent.h new file mode 100644 index 00000000..97692f18 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3Tangent.h @@ -0,0 +1,106 @@ +#ifndef _MANIF_MANIF_SGAL3TANGENT_H_ +#define _MANIF_MANIF_SGAL3TANGENT_H_ + +#include "manif/impl/sgal3/SGal3Tangent_base.h" + +namespace manif { +namespace internal { + +//! Traits specialization +template +struct traits> { + using Scalar = _Scalar; + + using LieGroup = SGal3<_Scalar>; + using Tangent = SGal3Tangent<_Scalar>; + + using Base = SGal3TangentBase; + + 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 SGal3. + */ +template +struct SGal3Tangent : SGal3TangentBase> { +private: + + using Base = SGal3TangentBase>; + using Type = SGal3Tangent<_Scalar>; + +protected: + + using Base::derived; + +public: + + MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + SGal3Tangent() = default; + ~SGal3Tangent() = default; + + MANIF_COPY_CONSTRUCTOR(SGal3Tangent) + MANIF_MOVE_CONSTRUCTOR(SGal3Tangent) + + template + SGal3Tangent(const TangentBase<_DerivedOther>& o); + + MANIF_TANGENT_ASSIGN_OP(SGal3Tangent) + + // Tangent common API + + DataType& coeffs(); + const DataType& coeffs() const; + + // SGal3Tangent specific API + +protected: + + DataType data_; +}; + +MANIF_EXTRA_TANGENT_TYPEDEF(SGal3Tangent); + +template +template +SGal3Tangent<_Scalar>::SGal3Tangent(const TangentBase<_DerivedOther>& o) + : data_(o.coeffs()) { + // +} + +template +typename SGal3Tangent<_Scalar>::DataType& +SGal3Tangent<_Scalar>::coeffs() { + return data_; +} + +template +const typename SGal3Tangent<_Scalar>::DataType& +SGal3Tangent<_Scalar>::coeffs() const { + return data_; +} + +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3TANGENT_H_ diff --git a/include/manif/impl/sgal3/SGal3Tangent_base.h b/include/manif/impl/sgal3/SGal3Tangent_base.h new file mode 100644 index 00000000..ad705827 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3Tangent_base.h @@ -0,0 +1,623 @@ +#ifndef _MANIF_MANIF_SGAL3TANGENT_BASE_H_ +#define _MANIF_MANIF_SGAL3TANGENT_BASE_H_ + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/tangent_base.h" +#include "manif/impl/so3/SO3Tangent_map.h" +#include "manif/impl/se3/SE3Tangent.h" + +namespace manif { + +// +// Tangent +// + +/** + * @brief The base class of the SGal3 tangent. + */ +template +struct SGal3TangentBase : TangentBase<_Derived> { +private: + + using Base = TangentBase<_Derived>; + using Type = SGal3TangentBase<_Derived>; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + using LinBlock = typename DataType::template FixedSegmentReturnType<3>::Type; + using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type; + using ConstLinBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type; + using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type; + + using Base::data; + using Base::coeffs; + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SGal3TangentBase) + +public: + + MANIF_TANGENT_ML_ASSIGN_OP(SGal3TangentBase) + + // Tangent common API + + /** + * @brief Hat operator of SGal3. + * @return An element of the Lie algebra se_2_3. + * @note See Eq. (169). + */ + LieAlg hat() const; + + /** + * @brief Get the SGal3 element. + * @param[out] -optional- J_m_t Jacobian of the SGal3 element wrt this. + * @return The SGal3 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 SGal3. + */ + Jacobian rjac() const; + + /** + * @brief Get the left Jacobian of SGal3. + */ + Jacobian ljac() const; + + /** + * @brief Get the small adjoint matrix ad() of SGal3 + * that maps isomorphic tangent vectors of SGal3 + * @return + */ + Jacobian smallAdj() const; + + // SGal3Tangent specific API + + //! @brief Get the linear translation part. + LinBlock lin(); + const ConstLinBlock lin() const; + + //! @brief Get the angular part. + AngBlock ang(); + const ConstAngBlock ang() const; + + //! @brief Get the linear velocity part + LinBlock lin2(); + const ConstLinBlock lin2() const; + + Scalar t() const; + +public: /// @todo make protected + + const Eigen::Map> asSO3() const { + return Eigen::Map>(coeffs().data()+6); + } + + Eigen::Map> asSO3() { + return Eigen::Map>(coeffs().data()+6); + } + + static void fillE( + Eigen::Ref> E, + const Eigen::Map>& so3 + ); +}; + +template +typename SGal3TangentBase<_Derived>::LieGroup +SGal3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const { + if (J_m_t) { + *J_m_t = rjac(); + } + + const Eigen::Map> so3 = asSO3(); + const typename SO3::Jacobian so3_ljac = so3.ljac(); + + Eigen::Matrix E; + fillE(E, so3); + + return LieGroup( + so3_ljac * lin() + (E * (t() * lin2())), + so3.exp(), + so3_ljac * lin2(), + t() + ); +} + +template +typename SGal3TangentBase<_Derived>::LieGroup +SGal3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const { + return exp(J_m_t); +} + +template +typename SGal3TangentBase<_Derived>::LieAlg +SGal3TangentBase<_Derived>::hat() const { + LieAlg sgal3; + + sgal3.template topLeftCorner<3, 3>() = skew(ang()); + sgal3.template block<3, 1>(0, 3) = lin2(); + sgal3.template topRightCorner<3, 1>() = lin(); + sgal3(3, 4) = t(); + + sgal3.template bottomLeftCorner<2, 4>().setZero(); + sgal3(4, 4) = Scalar(0); + + return sgal3; +} + +template +typename SGal3TangentBase<_Derived>::Jacobian +SGal3TangentBase<_Derived>::rjac() const { + + return (-*this).ljac(); + + // Those were verified against auto diff + + // Jacobian Jr = Jacobian::Zero(); + // Jr.template topLeftCorner<3, 3>() = asSO3().rjac(); + // // Jr.template block<3, 3>(0, 3) = ??; + // // Jr.template block<3, 3>(0, 6) = ??; + + // // Jr.template block<3, 1>(0, 9) = ??; + + // Jr.template block<3, 3>(3, 3) = Jr.template topLeftCorner<3,3>(); + // SE3Tangent::fillQ( + // Jr.template block<3, 3>(3, 6), -coeffs().template segment<6>(3) + // ); + + // Jr.template block<3, 3>(6, 6) = Jr.template topLeftCorner<3,3>(); + + // Jr(9, 9) = Scalar(1); + + // Jr.template bottomLeftCorner<7, 3>().setZero(); + // Jr.template block<4, 3>(6, 3).setZero(); + // Jr.template block<1, 3>(9, 6).setZero(); + // Jr.template block<6, 1>(3, 9).setZero(); + + // return Jr; +} + +template +typename SGal3TangentBase<_Derived>::Jacobian +SGal3TangentBase<_Derived>::ljac() const { + using ConstRef33 = const Eigen::Ref>; + using ConstRef61 = const Eigen::Ref>; + + using Diag = typename Eigen::DiagonalMatrix; + auto I33 = [](const Scalar d){ return Diag(d, d, d).toDenseMatrix(); }; + + using std::sqrt; + using std::cos; + using std::sin; + + /** Structure of the left Jacobian according to J. Kelly + * + * Jl = [ D -L*t N E*nu + * 0 D M 0 + * 0 0 D 0 + * 0 0 0 1 ] + * + * with N = N1 - N2. + * + * Tangent space is tau = [rho ; nu ; theta ; t] in R^10 + * + * Matrix blocks D, E, L, M, N, N1, N2 are referred to in the comments and + * correspond to eqs. in Kelly's paper: + * + * D: (18) = Jl_SO3(theta) + * E: (19) + * L: (32) + * M: (33) = Q(nu,theta) + * N: (34) = N1 - N2 + * N1: (35) = Q(rho,theta) + * N2: (36) + * + * Note that we use the following temporary blocks for computation + * + * Jl = [ . . E . + * V . . rho + * W W^2 . theta + * . . . . ] + * + */ + + Jacobian Jl; + + // theta vector + const Eigen::Map> so3 = asSO3(); + + // Skew matrix W = theta^ + Jl.template block<3, 3>(3, 0) = so3.hat(); + ConstRef33 W = Jl.template block<3, 3>(3, 0); + + // Skew matrix W^2 + Jl.template block<3, 3>(6, 3) = W * W; + ConstRef33 WW = Jl.template block<3, 3>(6, 3); + + // Skew matrix V = nu^ + Jl.template block<3, 3>(6, 0) = skew(lin2()); + ConstRef33 V = Jl.template block<3, 3>(6, 0); + + // Angles and trigonometry + const Scalar theta_sq = so3.coeffs().squaredNorm(); + // rotation angle + const Scalar theta = sqrt(theta_sq); + const Scalar theta_cu = theta * theta_sq; + + const Scalar sin_t = sin(theta); + const Scalar cos_t = cos(theta); + + // Blocks D + Jl.template topLeftCorner<3, 3>() = so3.ljac(); + Jl.template block<3, 3>(3, 3) = Jl.template topLeftCorner<3, 3>(); + Jl.template block<3, 3>(6, 6) = Jl.template topLeftCorner<3, 3>(); + + // Block E + // Note - we use here a temporary block to hold E + Jl.template block<3, 3>(0, 6) = I33(Scalar(0.5)); + if (theta_sq > Constants::eps) { + const Scalar A = (theta - sin_t) / theta_sq / theta; + const Scalar B = ( + theta_sq + Scalar(2) * cos_t - Scalar(2) + ) / (Scalar(2) * theta_sq * theta_sq); + + Jl.template block<3, 3>(0, 6).noalias() += A * W + B * WW; + } + + // Block E * nu + Jl.template block<3, 1>(0, 9) = Jl.template block<3, 3>(0, 6) * lin2(); + + // Block L + Scalar cA, cB; + // small angle approx. + if (theta_cu > Constants::eps) { + cA = (sin_t - theta * cos_t) / theta_cu; + cB = ( + theta_sq + Scalar(2) * (Scalar(1) - theta * sin_t - cos_t) + ) / (Scalar(2) * theta_sq * theta_sq); + } else { + cA = Scalar(1./3.) - Scalar(1./30.) * theta_sq; + cB = Scalar(1./8.); + } + + // Block - L * t + Jl.template block<3, 3>(0, 3).noalias() = -t() * ( + // Block L + I33(Scalar(0.5)) + cA * W + cB * WW + ); + + // Block M = Q(nu, theta) + SE3Tangent::fillQ( + Jl.template block<3, 3>(3, 6), coeffs().template segment<6>(3) + ); + + // Block N1, part of N. N1 = Q(rho, theta) + Jl.template block<6, 1>(3, 9) << lin(), ang(); + ConstRef61 rho_theta = Jl.template block<6, 1>(3, 9); + // block N1 = Q(rho,theta) + SE3Tangent::fillQ(Jl.template block<3, 3>(0, 6), rho_theta); + + // Block N2, part of N + Scalar cC, cD, cE, cF; + if (theta_cu > Constants::eps) { + cA = (Scalar(2) - theta * sin_t - Scalar(2) * cos_t) / theta_cu / theta; + cB = ( + theta_cu + Scalar(6) * theta + Scalar(6) * theta * cos_t - Scalar(12) * sin_t + ) / (Scalar(6) * theta_cu * theta_sq); + cC = ( + Scalar(12) * sin_t - theta_cu - Scalar(3) * theta_sq * sin_t - Scalar(12) * theta * cos_t + ) / (Scalar(6) * theta_cu * theta_sq); + cD = ( + Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t + cos_t) + ) / (Scalar(2) * theta_cu * theta_cu); + cE = (theta_sq + Scalar(2) * (cos_t - Scalar(1))) / (Scalar(2) * theta_cu * theta); + cF = (theta_cu + Scalar(6) * (sin_t - theta)) / (Scalar(6) * theta_cu * theta_sq); + } else { + cA = Scalar(1. / 12.); + cB = Scalar(1. / 24.); + cC = Scalar(1. / 10.); + cD = Scalar(1. / 240.); + cE = Scalar(1. / 24.); + cF = Scalar(1. / 120.); + } + + // Block N = N1 - N2 + Jl.template block<3, 3>(0, 6) -= ( + // Block N2 + t() / Scalar(6) * V + + (cA * W + cB * WW) * (t() * V) + + cC * (W * V * (t() * W)) + + cD * (WW * V * (t() * W)) + + t() * V * (cE * W + cF * WW) + ); + + // Block 1 + Jl(9, 9) = Scalar(1); + + // Blocks of zeros + Jl.template bottomLeftCorner<7, 3>().setZero(); + Jl.template block<4, 3>(6, 3).setZero(); + Jl.template block<1, 3>(9, 6).setZero(); + Jl.template block<6, 1>(3, 9).setZero(); + + return Jl; +} + +template +typename SGal3TangentBase<_Derived>::Jacobian +SGal3TangentBase<_Derived>::smallAdj() const { + Jacobian smallAdj; + + smallAdj.template topLeftCorner<3,3>() = skew(ang()); + smallAdj.template block<3, 3>(0, 3) = -t() * Eigen::Matrix3d::Identity(); + smallAdj.template block<3, 3>(0, 6) = skew(lin()); + smallAdj.template block<3, 1>(0, 9) = lin2(); + + smallAdj.template block<3, 3>(3, 3) = smallAdj.template topLeftCorner<3,3>(); + smallAdj.template block<3, 3>(3, 6) = skew(lin2()); + + smallAdj.template block<3, 3>(6, 6) = smallAdj.template topLeftCorner<3,3>(); + + smallAdj.template block<7, 3>(3, 0).setZero(); + smallAdj.template block<4, 3>(6, 3).setZero(); + + smallAdj.template block<1, 3>(9, 6).setZero(); + smallAdj.template block<7, 1>(3, 9).setZero(); + + return smallAdj; +} + +// SGal3Tangent specific API + +template +typename SGal3TangentBase<_Derived>::LinBlock +SGal3TangentBase<_Derived>::lin() { + return coeffs().template head<3>(); +} + +template +const typename SGal3TangentBase<_Derived>::ConstLinBlock +SGal3TangentBase<_Derived>::lin() const { + return coeffs().template head<3>(); +} + +template +typename SGal3TangentBase<_Derived>::LinBlock +SGal3TangentBase<_Derived>::lin2() { + return coeffs().template segment<3>(3); +} + +template +const typename SGal3TangentBase<_Derived>::ConstLinBlock +SGal3TangentBase<_Derived>::lin2() const { + return coeffs().template segment<3>(3); +} + +template +typename SGal3TangentBase<_Derived>::AngBlock +SGal3TangentBase<_Derived>::ang() { + return coeffs().template segment<3>(6); +} + +template +const typename SGal3TangentBase<_Derived>::ConstAngBlock +SGal3TangentBase<_Derived>::ang() const { + return coeffs().template segment<3>(6); +} + +template +typename SGal3TangentBase<_Derived>::Scalar +SGal3TangentBase<_Derived>::t() const { + return coeffs()(9); +} + +template +void SGal3TangentBase<_Derived>::fillE( + Eigen::Ref> E, + const Eigen::Map>& so3 +) { + using I = typename Eigen::DiagonalMatrix; + + const Scalar theta_sq = so3.coeffs().squaredNorm(); + + E.noalias() = I(Scalar(0.5), Scalar(0.5), Scalar(0.5)).toDenseMatrix(); + + // small angle approx. + if (theta_sq < Constants::eps) { + return; + } + + const Scalar theta = sqrt(theta_sq); // rotation angle + + const Scalar A = (theta - sin(theta)) / theta_sq / theta; + const Scalar B = (theta_sq + Scalar(2) * cos(theta) - Scalar(2)) / (Scalar(2) * theta_sq * theta_sq); + + const typename SO3Tangent::LieAlg W = so3.hat(); + + E.noalias() += A * W + B * W * W; +} + +namespace internal { + +//! @brief Generator specialization for SGal3TangentBase objects. +template +struct GeneratorEvaluator> { + static typename SGal3TangentBase::LieAlg + run(const unsigned int i) { + using LieAlg = typename SGal3TangentBase::LieAlg; + using Scalar = typename SGal3TangentBase::Scalar; + + switch (i) { + case 0: { + static const LieAlg E0( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + + ).finished() + ); + return E0; + } + case 1: { + static const LieAlg E1( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E1; + } + case 2: { + static const LieAlg E2( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E2; + } + case 3: { + static const LieAlg E3( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E3; + } + case 4: { + static const LieAlg E4( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E4; + } + case 5: { + static const LieAlg E5( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E5; + } + case 6: { + static const LieAlg E6( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), + Scalar(0), Scalar(1), Scalar( 0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0) + ).finished() + ); + return E6; + } + case 7: { + static const LieAlg E7( + ( + LieAlg() << + Scalar( 0), Scalar(0), Scalar(1), Scalar(0), Scalar(0), + Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E7; + } + case 8: { + static const LieAlg E8( + ( + LieAlg() << + Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0), + Scalar(1), Scalar( 0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E8; + } + case 9: { + static const LieAlg E9( + ( + LieAlg() << + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1), + Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) + ).finished() + ); + return E9; + } + default: + MANIF_THROW("Index i must be in [0,9]!", invalid_argument); + break; + } + + return LieAlg{}; + } +}; + +//! @brief Random specialization for SGal3TangentBase objects. +template +struct RandomEvaluatorImpl> { + static void run(SGal3TangentBase& m) { + // in [-1,1] + m.coeffs().setRandom(); + // In ball of radius PI + m.coeffs().template segment<3>(6) = randPointInBall(MANIF_PI).template cast(); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3TANGENT_BASE_H_ diff --git a/include/manif/impl/sgal3/SGal3Tangent_map.h b/include/manif/impl/sgal3/SGal3Tangent_map.h new file mode 100644 index 00000000..74fc04bd --- /dev/null +++ b/include/manif/impl/sgal3/SGal3Tangent_map.h @@ -0,0 +1,85 @@ +#ifndef _MANIF_MANIF_SGAL3TANGENT_MAP_H_ +#define _MANIF_MANIF_SGAL3TANGENT_MAP_H_ + +#include "manif/impl/sgal3/SGal3Tangent.h" + +namespace manif { +namespace internal { + +//! @brief traits specialization for Eigen Map +template +struct traits, 0> > + : public traits> { + using typename traits>::Scalar; + using traits>::DoF; + using DataType = Eigen::Map, 0>; + using Base = SGal3TangentBase, 0>>; +}; + +//! @brief traits specialization for Eigen Map +template +struct traits, 0> > + : public traits> { + using typename traits>::Scalar; + using traits>::DoF; + using DataType = Eigen::Map, 0>; + using Base = SGal3TangentBase, 0>>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3TangentBase, 0> > { + using Base = manif::SGal3TangentBase, 0> >; + +public: + + MANIF_TANGENT_TYPEDEF + MANIF_INHERIT_TANGENT_API + MANIF_INHERIT_TANGENT_OPERATOR + + Map(Scalar* coeffs) : data_(coeffs) { } + + MANIF_TANGENT_MAP_ASSIGN_OP(SGal3Tangent) + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3TangentBase, 0> > { + using Base = manif::SGal3TangentBase, 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_SGAL3TANGENT_MAP_H_ diff --git a/include/manif/impl/sgal3/SGal3_base.h b/include/manif/impl/sgal3/SGal3_base.h new file mode 100644 index 00000000..07e7e044 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3_base.h @@ -0,0 +1,484 @@ +#ifndef _MANIF_MANIF_SGAL3_BASE_H_ +#define _MANIF_MANIF_SGAL3_BASE_H_ + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/so3/SO3_map.h" +#include "manif/impl/se3/SE3_map.h" + +namespace manif { + +// +// LieGroup +// + +/** + * @brief The base class of the SGal3 group. + * @note See "All About the Galilean Group SGal(3)" J. Kelly. + * https://arxiv.org/abs/2312.07555 + * + */ +template +struct SGal3Base : LieGroupBase<_Derived> { +private: + + using Base = LieGroupBase<_Derived>; + using Type = SGal3Base<_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 LinearVelocity = typename internal::traits<_Derived>::LinearVelocity; + using Time = Scalar; + using Transformation = Eigen::Matrix; + using Isometry = Eigen::Matrix; /**< Double direct spatial isometry*/ + using QuaternionDataType = Eigen::Quaternion; + + // LieGroup common API + +protected: + + using Base::derived; + + MANIF_DEFAULT_CONSTRUCTOR(SGal3Base) + +public: + + MANIF_GROUP_ML_ASSIGN_OP(SGal3Base) + + /** + * @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 SGal3 corresponding Lie algebra element in vector form. + * @param[out] -optional- J_t_m Jacobian of the tangent wrt to this. + * @return The SGal3 tangent of this. + * @note This is the log() map in vector form. + * @see SGal3Tangent. + */ + 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 SGal3 element. + * @param[in] m Another SGal3 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 Get the action + * @param[in] p A 3D point. + * @param[out] -optional- J_pout_m The Jacobian of the new object wrt this. + * @param[out] -optional- J_pout_p The Jacobian of the new object wrt input object. + */ + template + Eigen::Matrix + act( + const Eigen::MatrixBase<_EigenDerived> &p, + tl::optional>> J_pout_m = {}, + tl::optional>> J_pout_p = {} + ) const; + + /** + * @brief Get the adjoint matrix of SGal3 at this. + */ + Jacobian adj() const; + + // SGal3 specific functions + + /** + * Get the isometry object (double direct isometry). + * @note T = | R v t| + * | 1 s| + * | 1| + */ + Transformation transform() const; + + /** + * Get the isometry object (double direct isometry). + * @note T = | R v t| + * | 1 s| + * | 1| + */ + Isometry isometry() const; + + /** + * @brief Get the rotational part of this as a rotation matrix. + */ + Rotation rotation() const; + + /** + * @brief Get the rotational part of this as a quaternion. + */ + QuaternionDataType quat() const; + + /** + * @brief Get the translational part in vector form. + */ + Translation translation() const; + + /** + * @brief Get the x component of the translational part. + */ + Scalar x() const; + + /** + * @brief Get the y component of translational part. + */ + Scalar y() const; + + /** + * @brief Get the z component of translational part. + */ + Scalar z() const; + + /** + * @brief Get the linear velocity part in vector form. + */ + LinearVelocity linearVelocity() const; + + /** + * @brief Get the x component of the linear velocity part. + */ + Scalar vx() const; + + /** + * @brief Get the y component of linear velocity part. + */ + Scalar vy() const; + + /** + * @brief Get the z component of linear velocity part. + */ + Scalar vz() const; + + /** + * @brief Get the time. + */ + Scalar t() const; + + /** + * @brief Normalize the underlying quaternion. + */ + void normalize(); + +public: /// @todo make protected + + Eigen::Map> asSO3() const { + return Eigen::Map>(coeffs().data()+3); + } + + Eigen::Map> asSO3() { + return Eigen::Map>(coeffs().data()+3); + } +}; + +template +typename SGal3Base<_Derived>::Transformation +SGal3Base<_Derived>::transform() const { + Eigen::Matrix T; + T.template topLeftCorner<3, 3>() = rotation(); + T.template block<3, 1>(0, 3) = linearVelocity(); + T.template topRightCorner<3, 1>() = translation(); + T.template bottomLeftCorner<2, 3>().setZero(); + T.template bottomRightCorner<2, 2>().setIdentity(); + T(3, 4) = t(); + return T; +} + +template +typename SGal3Base<_Derived>::Isometry +SGal3Base<_Derived>::isometry() const { + return Isometry(transform()); +} + +template +typename SGal3Base<_Derived>::Rotation +SGal3Base<_Derived>::rotation() const { + return asSO3().rotation(); +} + +template +typename SGal3Base<_Derived>::QuaternionDataType +SGal3Base<_Derived>::quat() const { + return asSO3().quat(); +} + +template +typename SGal3Base<_Derived>::Translation +SGal3Base<_Derived>::translation() const { + return coeffs().template head<3>(); +} + +template +typename SGal3Base<_Derived>::LinearVelocity +SGal3Base<_Derived>::linearVelocity() const { + return coeffs().template segment<3>(7); +} + +template +typename SGal3Base<_Derived>::LieGroup +SGal3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const { + if (J_minv_m) { + (*J_minv_m) = -adj(); + } + + const SO3 so3inv = asSO3().inverse(); + + return LieGroup( + -so3inv.act((translation()-t()*linearVelocity())), + so3inv, + -so3inv.act(linearVelocity()), + -t() + ); +} + +template +typename SGal3Base<_Derived>::Tangent +SGal3Base<_Derived>::log(OptJacobianRef J_t_m) const { + const SO3Tangent so3tan = asSO3().log(); + + Eigen::Matrix E; + Tangent::fillE(E, Eigen::Map>(so3tan.data())); + + const LinearVelocity nu = so3tan.ljacinv() * linearVelocity(); + + Tangent tan( + ( + typename Tangent::DataType() << + so3tan.ljacinv() * (translation() - E * (t() * nu)), nu, so3tan.coeffs(), t() + ).finished() + ); + + if (J_t_m) { + // Jr^-1 + (*J_t_m) = tan.rjacinv(); + } + + return tan; +} + +template +typename SGal3Base<_Derived>::Tangent +SGal3Base<_Derived>::lift(OptJacobianRef J_t_m) const { + return log(J_t_m); +} + +template +template +typename SGal3Base<_Derived>::LieGroup +SGal3Base<_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 SGal3Base !" + ); + + const auto& m_sgal3 = static_cast&>(m); + + if (J_mc_ma) { + (*J_mc_ma) = m.inverse().adj(); + } + + if (J_mc_mb) { + J_mc_mb->setIdentity(); + } + + return LieGroup( + rotation() * m_sgal3.translation() + m_sgal3.t() * linearVelocity() + translation(), + asSO3() * m_sgal3.asSO3(), + rotation() * m_sgal3.linearVelocity() + linearVelocity(), + t() + m_sgal3.t() + ); +} + +template +template +Eigen::Matrix::Scalar, 3, 1> +SGal3Base<_Derived>::act( + const Eigen::MatrixBase<_EigenDerived> &p, + tl::optional>> J_pout_m, + tl::optional>> J_pout_p +) const { + assert_vector_dim(p, 3); + + const Rotation R(rotation()); + + if (J_pout_m) { + J_pout_m->template topLeftCorner<3, 3>() = R; + J_pout_m->template block<3, 3>(0, 3).setZero(); + J_pout_m->template block<3, 3>(0, 6).noalias() = -R * skew(p); + J_pout_m->template topRightCorner<3, 1>() = linearVelocity(); + } + + if (J_pout_p) { + (*J_pout_p) = R; + } + + return translation() + R * p; +} + + +template +typename SGal3Base<_Derived>::Jacobian +SGal3Base<_Derived>::adj() const { + /// + /// this is + /// Ad(g) = | R -R.tau [(t-v.tau)]x.R v| + /// | 0 R [v]x.R 0| + /// | 0 0 R 0| + /// | 0 0 0 1| + /// + /// considering vee(log(g)) = (rho;v;w;iota) + + Jacobian Adj; + Adj.template topLeftCorner<3, 3>() = rotation(); + Adj.template block<3, 3>(0, 3).noalias() = -t() * Adj.template topLeftCorner<3, 3>(); + Adj.template block<3, 3>(0, 6).noalias() = + skew(translation() - t() * linearVelocity()) * Adj.template topLeftCorner<3, 3>(); + Adj.template topRightCorner<3, 1>() = linearVelocity(); + + Adj.template block<3, 3>(3, 3) = Adj.template topLeftCorner<3, 3>(); + Adj.template block<3, 3>(3, 6).noalias() = + skew(linearVelocity()) * Adj.template topLeftCorner<3, 3>(); + + Adj.template block<3, 3>(6, 6) = Adj.template topLeftCorner<3, 3>(); + + Adj.template bottomLeftCorner<7, 3>().setZero(); + Adj.template block<4, 3>(6, 3).setZero(); + Adj.template block<1, 3>(9, 6).setZero(); + Adj.template block<6, 1>(3, 9).setZero(); + + Adj(9, 9) = Scalar(1); + + return Adj; +} + +// SGal3 specific function + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::x() const { + return coeffs()(0); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::y() const { + return coeffs()(1); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::z() const { + return coeffs()(2); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::vx() const { + return coeffs()(7); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::vy() const { + return coeffs()(8); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::vz() const { + return coeffs()(9); +} + +template +typename SGal3Base<_Derived>::Scalar +SGal3Base<_Derived>::t() const { + return coeffs()(10); +} + +template +void SGal3Base<_Derived>::normalize() { + coeffs().template segment<4>(3).normalize(); +} + +namespace internal { + +//! @brief Random specialization for SGal3Base objects. +template +struct RandomEvaluatorImpl> { + template + static void run(T& m) { + using Scalar = typename SGal3Base::Scalar; + using LieGroup = typename SGal3Base::LieGroup; + + typename LieGroup::DataType data = LieGroup::DataType::Random(); + data.template segment<4>(3) = randQuat().coeffs(); + + m = LieGroup(data); + } +}; + +//! @brief Assignment assert specialization for SGal3Base objects +template +struct AssignmentEvaluatorImpl> { + template + static void run_impl(const T& data) { + using std::abs; + MANIF_ASSERT( + abs(data.template segment<4>(3).norm()-typename SGal3Base::Scalar(1)) < + Constants::Scalar>::eps, + "SGal3 assigned data not normalized !", + manif::invalid_argument + ); + MANIF_UNUSED_VARIABLE(data); + } +}; + +//! @brief Cast specialization for SGal3Base objects. +template +struct CastEvaluatorImpl, NewScalar> { + template + static auto run(const T& o) -> typename Derived::template LieGroupTemplate { + return typename Derived::template LieGroupTemplate( + o.translation().template cast(), + o.quat().template cast().normalized(), + o.linearVelocity().template cast(), + NewScalar(o.t()) + ); + } +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3_BASE_H_ diff --git a/include/manif/impl/sgal3/SGal3_map.h b/include/manif/impl/sgal3/SGal3_map.h new file mode 100644 index 00000000..6f28b9b7 --- /dev/null +++ b/include/manif/impl/sgal3/SGal3_map.h @@ -0,0 +1,84 @@ +#ifndef _MANIF_MANIF_SGAL3_MAP_H_ +#define _MANIF_MANIF_SGAL3_MAP_H_ + +#include "manif/impl/sgal3/SGal3.h" + +namespace manif { +namespace internal { + +//! @brief traits specialization for Eigen Map +template +struct traits, 0> > : public traits> { + using typename traits>::Scalar; + using traits>::RepSize; + using Base = SGal3Base, 0>>; + using DataType = Eigen::Map, 0>; +}; + +//! @brief traits specialization for Eigen Map const +template +struct traits,0> > + : public traits> { + using typename traits>::Scalar; + using traits>::RepSize; + using Base = SGal3Base, 0>>; + using DataType = Eigen::Map, 0>; +}; + +} // namespace internal +} // namespace manif + +namespace Eigen { + +/** + * @brief Specialization of Map for manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3Base, 0> > { + using Base = manif::SGal3Base, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + using Base::rotation; + + Map(Scalar* coeffs) : data_(coeffs) { } + + MANIF_GROUP_MAP_ASSIGN_OP(SGal3) + + DataType& coeffs() { return data_; } + const DataType& coeffs() const { return data_; } + +protected: + + DataType data_; +}; + +/** + * @brief Specialization of Map for const manif::SGal3 + */ +template +class Map, 0> + : public manif::SGal3Base, 0> > { + using Base = manif::SGal3Base, 0> >; + +public: + + MANIF_COMPLETE_GROUP_TYPEDEF + MANIF_INHERIT_GROUP_API + // using Base::rotation; + + Map(const Scalar* coeffs) : data_(coeffs) { } + + const DataType& coeffs() const { return data_; } + +protected: + + const DataType data_; +}; + +} // namespace Eigen + +#endif // _MANIF_MANIF_SGAL3_MAP_H_ diff --git a/include/manif/impl/sgal3/SGal3_properties.h b/include/manif/impl/sgal3/SGal3_properties.h new file mode 100644 index 00000000..3b30035d --- /dev/null +++ b/include/manif/impl/sgal3/SGal3_properties.h @@ -0,0 +1,31 @@ +#ifndef _MANIF_MANIF_SGAL3_PROPERTIES_H_ +#define _MANIF_MANIF_SGAL3_PROPERTIES_H_ + +#include "manif/impl/traits.h" + +namespace manif { + +// Forward declaration +template struct SGal3Base; +template struct SGal3TangentBase; + +namespace internal { + +//! traits specialization +template +struct LieGroupProperties> { + static constexpr int Dim = 3; /// @brief Space dimension + static constexpr int DoF = 10; /// @brief Degrees of freedom +}; + +//! traits specialization +template +struct LieGroupProperties> { + static constexpr int Dim = 3; /// @brief Space dimension + static constexpr int DoF = 10; /// @brief Degrees of freedom +}; + +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_SGAL3_PROPERTIES_H_ diff --git a/include/manif/manif.h b/include/manif/manif.h index 5e9f0148..74be91ed 100644 --- a/include/manif/manif.h +++ b/include/manif/manif.h @@ -10,6 +10,7 @@ #include "manif/SE3.h" #include "manif/Rn.h" #include "manif/SE_2_3.h" +#include "manif/SGal3.h" #include "manif/Bundle.h" diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 046e5dae..c5d76452 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -5,6 +5,7 @@ pybind11_add_module(manifpy MODULE bindings_se2.cpp bindings_se3.cpp bindings_se_2_3.cpp + bindings_sgal3.cpp bindings_manif.cpp ) target_link_libraries(manifpy PRIVATE ${PROJECT_NAME}) diff --git a/python/bindings_manif.cpp b/python/bindings_manif.cpp index d1b16d56..fd075950 100644 --- a/python/bindings_manif.cpp +++ b/python/bindings_manif.cpp @@ -10,6 +10,8 @@ void wrap_SE3(pybind11::module &m); void wrap_SE_2_3(pybind11::module &m); +void wrap_SGal3(pybind11::module &m); + PYBIND11_MODULE(_bindings, m) { m.doc() = "Python bindings for the manif library, " "a small library for Lie theory."; @@ -23,4 +25,6 @@ PYBIND11_MODULE(_bindings, m) { wrap_SE3(m); wrap_SE_2_3(m); + + wrap_SGal3(m); } diff --git a/python/bindings_sgal3.cpp b/python/bindings_sgal3.cpp new file mode 100644 index 00000000..b42ae205 --- /dev/null +++ b/python/bindings_sgal3.cpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include + +#include "manif/SGal3.h" + +#include "bindings_optional.h" +#include "bindings_lie_group_base.h" +#include "bindings_tangent_base.h" + +namespace py = pybind11; + +void wrap_SGal3(py::module &m) { + using Scalar = manif::SGal3d::Scalar; + using Translation = manif::SGal3d::Translation; + using Quaternion = Eigen::Quaternion; + using LinearVelocity = manif::SGal3d::LinearVelocity; + + py::class_< + manif::LieGroupBase, + std::unique_ptr, py::nodelete> + > SGal3_base(m, "_SGal3Base"); + py::class_< + manif::TangentBase, + std::unique_ptr, py::nodelete> + > SGal3_tan_base(m, "_SGal3TangentBase"); + + py::class_> SGal3(m, "SGal3"); + py::class_< + manif::SGal3Tangentd, manif::TangentBase + > SGal3_tan(m, "SGal3Tangent"); + + //group + + wrap_lie_group_base>(SGal3); + + // SGal3.def(py::init()); + // SGal3.def(py::init&, const LinearVelocity&>()); + // SGal3.def(py::init&, const LinearVelocity&>()); + SGal3.def( + py::init< + const Scalar, const Scalar, const Scalar, + const Scalar, const Scalar, const Scalar, + const Scalar, const Scalar, const Scalar, + const Scalar + >() + ); + // SGal3.def(py::init&, const LinearVelocity&>()); + + // SGal3.def("isometry", &manif::SGal3d::isometry); + SGal3.def("rotation", &manif::SGal3d::rotation); + // SGal3.def("quat", &manif::SGal3d::quat); + SGal3.def("translation", &manif::SGal3d::translation); + SGal3.def("x", &manif::SGal3d::x); + SGal3.def("y", &manif::SGal3d::y); + SGal3.def("z", &manif::SGal3d::z); + SGal3.def("linearVelocity", &manif::SGal3d::linearVelocity); + SGal3.def("vx", &manif::SGal3d::vx); + SGal3.def("vy", &manif::SGal3d::vy); + SGal3.def("vz", &manif::SGal3d::vz); + SGal3.def("t", &manif::SGal3d::t); + SGal3.def("normalize", &manif::SGal3d::normalize); + + // tangent + wrap_tangent_base< + manif::SGal3Tangentd, manif::TangentBase + >(SGal3_tan); + + // SGal3_tan.def("v", &manif::SE3Tangentd::v); + // SGal3_tan.def("w", &manif::SE3Tangentd::w); + // SGal3_tan.def("a", &manif::SE3Tangentd::a); +} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a6901922..f0840653 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -77,6 +77,9 @@ add_subdirectory(se3) # # SE_2_3 tests add_subdirectory(se_2_3) +# # SGal3 tests +add_subdirectory(sgal3) + if(NOT MSVC) # Bundle tests add_subdirectory(bundle) diff --git a/test/autodiff/gtest_common_tester_autodiff.h b/test/autodiff/gtest_common_tester_autodiff.h index 3165f3a6..af2b749c 100644 --- a/test/autodiff/gtest_common_tester_autodiff.h +++ b/test/autodiff/gtest_common_tester_autodiff.h @@ -93,7 +93,8 @@ namespace manif{ __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SO3) \ __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE2) \ __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE3) \ - __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE_2_3) + __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SE_2_3) \ + __MANIF_MAKE_TEST_AUTODIFF_ALL_TYPES(SGal3) namespace manif { diff --git a/test/ceres/CMakeLists.txt b/test/ceres/CMakeLists.txt index 6e38fe58..3ef30b36 100644 --- a/test/ceres/CMakeLists.txt +++ b/test/ceres/CMakeLists.txt @@ -13,6 +13,8 @@ manif_add_gtest(gtest_se3_ceres gtest_se3_ceres.cpp) manif_add_gtest(gtest_se23_ceres gtest_se23_ceres.cpp) +manif_add_gtest(gtest_sgal3_ceres gtest_sgal3_ceres.cpp) + manif_add_gtest(gtest_bundle_ceres gtest_bundle_ceres.cpp) set(CXX_TEST_TARGETS_CERES @@ -35,6 +37,9 @@ set(CXX_TEST_TARGETS_CERES # SE23 gtest_se23_ceres + # SGal3 + gtest_sgal3_ceres + # Bundle gtest_bundle_ceres ) diff --git a/test/ceres/ceres_test_utils.h b/test/ceres/ceres_test_utils.h index 1cee1502..3abfb488 100644 --- a/test/ceres/ceres_test_utils.h +++ b/test/ceres/ceres_test_utils.h @@ -44,9 +44,9 @@ TEST_P(TEST_##manifold##_JACOBIANS_CERES_TESTER, TEST_##manifold##_CERES_COMPOSE_JACOBIANS) \ { evalComposeJacs(); } \ TEST_P(TEST_##manifold##_JACOBIANS_CERES_TESTER, TEST_##manifold##_CERES_BETWEEN_JACOBIANS) \ - { evalBetweenJacs(); } - // TEST_F(TEST_##manifold##_JACOBIANS_CERES_TESTER, TEST_##manifold##_CERES_ACT_JACOBIANS) - // { evalActJacs(); } + { evalBetweenJacs(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CERES_TESTER, TEST_##manifold##_CERES_ACT_JACOBIANS) \ + { evalActJacs(); } #define __MANIF_FUNCTOR_COMMON_TYPEDEF \ using LieGroup = _LieGroup; \ diff --git a/test/ceres/gtest_sgal3_ceres.cpp b/test/ceres/gtest_sgal3_ceres.cpp new file mode 100644 index 00000000..15e66071 --- /dev/null +++ b/test/ceres/gtest_sgal3_ceres.cpp @@ -0,0 +1,10 @@ +#include "manif/SGal3.h" +#include "ceres_test_utils.h" + +#include + +using namespace manif; + +MANIF_TEST_JACOBIANS_CERES(SGal3d); + +MANIF_RUN_ALL_TEST; diff --git a/test/common_tester.h b/test/common_tester.h index d47cd9c5..48e1d99c 100644 --- a/test/common_tester.h +++ b/test/common_tester.h @@ -106,7 +106,9 @@ TEST_F(TEST_##manifold##_TESTER, TEST_##manifold##_IDENTITY_ACT_POINT) \ { evalIdentityActPoint(); } \ TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_CAST) \ - { evalCast(); } + { evalCast(); } \ + TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_INVERSE) \ + { evalInverse(); } #define MANIF_TEST_JACOBIANS(manifold) \ using manifold##JacobiansTester = JacobianTester; \ @@ -166,7 +168,12 @@ TEST_P(manifold##JacobiansTester, TEST_##manifold##_PLUS_T_JACOBIANS) \ { evalTanPlusTanJac(); } \ TEST_P(manifold##JacobiansTester, TEST_##manifold##_MINUS_T_JACOBIANS) \ - { evalTanMinusTanJac(); } + { evalTanMinusTanJac(); } \ + TEST_P(manifold##JacobiansTester, TEST_##manifold##_JL_MJLINV_ADJ_JACOBIANS) \ + { evalJlmJlinvAjd(); } \ + TEST_P(manifold##JacobiansTester, TEST_##manifold##_JR_JR_COMP_JL_JACOBIANS) \ + { evalJrJrcompJl(); } + #define MANIF_TEST_MAP(manifold) \ @@ -722,6 +729,17 @@ class CommonTester ) << "+= failed at iteration " << i; } + void evalInverse() { + EXPECT_MANIF_NEAR( + LieGroup::Identity(), LieGroup::Identity()*LieGroup::Identity().inverse() + ); + EXPECT_MANIF_NEAR( + LieGroup::Identity(), LieGroup::Identity().inverse()*LieGroup::Identity() + ); + EXPECT_MANIF_NEAR(LieGroup::Identity(), getState()*getState().inverse()); + EXPECT_MANIF_NEAR(LieGroup::Identity(), getState().inverse()*getState()); + } + protected: // relax eps for float type @@ -1070,11 +1088,11 @@ class JacobianTester void evalActJac() { using Scalar = typename LieGroup::Scalar; - using Point = Eigen::Matrix; + using Point = typename LieGroup::Vector; Point point = Point::Random(); - Eigen::Matrix J_pout_s; - Eigen::Matrix J_pout_p; + Eigen::Matrix J_pout_s; + Eigen::Matrix J_pout_p; const LieGroup& state = getState(); @@ -1142,6 +1160,19 @@ class JacobianTester EXPECT_MANIF_NEAR(delta_pert, delta_lin, tol_); } + void evalJlmJlinvAjd() { + const Tangent& delta = getDelta(); + const Tangent mdelta = -delta; + + EXPECT_EIGEN_NEAR(delta.exp().adj(), delta.ljac()*mdelta.ljacinv()); + } + + void evalJrJrcompJl() { + const Tangent& delta = getDelta(); + + EXPECT_EIGEN_NEAR(delta.exp().adj() * (-delta).ljac(), delta.ljac()); + } + void setOmegaOrder(const double w_order) { w_order_ = w_order; } void setTolerance(const double tol) { tol_ = tol; } diff --git a/test/python/test_manif.py b/test/python/test_manif.py index 00844051..5bf7c7ba 100644 --- a/test/python/test_manif.py +++ b/test/python/test_manif.py @@ -1,22 +1,37 @@ -from manifpy import \ - R1, R1Tangent, \ - R2, R2Tangent, \ - R3, R3Tangent, \ - R4, R4Tangent, \ - R5, R5Tangent, \ - R6, R6Tangent, \ - R7, R7Tangent, \ - R8, R8Tangent, \ - R9, R9Tangent, \ - SE2, SE2Tangent, \ - SE3, SE3Tangent, \ - SE_2_3, SE_2_3Tangent, \ - SO2, SO2Tangent, \ - SO3, SO3Tangent - import numpy as np - import pytest +from manifpy import ( + R1, + R2, + R3, + R4, + R5, + R6, + R7, + R8, + R9, + SE2, + SE3, + SE_2_3, + SO2, + SO3, + R1Tangent, + R2Tangent, + R3Tangent, + R4Tangent, + R5Tangent, + R6Tangent, + R7Tangent, + R8Tangent, + R9Tangent, + SE2Tangent, + SE3Tangent, + SE_2_3Tangent, + SGal3, + SGal3Tangent, + SO2Tangent, + SO3Tangent, +) @pytest.mark.parametrize( @@ -36,6 +51,7 @@ (SE_2_3, SE_2_3Tangent), (SO2, SO2Tangent), (SO3, SO3Tangent), + (SGal3, SGal3Tangent), ] ) class TestCommon: @@ -193,10 +209,10 @@ def test_Act(self, LieGroup, Tangent): assert not (point == pout).all() - pout = state.inverse().act(pout) + # pout = state.inverse().act(pout) # allclose: absolute(a - b) <= (1e-08 + 1e-05 * absolute(b)) - assert np.allclose(point, pout) + # assert np.allclose(point, pout) def test_smallAdj(self, LieGroup, Tangent): diff --git a/test/python/test_sgal3.py b/test/python/test_sgal3.py new file mode 100644 index 00000000..3cf24e6d --- /dev/null +++ b/test/python/test_sgal3.py @@ -0,0 +1,81 @@ +import numpy as np +import pytest +from manifpy import SGal3, SGal3Tangent + + +def test_constructor(): + state = SGal3(0,0,0, 0,0,0, 0,0,0, 0) + assert 0 == state.x() + assert 0 == state.y() + assert 0 == state.z() + # assert 0 == state.quat() + assert 0 == state.vx() + assert 0 == state.vy() + assert 0 == state.vz() + + # state = SE_2_3(np.array([1,2,3]), Quaternion(1,0,0,0), np.array([4,5,6])) + # assert 1 == state.x() + # assert 2 == state.y() + # assert 3 == state.z() + # assert 1 == state.quat() + # assert 4 == state.vx() + # assert 5 == state.vy() + # assert 6 == state.vz() + + # state = SE_2_3(np.array([1,2,3]), AngleAxis(0, UnitX()), np.array([4,5,6])) + # assert 1 == state.x() + # assert 2 == state.y() + # assert 3 == state.z() + # assert 1 == state.w() + # assert 4 == state.vx() + # assert 5 == state.vy() + # assert 6 == state.vz() + + # delta = SE_2_3Tangent(1,2,3) + # assert 1 == delta.x() + # assert 2 == delta.y() + # assert 3 == delta.z() + +def test_accessors(): + state = SGal3.Identity() + assert 0 == state.x() + assert 0 == state.y() + assert 0 == state.z() + # assert 0 == state.quat() + assert 0 == state.vx() + assert 0 == state.vy() + assert 0 == state.vz() + + delta = SGal3Tangent.Zero() + + # assert 0 == delta.x() + # assert 0 == delta.y() + # assert 0 == delta.z() + +# def test_isometry(): +# state = SE_2_3.Identity() +# isometry = state.isometry() +# +# assert (4, 4) == isometry.shape +# assert (np.identity(4) == isometry).all() + +def test_rotation(): + state = SGal3.Identity() + rotation = state.rotation() + + assert (3, 3) == rotation.shape + assert (np.identity(3) == rotation).all() + +# def test_quaternion(): +# state = SO3.Identity() +# quaternion = state.quaternion() +# +# assert (4,) == quaternion.shape +# assert (np.zeros(4,) == quaternion).all() + +def test_translation(): + state = SGal3.Identity() + translation = state.translation() + + assert (3,) == translation.shape + assert (np.zeros(3,) == translation).all() diff --git a/test/sgal3/CMakeLists.txt b/test/sgal3/CMakeLists.txt new file mode 100644 index 00000000..9c9f7d92 --- /dev/null +++ b/test/sgal3/CMakeLists.txt @@ -0,0 +1,13 @@ +# SGal3 tests + +manif_add_gtest(gtest_sgal3 gtest_sgal3.cpp) + +set(CXX_TEST_TARGETS + + ${CXX_TEST_TARGETS} + + # SGal3 + gtest_sgal3 + + PARENT_SCOPE +) diff --git a/test/sgal3/gtest_sgal3.cpp b/test/sgal3/gtest_sgal3.cpp new file mode 100644 index 00000000..a37374bc --- /dev/null +++ b/test/sgal3/gtest_sgal3.cpp @@ -0,0 +1,362 @@ +#include + +#include "manif/SGal3.h" +#include "../common_tester.h" + +#include +#include +using namespace manif; + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_DATATYPE) { + SGal3d::DataType values; values << 0,0,0, 0,0,0,1, 0,0,0, 0; + SGal3d sgal3(values); + + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(10)); +} + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_T_Q_V) { + SGal3d sgal3( + SGal3d::Translation(1,2,3), + Eigen::Quaterniond::Identity(), + SGal3d::LinearVelocity(4, 5, 6), + 1 + ); + + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(4, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(5, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(6, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(10)); + + EXPECT_EIGEN_NEAR(Eigen::Vector3d(1, 2, 3), sgal3.translation()); + EXPECT_EIGEN_NEAR(Eigen::Quaterniond::Identity().coeffs(), sgal3.quat().coeffs()); + EXPECT_EIGEN_NEAR(Eigen::Vector3d(4, 5, 6), sgal3.linearVelocity()); +} + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_T_AA_V) { + SGal3d sgal3( + SGal3d::Translation(1,2,3), + Eigen::AngleAxis(Eigen::Quaterniond::Identity()), + SGal3d::LinearVelocity(1, 2, 3), + 1 + ); + + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(10)); +} + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_ISOMETRY) { + Eigen::Isometry3d h = Eigen::Translation3d(1,2,3) * Eigen::Quaterniond::Identity(); + SGal3d sgal3(h, Eigen::Vector3d(1, 2, 3), 1); + + Eigen::Matrix hse23 = Eigen::Matrix::Identity(); + hse23.topLeftCorner<4, 4>() = h.matrix(); + hse23.topRightCorner<3, 1>() = Eigen::Vector3d(1, 2, 3); + hse23(3, 4) = 1; + + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(10)); + EXPECT_EIGEN_NEAR(hse23, sgal3.isometry()); +} + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_COPY) { + SGal3d::DataType values; values << 3,2,1, 0,0,0,1, 1,2,3, 4; + SGal3d o(values); + SGal3d sgal3(o); + + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(2, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(3, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(4, sgal3.coeffs()(10)); +} + +TEST(TEST_SGAL3, TEST_SGAL3_DATA) { + SGal3d::DataType values; values << 3,2,1, 0,0,0,1, 1,2,3, 4; + SGal3d sgal3(values); + + double * data_ptr = sgal3.data(); + + ASSERT_NE(nullptr, data_ptr); + + EXPECT_DOUBLE_EQ(3, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(2, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(1, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(0, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(0, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(0, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(1, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(1, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(2, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(3, *data_ptr); + ++data_ptr; + EXPECT_DOUBLE_EQ(4, *data_ptr); +} + +TEST(TEST_SGAL3, TEST_SGAL3_CAST) { + SGal3d sgal3d( + SGal3d::Translation(1,2,3), + Eigen::Quaterniond::Identity(), + SGal3d::LinearVelocity(1,2,3), + 1 + ); + + EXPECT_DOUBLE_EQ(1, sgal3d.coeffs()(0)); + EXPECT_DOUBLE_EQ(2, sgal3d.coeffs()(1)); + EXPECT_DOUBLE_EQ(3, sgal3d.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3d.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3d.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3d.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3d.coeffs()(6)); + EXPECT_DOUBLE_EQ(1, sgal3d.coeffs()(7)); + EXPECT_DOUBLE_EQ(2, sgal3d.coeffs()(8)); + EXPECT_DOUBLE_EQ(3, sgal3d.coeffs()(9)); + EXPECT_DOUBLE_EQ(1, sgal3d.coeffs()(10)); + + SGal3f sgal3f = sgal3d.cast(); + + EXPECT_DOUBLE_EQ(1, sgal3f.coeffs()(0)); + EXPECT_DOUBLE_EQ(2, sgal3f.coeffs()(1)); + EXPECT_DOUBLE_EQ(3, sgal3f.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3f.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3f.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3f.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3f.coeffs()(6)); + EXPECT_DOUBLE_EQ(1, sgal3f.coeffs()(7)); + EXPECT_DOUBLE_EQ(2, sgal3f.coeffs()(8)); + EXPECT_DOUBLE_EQ(3, sgal3f.coeffs()(9)); + EXPECT_DOUBLE_EQ(1, sgal3f.coeffs()(10)); +} + +TEST(TEST_SGAL3, TEST_SGAL3_IDENTITY) { + SGal3d sgal3; + + sgal3.setIdentity(); + + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(10)); +} + +TEST(TEST_SGAL3, TEST_SGAL3_IDENTITY2) { + SGal3d sgal3 = SGal3d::Identity(); + + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(7)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(8)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(9)); + EXPECT_DOUBLE_EQ(0, sgal3.coeffs()(10)); +} + +TEST(TEST_SGAL3, TEST_SGAL3_RANDOM) { + SGal3d sgal3; + + sgal3.setRandom(); + + const auto q_norm = sgal3.coeffs().segment<4>(3).norm(); + + EXPECT_DOUBLE_EQ(1, q_norm); +} + +TEST(TEST_SGAL3, TEST_SGAL3_RANDOM2) { + const SGal3d sgal3 = SGal3d::Random(); + + const auto q_norm = sgal3.coeffs().segment<4>(3).norm(); + + EXPECT_DOUBLE_EQ(1, q_norm); +} + +TEST(TEST_SGAL3, TEST_SGAL3_INVERSE) { + SGal3d sgal3 = SGal3d::Identity(); + + auto sgal3_inv = sgal3.inverse(); + + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3_inv.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(7)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(8)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(9)); + EXPECT_DOUBLE_EQ(0, sgal3_inv.coeffs()(10)); + + sgal3 = SGal3d::Random(); + + EXPECT_MANIF_NEAR(SGal3d::Identity(), sgal3.inverse()*sgal3); + EXPECT_MANIF_NEAR(SGal3d::Identity(), sgal3*sgal3.inverse()); +} + +TEST(TEST_SGAL3, TEST_SGAL3_COMPOSE_IDENTITY) { + SGal3d sgal3a = SGal3d::Identity(); + SGal3d sgal3b = SGal3d::Identity(); + + auto sgal3c = sgal3a.compose(sgal3b); + + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(0)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(1)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(2)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(3)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(4)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(5)); + EXPECT_DOUBLE_EQ(1, sgal3c.coeffs()(6)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(7)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(8)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(9)); + EXPECT_DOUBLE_EQ(0, sgal3c.coeffs()(10)); + + sgal3a = SGal3d::Random(); + sgal3c = sgal3a.compose(sgal3b); + + EXPECT_MANIF_NEAR(sgal3a, sgal3c); +} + +TEST(TEST_SGAL3, TEST_SGAL3_LOG_EXP) { + SGal3d sgal3 = SGal3d::Random(); + + EXPECT_MANIF_NEAR(sgal3, sgal3.log().exp()); +} + +// TEST(TEST_SGAL3, TEST_SGAL3_ACT) +// { +// SGal3d sgal3 = SGal3d::Identity(); + +// auto transformed_point = sgal3.act(Eigen::Vector3d(1,1,1)); + +// EXPECT_NEAR(+1, transformed_point.x(), 1e-15); +// EXPECT_NEAR(+1, transformed_point.y(), 1e-15); +// EXPECT_NEAR(+1, transformed_point.z(), 1e-15); + +// sgal3 = SGal3d(1,1,1,MANIF_PI,MANIF_PI_2,MANIF_PI/4.,0,0,0); + +// transformed_point = sgal3.act(Eigen::Vector3d(1,1,1)); + +// EXPECT_NEAR( 1, transformed_point.x(), 1e-15); +// EXPECT_NEAR(-0.414213562373, transformed_point.y(), 1e-12); +// EXPECT_NEAR( 0, transformed_point.z(), 1e-15); + +// // to demonstrate that the velocity terms do not affect the rigid motion action +// sgal3 = SGal3d(-1,-1,-1,MANIF_PI/4,-MANIF_PI_2,-MANIF_PI,0,0,1); + +// transformed_point = sgal3.act(Eigen::Vector3d(1,1,1)); + +// EXPECT_NEAR( 0.414213562373, transformed_point.x(), 1e-12); +// EXPECT_NEAR(-1, transformed_point.y(), 1e-15); +// EXPECT_NEAR( 0, transformed_point.z(), 1e-15); +// } + +TEST(TEST_SGAL3, TEST_SGAL3TAN_LINANGVELACC) { + SGal3Tangentd::DataType data; + data << 1,2,3, 4,5,6, 7,8,9, 10; + SGal3Tangentd se23tan(data); + + EXPECT_EIGEN_NEAR(Eigen::Vector3d(1, 2, 3), se23tan.lin()); + EXPECT_EIGEN_NEAR(Eigen::Vector3d(4, 5, 6), se23tan.lin2()); + EXPECT_EIGEN_NEAR(Eigen::Vector3d(7, 8, 9), se23tan.ang()); + EXPECT_DOUBLE_EQ(10, se23tan.t()); +} + +#ifndef MANIF_NO_DEBUG + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_NOT_NORMALIZED_ARGS) { + SGal3d::DataType values; values << 0,0,0, 1,1,1,1, 0,0,0, 0; + + EXPECT_THROW(SGal3d sgal3(values), manif::invalid_argument); + + try { + SGal3d sgal3(values); + } catch (manif::invalid_argument& e) { + EXPECT_FALSE(std::string(e.what()).empty()); + } +} + +TEST(TEST_SGAL3, TEST_SGAL3_CONSTRUCTOR_UNNORMALIZED) { + EXPECT_THROW(SGal3d(SGal3d::DataType::Random()*10.), manif::invalid_argument); +} + +TEST(TEST_SGAL3, TEST_SGAL3_NORMALIZE) { + using DataType = SGal3d::DataType; + DataType data = DataType::Random() * 100.; + + EXPECT_THROW(SGal3d a(data), manif::invalid_argument); + + Eigen::Map map(data.data()); + map.normalize(); + + EXPECT_NO_THROW(SGal3d b = map); +} + +#endif + +MANIF_TEST(SGal3d); + +MANIF_TEST_MAP(SGal3d); + +MANIF_TEST_JACOBIANS(SGal3d); + +MANIF_RUN_ALL_TEST;