diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 7dad22964556e53e0c7cccb5381ce9a98519d73a..34c448e682c0ba896f1db14b6f7ed0729ddf9fea 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;