Skip to content
Snippets Groups Projects
Commit 977de797 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

more typedefs

parent d5b4a0bc
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment