From 5929a65628bd8593ee323d67bf32ee22b340a991 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:29:14 +0200
Subject: [PATCH] more typedefs
---
include/IMU/math/IMU_tools_Lie.h | 56 +++++++++++++++++++-------------
1 file changed, 33 insertions(+), 23 deletions(-)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 9799a0101..064606493 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -308,16 +308,20 @@ inline void composeOverState(const MatrixBase<D1>& x,
MatrixSizeCheck<11, 1>::check(d);
MatrixSizeCheck<10, 1>::check(x_plus_d);
- Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & x( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > q ( & x( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > v ( & x( 7 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq ( & d( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( & d( 7 ) );
- const typename D2::Scalar& dt = d(10);
- Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
- Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
+ typedef typename D1::Scalar T1;
+ typedef typename D2::Scalar T2;
+ typedef typename D3::Scalar T3;
+
+ Map<const Matrix<T1, 3, 1> > p ( & x( 0 ) );
+ Map<const Quaternion<T1> > q ( & x( 3 ) );
+ Map<const Matrix<T1, 3, 1> > v ( & x( 7 ) );
+ Map<const Matrix<T2, 3, 1> > dp ( & d( 0 ) );
+ Map<const Quaternion<T2> > dq ( & d( 3 ) );
+ Map<const Matrix<T2, 3, 1> > dv ( & d( 7 ) );
+ const typename D2::Scalar& dt = d(10);
+ Map<Matrix<T3, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
+ Map<Quaternion<T3> > q_plus_d ( & x_plus_d( 3 ) );
+ Map<Matrix<T3, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
v_plus_d = v + gravity()*dt + q*dv;
@@ -363,16 +367,20 @@ inline void betweenStates(const MatrixBase<D1>& x1,
MatrixSizeCheck<10, 1>::check(x2);
MatrixSizeCheck<11, 1>::check(x2_minus_x1);
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & x1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( & x1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & x2(0) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( & x2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & x2_minus_x1(0) );
- Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & x2_minus_x1(7) );
- T& Dt = x2_minus_x1(10);
+ typedef typename D1::Scalar T1;
+ typedef typename D2::Scalar T2;
+ typedef typename D3::Scalar T3;
+
+ Map<const Matrix<T1, 3, 1> > p1 ( & x1(0) );
+ Map<const Quaternion<T1> > q1 ( & x1(3) );
+ Map<const Matrix<T1, 3, 1> > v1 ( & x1(7) );
+ Map<const Matrix<T2, 3, 1> > p2 ( & x2(0) );
+ Map<const Quaternion<T2> > q2 ( & x2(3) );
+ Map<const Matrix<T2, 3, 1> > v2 ( & x2(7) );
+ Map<Matrix<T3, 3, 1> > dp ( & x2_minus_x1(0) );
+ Map<Quaternion<T3> > dq ( & x2_minus_x1(3) );
+ Map<Matrix<T3, 3, 1> > dv ( & x2_minus_x1(7) );
+ T& Dt = x2_minus_x1(10);
betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv, Dt);
}
@@ -403,10 +411,12 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
P *= 0.5;
}
else{
- Matrix<typename D1::Scalar, 3, 3> Id;
+ typedef typename D1::Scalar T;
+
+ Matrix<T, 3, 3> Id;
Id.setIdentity();
- Matrix<typename D1::Scalar, 3, 1> u = th / thn;
- Matrix<typename D1::Scalar, 3, 3> ux = skew(u);
+ Matrix<T, 3, 1> u = th / thn;
+ Matrix<T, 3, 3> ux = skew(u);
Q = Id
+ ((1 - cos(thn))/thn)*ux
--
GitLab