From d14f4cf2c1f3d06f08cdab15e3bfd28c68404192 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Thu, 24 Oct 2019 15:53:05 +0200 Subject: [PATCH] Add small adjoint, hat, vee --- include/IMU/math/IMU_tools_Lie.h | 44 +++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 0bf90615a..72b3e5236 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -215,13 +215,51 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){ } -template<typename D1, typename D2> -inline Matrix<T, 10, 10> smalladjoint(const MatrixBase<D1> tau) +template<typename D> +inline Matrix<typename D::Scalar, 5, 5> hat(const MatrixBase<D> tau) { MatrixSizeCheck<10, 1>::check(tau); - typedef typename D1::Scalar T; + typedef typename D::Scalar T; + + + Map<const Matrix<T, 3, 1> > p ( & tau(0) ); + Map<const Matrix<T, 3, 1> > th ( & tau(3) ); + Map<const Matrix<T, 3, 1> > v ( & tau(6) ); + const T& t = tau(9); + + Matrix<T, 5, 5> taux; + taux.setZero(); + + taux.block<3,3>(0,0) = skew(th); + taux.block<3,1>(0,3) = v; + taux.block<3,1>(0,4) = p; + taux.block<3,1>(3,4) = t; + + return taux; +} + +template<typename D> +inline Matrix<typename D::Scalar, 10, 1> vee(const MatrixBase<D> taux) +{ + MatrixSizeCheck<5, 5>::check(taux); + + Matrix<typename D::Scalar, 5, 5> tau; + + tau.segment<3>(0) = taux.block<3,1>(0,4); // p + tau.segment<3>(3) = vee(taux.block<3,3>(0,0)); // th + tau.segment<3>(6) = taux.block<3,1>(0,3); // v + tau (9) = taux (3,4); // t + + return tau; +} + +template<typename D> +inline Matrix<typename D::Scalar, 10, 10> smalladjoint(const MatrixBase<D> tau) +{ + MatrixSizeCheck<10, 1>::check(tau); + typedef typename D::Scalar T; Map<const Matrix<T, 3, 1> > p ( & tau(0) ); Map<const Matrix<T, 3, 1> > th ( & tau(3) ); -- GitLab