Skip to content
Snippets Groups Projects
Commit bb298ecf authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Merge branch 'imu_preintegration_Lie' of... 

Merge branch 'imu_preintegration_Lie' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/imu into imu_preintegration_Lie
parents fb57fa2c 458a2678
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/IMU/math/IMU_tools_Lie.h
+ 36
− 28
View file @ bb298ecf
...@@ -138,7 +138,7 @@ inline void inverse(const MatrixBase<D1>& d, ...@@ -138,7 +138,7 @@ inline void inverse(const MatrixBase<D1>& d,
MatrixSizeCheck<11, 1>::check(id); MatrixSizeCheck<11, 1>::check(id);
typedef typename D1::Scalar T1; 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 Matrix<T1, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<T1> > dq ( & d( 3 ) ); Map<const Quaternion<T1> > dq ( & d( 3 ) );
...@@ -173,9 +173,9 @@ inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, co ...@@ -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_p);
MatrixSizeCheck<3, 1>::check(sum_v); MatrixSizeCheck<3, 1>::check(sum_v);
sum_p = dp1 + dv1*dt2 + dq1*dp2; sum_p = dp1 + dv1*dt2 + dq1*dp2;
sum_v = dv1 + dq1*dv2; sum_v = dv1 + dq1*dv2;
sum_q = dq1*dq2; sum_q = dq1*dq2;
sum_dt = dt1 + dt2; sum_dt = dt1 + dt2;
} }
...@@ -188,18 +188,22 @@ inline void compose(const MatrixBase<D1>& delta1, ...@@ -188,18 +188,22 @@ inline void compose(const MatrixBase<D1>& delta1,
MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<11, 1>::check(delta2);
MatrixSizeCheck<11, 1>::check(sum); MatrixSizeCheck<11, 1>::check(sum);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1( 0 ) ); typedef typename D1::Scalar T1;
Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1( 3 ) ); typedef typename D2::Scalar T2;
Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1( 7 ) ); typedef typename D3::Scalar T3;
const typename D1::Scalar& dt1 = delta1(10);
Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2( 0 ) ); Map<const Matrix<T1, 3, 1> > dp1 ( & delta1( 0 ) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2( 3 ) ); Map<const Quaternion<T1> > dq1 ( & delta1( 3 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2( 7 ) ); Map<const Matrix<T1, 3, 1> > dv1 ( & delta1( 7 ) );
const typename D2::Scalar& dt2 = delta2(10); const T1& dt1 = delta1(10 ) ;
Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) ); Map<const Matrix<T2, 3, 1> > dp2 ( & delta2( 0 ) );
Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) ); Map<const Quaternion<T2> > dq2 ( & delta2( 3 ) );
Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( & sum( 7 ) ); Map<const Matrix<T2, 3, 1> > dv2 ( & delta2( 7 ) );
typename D3::Scalar& sum_dt = sum(10); 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); 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, ...@@ -265,18 +269,22 @@ inline void between(const MatrixBase<D1>& delta1,
MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<11, 1>::check(delta2);
MatrixSizeCheck<11, 1>::check(delta2_minus_delta1); MatrixSizeCheck<11, 1>::check(delta2_minus_delta1);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1(0) ); typedef typename D1::Scalar T1;
Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1(3) ); typedef typename D2::Scalar T2;
Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1(7) ); typedef typename D3::Scalar T3;
const typename D1::Scalar& dt1 = delta1(10);
Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2(0) ); Map<const Matrix<T1, 3, 1> > dp1 ( & delta1(0) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2(3) ); Map<const Quaternion<T1> > dq1 ( & delta1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2(7) ); Map<const Matrix<T1, 3, 1> > dv1 ( & delta1(7) );
const typename D2::Scalar& dt2 = delta2(10); const T1& dt1 = delta1(10) ;
Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & delta2_minus_delta1(0) ); Map<const Matrix<T2, 3, 1> > dp2 ( & delta2(0) );
Map<Quaternion<typename D3::Scalar> > diff_q ( & delta2_minus_delta1(3) ); Map<const Quaternion<T2> > dq2 ( & delta2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & delta2_minus_delta1(7) ); Map<const Matrix<T2, 3, 1> > dv2 ( & delta2(7) );
typename D3::Scalar& diff_dt = delta2_minus_delta1(10); 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); between(dp1, dq1, dv1, dt1, dp2, dq2, dv2, dt2, diff_p, diff_q, diff_v, diff_dt);
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment