diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 05ad1b2badf2c3b7541a86767f0afb7c611b6f04..7a1cd418d91cb5ec783f72e320afb95af2977d60 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -138,7 +138,7 @@ inline void inverse(const MatrixBase<D1>& d, MatrixSizeCheck<11, 1>::check(id); typedef typename D1::Scalar T1; - typedef typename D1::Scalar T2; + typedef typename D2::Scalar T2; Map<const Matrix<T1, 3, 1> > dp ( & d( 0 ) ); Map<const Quaternion<T1> > dq ( & d( 3 ) ); @@ -173,9 +173,9 @@ inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, co MatrixSizeCheck<3, 1>::check(sum_p); MatrixSizeCheck<3, 1>::check(sum_v); - sum_p = dp1 + dv1*dt2 + dq1*dp2; - sum_v = dv1 + dq1*dv2; - sum_q = dq1*dq2; + sum_p = dp1 + dv1*dt2 + dq1*dp2; + sum_v = dv1 + dq1*dv2; + sum_q = dq1*dq2; sum_dt = dt1 + dt2; } @@ -188,18 +188,22 @@ inline void compose(const MatrixBase<D1>& delta1, MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<11, 1>::check(sum); - Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1( 0 ) ); - Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1( 3 ) ); - Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1( 7 ) ); - const typename D1::Scalar& dt1 = delta1(10); - Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2( 0 ) ); - Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2( 3 ) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2( 7 ) ); - const typename D2::Scalar& dt2 = delta2(10); - Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) ); - Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) ); - Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( & sum( 7 ) ); - typename D3::Scalar& sum_dt = sum(10); + typedef typename D1::Scalar T1; + typedef typename D2::Scalar T2; + typedef typename D3::Scalar T3; + + Map<const Matrix<T1, 3, 1> > dp1 ( & delta1( 0 ) ); + Map<const Quaternion<T1> > dq1 ( & delta1( 3 ) ); + Map<const Matrix<T1, 3, 1> > dv1 ( & delta1( 7 ) ); + const T1& dt1 = delta1(10 ) ; + Map<const Matrix<T2, 3, 1> > dp2 ( & delta2( 0 ) ); + Map<const Quaternion<T2> > dq2 ( & delta2( 3 ) ); + Map<const Matrix<T2, 3, 1> > dv2 ( & delta2( 7 ) ); + const T2& dt2 = delta2(10 ) ; + Map<Matrix<T3, 3, 1> > sum_p ( & sum( 0 ) ); + Map<Quaternion<T3> > sum_q ( & sum( 3 ) ); + Map<Matrix<T3, 3, 1> > sum_v ( & sum( 7 ) ); + T3& sum_dt = sum(10 ) ; compose(dp1, dq1, dv1, dt1, dp2, dq2, dv2, dt2, sum_p, sum_q, sum_v, sum_dt); } @@ -265,18 +269,22 @@ inline void between(const MatrixBase<D1>& delta1, MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<11, 1>::check(delta2_minus_delta1); - Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1(0) ); - Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1(3) ); - Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1(7) ); - const typename D1::Scalar& dt1 = delta1(10); - Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2(0) ); - Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2(3) ); - Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2(7) ); - const typename D2::Scalar& dt2 = delta2(10); - Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & delta2_minus_delta1(0) ); - Map<Quaternion<typename D3::Scalar> > diff_q ( & delta2_minus_delta1(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & delta2_minus_delta1(7) ); - typename D3::Scalar& diff_dt = delta2_minus_delta1(10); + typedef typename D1::Scalar T1; + typedef typename D2::Scalar T2; + typedef typename D3::Scalar T3; + + Map<const Matrix<T1, 3, 1> > dp1 ( & delta1(0) ); + Map<const Quaternion<T1> > dq1 ( & delta1(3) ); + Map<const Matrix<T1, 3, 1> > dv1 ( & delta1(7) ); + const T1& dt1 = delta1(10) ; + Map<const Matrix<T2, 3, 1> > dp2 ( & delta2(0) ); + Map<const Quaternion<T2> > dq2 ( & delta2(3) ); + Map<const Matrix<T2, 3, 1> > dv2 ( & delta2(7) ); + const T2& dt2 = delta2(10) ; + Map<Matrix<T3, 3, 1> > diff_p ( & delta2_minus_delta1(0) ); + Map<Quaternion<T3> > diff_q ( & delta2_minus_delta1(3) ); + Map<Matrix<T3, 3, 1> > diff_v ( & delta2_minus_delta1(7) ); + T3& diff_dt = delta2_minus_delta1(10) ; between(dp1, dq1, dv1, dt1, dp2, dq2, dv2, dt2, diff_p, diff_q, diff_v, diff_dt); }