From 977de797f6faefa6596da7838663df6bbdc83bc3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Wed, 9 Oct 2019 20:49:50 +0200 Subject: [PATCH] more typedefs --- include/IMU/math/IMU_tools_Lie.h | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 7dad22964..34c448e68 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -438,19 +438,22 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in) { MatrixSizeCheck<10, 1>::check(d_in); - Matrix<typename Derived::Scalar, 11, 1> ret; + typedef typename Derived::Scalar T; - Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) ); - Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) ); - Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & d_in(6) ); - const typename Derived::Scalar& dt_in = d_in(9); - Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) ); - Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) ); - Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) ); - typename Derived::Scalar& dt = ret(10); - Matrix<typename Derived::Scalar, 3, 3> Q; - Matrix<typename Derived::Scalar, 3, 3> P; + Matrix<T, 11, 1> ret; + + Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) ); + Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) ); + Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) ); + const T& dt_in = d_in(9); + Map<Matrix<T, 3, 1> > dp ( & ret(0) ); + Map<Quaternion<T> > dq ( & ret(3) ); + Map<Matrix<T, 3, 1> > dv ( & ret(7) ); + T& dt = ret(10); + + Matrix<T, 3, 3> Q; + Matrix<T, 3, 3> P; QandPmat(do_in, Q, P); dp = Q*dp_in + P*dv_in*dt_in; -- GitLab