diff --git a/CMakeLists.txt b/CMakeLists.txt
index 67ef6f0b03b6d7d9c8638a38439607ddb35b09d9..b11c6f81823fd082020c9fe0fa0c37d3ab3c3884 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -129,6 +129,9 @@ SET(HDRS_COMMON
SET(HDRS_MATH
include/IMU/math/IMU_tools.h
)
+SET(HDRS_MATH
+ include/IMU/math/IMU_tools_Lie.h
+ )
SET(HDRS_UTILS
)
SET(HDRS_CAPTURE
@@ -160,6 +163,9 @@ SET(SRCS_COMMON
SET(SRCS_MATH
include/IMU/math/IMU_tools.h
)
+SET(SRCS_MATH
+ include/IMU/math/IMU_tools_Lie.h
+ )
SET(SRCS_UTILS
)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6abf56578104e405775db1612c148aaa0d04c6b
--- /dev/null
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -0,0 +1,702 @@
+/*
+ * imu_tools_Lie.h
+ *
+ * Created on: Oct 7, 2019
+ * Author: mfourmy
+ */
+
+#ifndef IMU_TOOLS_LIE_H_
+#define IMU_TOOLS_LIE_H_
+
+#include "core/common/wolf.h"
+#include "core/math/rotations.h"
+
+/*
+ * Most functions in this file are explained in the document:
+ *
+ * Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
+ *
+ * They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
+ *
+ * The Delta is defined as
+ * Delta = [Dp, Dq, Dv]
+ * with
+ * Dp : position delta
+ * Dq : quaternion delta
+ * Dv : velocity delta
+ *
+ * They are listed below:
+ *
+ * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], Dv = [0,0,0]
+ * - inverse: so that D (+) D.inv = I
+ * - compose: Dc = D1 (+) D2
+ * - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
+ * - composeOverState: x2 = x1 (+) D
+ * - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
+ * - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 (+) exp_IMU(d)
+ * - diff: d = log_IMU( D2 (-) D1 )
+ * - body2delta: construct a delta from body magnitudes of linAcc and angVel
+ */
+
+namespace wolf
+{
+namespace imu_with_dt {
+using namespace Eigen;
+
+template<typename D1, typename D2, typename D3, typename T>
+inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v, T& dt)
+{
+ // Same as imu_tools.h with dt added to the state
+ p = MatrixBase<D1>::Zero(3,1);
+ q = QuaternionBase<D2>::Identity();
+ v = MatrixBase<D3>::Zero(3,1);
+ dt = T(0);
+}
+
+template<typename D1, typename D2, typename D3, typename T>
+inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q, MatrixBase<D3>& v, T& dt)
+{
+ typedef typename D1::Scalar T1;
+ typedef typename D2::Scalar T2;
+ typedef typename D3::Scalar T3;
+ p << T1(0), T1(0), T1(0);
+ q << T2(0), T2(0), T2(0), T2(1);
+ v << T3(0), T3(0), T3(0);
+ dt = T(0);
+}
+
+template<typename T = Scalar>
+inline Matrix<T, 11, 1> identity()
+{
+ Matrix<T, 11, 1> ret;
+ ret<< T(0), T(0), T(0),
+ T(0), T(0), T(0), T(1),
+ T(0), T(0), T(0),
+ T(0);
+ return ret;
+}
+
+template<typename D1, typename D2>
+inline void adjoint(const MatrixBase<D1> d, MatrixBase<D1> adjd)
+{
+ // Adjoint matrix associated to the adjoint operator
+ Matrix<typename D1::Scalar, 3, 1> dp = d.segment<3>(0);
+ Matrix<typename D1::Scalar, 3, 3> dR = q2R(d.segment<4>(3));
+ Matrix<typename D1::Scalar, 3, 1> dv = d.segment<3>(7);
+ typename D1::Scalar dt = d(10);
+
+ adjd.setIdentity();
+ adjd<3,3>(0,0) = dR;
+ adjd<3,3>(0,3) = skew(dp - dv * dt) * dR;
+ adjd<3,3>(0,6) = -dR * dt;
+ adjd<3,1>(0,9) = dv;
+ adjd<3,3>(3,3) = dR;
+ adjd<3,3>(6,3) = skew(dv) * dR;
+ adjd<3,3>(6,6) = dR;
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D> d){
+ Matrix<typename D::Scalar, 10, 10> adjd;
+ adjoint(d, adjd);
+ return adjd;
+}
+
+
+// template<typename D1, typename D2>
+// inline Matrix<T, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
+// {
+// // Adjoint matrix associated to the adjoint operator
+ // TODO
+
+// return ret;
+// }
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename T>
+inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, const MatrixBase<D3>& dv, const T& dt,
+ MatrixBase<D4>& idp, QuaternionBase<D5>& idq, MatrixBase<D6>& idv, T& idt)
+{
+ // Same as imu_tools.h
+ MatrixSizeCheck<3, 1>::check(dp);
+ MatrixSizeCheck<3, 1>::check(dv);
+ MatrixSizeCheck<3, 1>::check(idp);
+ MatrixSizeCheck<3, 1>::check(idv);
+
+ idp = - ( dq.conjugate() * (dp - dv * typename D3::Scalar(dt) ) );
+ idq = dq.conjugate();
+ idv = - ( dq.conjugate() * dv );
+ idt = - dt;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+ MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<11, 1>::check(d);
+ MatrixSizeCheck<11, 1>::check(id);
+
+ typedef typename D1::Scalar T;
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 7 ) );
+ const T& dt = d( 10 ) ;
+ Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
+ Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
+ Map<Matrix<typename D2::Scalar, 3, 1> > idv ( & id( 7 ) );
+ T& idt = id( 10 ) ;
+
+ inverse(dp, dq, dv, dt, idp, idq, idv, idt);
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 11, 1> inverse(const MatrixBase<D>& d)
+{
+ Matrix<typename D::Scalar, 11, 1> id;
+ inverse(d, id);
+ return id;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename T>
+inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1, const T& dt1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2, const T& dt2,
+ MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q, MatrixBase<D9>& sum_v, T& sum_dt)
+{
+ // Same as imu_tools.h
+ MatrixSizeCheck<3, 1>::check(dp1);
+ MatrixSizeCheck<3, 1>::check(dv1);
+ MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(dv2);
+ 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_dt = dt1 + dt2;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& sum)
+{
+ MatrixSizeCheck<11, 1>::check(d1);
+ MatrixSizeCheck<11, 1>::check(d2);
+ MatrixSizeCheck<11, 1>::check(sum);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1( 7 ) );
+ const typename D1::Scalar& dt1 = d1(10);
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2( 7 ) );
+ const typename D2::Scalar& dt2 = d2(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);
+
+ compose(dp1, dq1, dv1, dt1, dp2, dq2, dv2, dt2, sum_p, sum_q, sum_v, sum_dt);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 11, 1> ret;
+ compose(d1, d2, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
+{
+ // Same as imu_tools.h
+ MatrixSizeCheck<11, 1>::check(d1);
+ MatrixSizeCheck<11, 1>::check(d2);
+ MatrixSizeCheck<11, 1>::check(sum);
+ MatrixSizeCheck< 10, 10>::check(J_sum_d1);
+ MatrixSizeCheck< 10, 10>::check(J_sum_d2);
+
+ // Jac wrt first delta -> adjoint inverse
+ J_sum_d1 = adjoint(inverse(d1));
+
+ // Jac wrt second delta -> indentity
+ J_sum_d2.setIdentity(); //
+
+ // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
+ compose(d1, d2, sum);
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename T>
+inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1, const T dt1,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2, const T dt2,
+ MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q, MatrixBase<D9>& diff_v, T& diff_dt )
+{
+ // Different from imu_tools.h
+ MatrixSizeCheck<3, 1>::check(dp1);
+ MatrixSizeCheck<3, 1>::check(dv1);
+ MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(dv2);
+ MatrixSizeCheck<3, 1>::check(diff_p);
+ MatrixSizeCheck<3, 1>::check(diff_v);
+
+ diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*(dt2 - dt1)); // very different!
+ diff_v = dq1.conjugate() * ( dv2 - dv1 );
+ diff_q = dq1.conjugate() * dq2;
+ diff_dt = dt2 - dt1;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d2_minus_d1)
+{
+ MatrixSizeCheck<11, 1>::check(d1);
+ MatrixSizeCheck<11, 1>::check(d2);
+ MatrixSizeCheck<11, 1>::check(d2_minus_d1);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+ const typename D1::Scalar& dt1 = d1(10);
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
+ const typename D2::Scalar& dt2 = d2(10);
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & d2_minus_d1(0) );
+ Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & d2_minus_d1(7) );
+ typename D3::Scalar& diff_dt = d2_minus_d1(10);
+
+ between(dp1, dq1, dv1, dt1, dp2, dq2, dv2, dt2, diff_p, diff_q, diff_v, diff_dt);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 11, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 11, 1> diff;
+ between(d1, d2, diff);
+ return diff;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ MatrixBase<D3>& x_plus_d)
+{
+ // Same as imu_tools.h
+ MatrixSizeCheck<10, 1>::check(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 ) );
+
+ p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
+ v_plus_d = v + gravity()*dt + q*dv;
+ q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_plus_d
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d)
+{
+ Matrix<typename D1::Scalar, 10, 1> ret;
+ composeOverState(x, d, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
+inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D3>& v1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, const MatrixBase<D6>& v2,
+ const T dt,
+ MatrixBase<D7>& dp, QuaternionBase<D8>& dq, MatrixBase<D9>& dv, T& Dt)
+{
+ // Same as imu_tools.h
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(v1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(v2);
+ MatrixSizeCheck<3, 1>::check(dp);
+ MatrixSizeCheck<3, 1>::check(dv);
+
+ dp = q1.conjugate() * ( p2 - p1 - v1*dt - (T)0.5*gravity().cast<T>()*(T)dt*(T)dt );
+ dv = q1.conjugate() * ( v2 - v1 - gravity().cast<T>()*(T)dt );
+ dq = q1.conjugate() * q2;
+ Dt = dt; // a bit stupid... to keep things consistant
+}
+
+template<typename D1, typename D2, typename D3, class T>
+inline void betweenStates(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt,
+ MatrixBase<D3>& x2_minus_x1)
+{
+ MatrixSizeCheck<10, 1>::check(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);
+
+ betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv, Dt);
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 11, 1> betweenStates(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 11, 1> ret;
+ betweenStates(x1, x2, dt, ret);
+ return ret;
+}
+
+
+template<typename D1, typename D2, typename D3>
+inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>& P){
+ // See equations 29-31 humanoids fourmy 19
+ // th = omega * dt
+ MatrixSizeCheck<3, 1>::check(th);
+ MatrixSizeCheck<3, 3>::check(Q);
+ MatrixSizeCheck<3, 3>::check(P);
+
+ typename D1::Scalar thn = th.norm();
+ if (thn < 1e-8){
+ Q.setIdentity();
+ P.setIdentity();
+ P *= 0.5;
+ }
+ else{
+ Matrix<typename D1::Scalar, 3, 3> Id;
+ Id.setIdentity();
+ Matrix<typename D1::Scalar, 3, 1> u = th / thn;
+ Matrix<typename D1::Scalar, 3, 3> ux = skew(u);
+
+ Q = Id +
+ + ((1 - cos(thn))/thn)*ux
+ + ((thn - sin(thn))/(thn*thn))*(ux*ux);
+
+ P = 0.5*Id
+ + ((thn - sin(thn))/(thn*thn))*ux
+ + ((cos(thn) + 0.5*thn*thn - 1)/(thn*thn))*(ux*ux);
+ }
+}
+
+template<typename Derived>
+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;
+
+ 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;
+ QandPmat(do_in, Q, P);
+ std::cout << "Q" << std::endl;
+ std::cout << Q << std::endl;
+ std::cout << "P" << std::endl;
+ std::cout << P << std::endl;
+
+ dp = Q*dp_in + P*dv_in*dt_in;
+ dv = Q*dp_in;
+ dq = exp_q(do_in);
+ dt = dt_in;
+
+ return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta_in)
+{
+ MatrixSizeCheck<11, 1>::check(delta_in);
+ Matrix<typename Derived::Scalar, 10, 1> ret;
+
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
+ Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & delta_in(7) );
+ const typename Derived::Scalar& dt_in = delta_in(9);
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) );
+ typename Derived::Scalar& dt_ret = ret(9);
+
+ Matrix<typename Derived::Scalar, 3, 3> Q;
+ Matrix<typename Derived::Scalar, 3, 3> Qinv;
+ Matrix<typename Derived::Scalar, 3, 3> P;
+ do_ret = log_q(dq_in);
+ QandPmat(do_ret, Q, P);
+ Qinv = Q.inverse();
+
+ dp_ret = Qinv*(dp_in - P*Qinv*dv_in*dt_in);
+ dv_ret = Qinv*dv_in;
+ dt_ret = dt_in;
+
+ return ret;
+}
+
+
+
+// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+// inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+// const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2,
+// MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v )
+// {
+// plus_p = dp1 + dp2;
+// plus_v = dv1 + dv2;
+// plus_q = dq1 * exp_q(do2);
+// }
+
+// template<typename D1, typename D2, typename D3>
+// inline void plus(const MatrixBase<D1>& d1,
+// const MatrixBase<D2>& d2,
+// MatrixBase<D3>& d_pert)
+// {
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+// Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) );
+// Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) );
+
+// plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p);
+// }
+
+// template<typename D1, typename D2>
+// inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
+// const MatrixBase<D2>& d2)
+// {
+// Matrix<typename D1::Scalar, 10, 1> ret;
+// plus(d1, d2, ret);
+// return ret;
+// }
+
+// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+// inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+// MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v )
+// {
+// diff_p = dp2 - dp1;
+// diff_v = dv2 - dv1;
+// diff_o = log_q(dq1.conjugate() * dq2);
+// }
+
+// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
+// inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+// MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
+// MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
+// {
+// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
+
+// J_do_dq1 = - jac_SO3_left_inv(diff_o);
+// J_do_dq2 = jac_SO3_right_inv(diff_o);
+// }
+
+// template<typename D1, typename D2, typename D3>
+// inline void diff(const MatrixBase<D1>& d1,
+// const MatrixBase<D2>& d2,
+// MatrixBase<D3>& err)
+// {
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+// Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+// Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & err(6) );
+
+// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
+// }
+
+// template<typename D1, typename D2, typename D3, typename D4, typename D5>
+// inline void diff(const MatrixBase<D1>& d1,
+// const MatrixBase<D2>& d2,
+// MatrixBase<D3>& dif,
+// MatrixBase<D4>& J_diff_d1,
+// MatrixBase<D5>& J_diff_d2)
+// {
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+// Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+// Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
+
+// Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+
+// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+
+// /* d = diff(d1, d2) is
+// * dp = dp2 - dp1
+// * do = Log(dq1.conj * dq2)
+// * dv = dv2 - dv1
+// *
+// * With trivial Jacobians for dp and dv, and:
+// * J_do_dq1 = - J_l_inv(theta)
+// * J_do_dq2 = J_r_inv(theta)
+// */
+
+// J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity
+// J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+
+// J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
+// J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+// }
+
+// template<typename D1, typename D2>
+// inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
+// const MatrixBase<D2>& d2)
+// {
+// Matrix<typename D1::Scalar, 9, 1> ret;
+// diff(d1, d2, ret);
+// return ret;
+// }
+
+// template<typename D1, typename D2, typename D3, typename D4, typename D5>
+// inline void body2delta(const MatrixBase<D1>& a,
+// const MatrixBase<D2>& w,
+// const typename D1::Scalar& dt,
+// MatrixBase<D3>& dp,
+// QuaternionBase<D4>& dq,
+// MatrixBase<D5>& dv)
+// {
+// MatrixSizeCheck<3,1>::check(a);
+// MatrixSizeCheck<3,1>::check(w);
+// MatrixSizeCheck<3,1>::check(dp);
+// MatrixSizeCheck<3,1>::check(dv);
+
+// dp = 0.5 * a * dt * dt;
+// dv = a * dt;
+// dq = exp_q(w * dt);
+// }
+
+// template<typename D1>
+// inline Matrix<typename D1::Scalar, 10, 1> body2delta(const MatrixBase<D1>& body,
+// const typename D1::Scalar& dt)
+// {
+// MatrixSizeCheck<6,1>::check(body);
+
+// typedef typename D1::Scalar T;
+
+// Matrix<T, 10, 1> delta;
+
+// Map< Matrix<T, 3, 1>> dp ( & delta(0) );
+// Map< Quaternion<T>> dq ( & delta(3) );
+// Map< Matrix<T, 3, 1>> dv ( & delta(7) );
+
+// body2delta(body.block(0,0,3,1), body.block(3,0,3,1), dt, dp, dq, dv);
+
+// return delta;
+// }
+
+// template<typename D1, typename D2, typename D3>
+// inline void body2delta(const MatrixBase<D1>& body,
+// const typename D1::Scalar& dt,
+// MatrixBase<D2>& delta,
+// MatrixBase<D3>& jac_body)
+// {
+// MatrixSizeCheck<6,1>::check(body);
+// MatrixSizeCheck<9,6>::check(jac_body);
+
+// typedef typename D1::Scalar T;
+
+// delta = body2delta(body, dt);
+
+// Matrix<T, 3, 1> w = body.block(3,0,3,1);
+
+// jac_body.setZero();
+// jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
+// jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
+// jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
+// }
+
+// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+// void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const QuaternionBase<D3>& q, const MatrixBase<D4>& a_b, const MatrixBase<D5>& w_b, MatrixBase<D6>& a_m, MatrixBase<D7>& w_m)
+// {
+// MatrixSizeCheck<3,1>::check(a);
+// MatrixSizeCheck<3,1>::check(w);
+// MatrixSizeCheck<3,1>::check(a_b);
+// MatrixSizeCheck<3,1>::check(w_b);
+// MatrixSizeCheck<3,1>::check(a_m);
+// MatrixSizeCheck<3,1>::check(w_m);
+
+// // Note: data = (a_m , w_m)
+// a_m = a + a_b - q.conjugate()*gravity();
+// w_m = w + w_b;
+// }
+
+// /* Create IMU data from body motion
+// * Input:
+// * motion : [ax, ay, az, wx, wy, wz] the motion in body frame
+// * q : the current orientation wrt horizontal
+// * bias : the bias of the IMU
+// * Output:
+// * return : the data vector as created by the IMU (with motion, gravity, and bias)
+// */
+// template<typename D1, typename D2, typename D3>
+// Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
+// {
+// Matrix<typename D1::Scalar, 6, 1> data;
+// Map<Matrix<typename D1::Scalar, 3, 1>> a_m (data.data() );
+// Map<Matrix<typename D1::Scalar, 3, 1>> w_m (data.data() + 3);
+
+// motion2data(motion.block(0,0,3,1),
+// motion.block(3,0,3,1),
+// q,
+// bias.block(0,0,3,1),
+// bias.block(3,0,3,1),
+// a_m,
+// w_m );
+
+// return data;
+// }
+
+} // namespace imu
+} // namespace wolf
+
+#endif /* IMU_TOOLS_LIE_H_ */
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 266bdfb1e5e5e3faf8adf566570285b0be5eee57..d4d5539b9100f6cad5edce7dc7a88a693a81156c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -22,6 +22,9 @@ target_link_libraries(gtest_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_IMU_tools gtest_IMU_tools.cpp)
target_link_libraries(gtest_IMU_tools ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_IMU_tools_Lie gtest_IMU_tools_Lie.cpp)
+target_link_libraries(gtest_IMU_tools_Lie ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_processor_IMU_jacobians gtest_processor_IMU_jacobians.cpp)
target_link_libraries(gtest_processor_IMU_jacobians ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..97fdfebb271e675fdf7dcb04b49089b9d1d0f6f1
--- /dev/null
+++ b/test/gtest_IMU_tools_Lie.cpp
@@ -0,0 +1,636 @@
+/*
+ * gtest_imu_tools_Lie.cpp
+ *
+ * Created on: Oct 7, 2019
+ * Author: mfourmy
+ */
+
+#include "IMU/math/IMU_tools_Lie.h"
+#include <core/utils/utils_gtest.h>
+
+using namespace Eigen;
+using namespace wolf;
+using namespace imu_with_dt; // used in IMU_tools_Lie
+
+TEST(IMU_tools, identity)
+{
+ VectorXs id_true;
+ id_true.setZero(11);
+ id_true(6) = 1.0;
+
+ VectorXs id = identity<Scalar>();
+ ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
+}
+
+TEST(IMU_tools, identity_split)
+{
+ VectorXs p(3), qv(4), v(3);
+ Scalar dt;
+ Quaternions q;
+
+ identity(p,q,v,dt);
+ ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
+ ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
+ ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
+ ASSERT_NEAR(dt, 0.0, 1e-10);
+
+ identity(p,qv,v,dt);
+ ASSERT_MATRIX_APPROX(p , Vector3s::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(qv, (Vector4s()<<0,0,0,1).finished(), 1e-10);
+ ASSERT_MATRIX_APPROX(v , Vector3s::Zero(), 1e-10);
+ ASSERT_NEAR(dt, 0.0, 1e-10);
+}
+
+TEST(IMU_tools, inverse)
+{
+ VectorXs d(11), id(11), iid(11), iiid(11), I(11);
+ Vector4s qv;
+
+ qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ d << 0, 1, 2, qv, 7, 8, 9, 0.1;
+
+ id = inverse(d);
+ iid = inverse(id);
+
+ ASSERT_MATRIX_APPROX(d, iid, 1e-10);
+
+ compose(d, id, I);
+ ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
+ compose(id, d, I);
+ ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
+
+ ASSERT_MATRIX_APPROX( d, iid, 1e-10);
+ iiid = inverse(iid);
+ ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
+}
+
+TEST(IMU_tools, compose_between)
+{
+ VectorXs dx1(11), dx2(11), dx3(11);
+ Matrix<Scalar, 11, 1> d1, d2, d3;
+ Vector4s qv;
+ Scalar dt = 0.1;
+
+ qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ dx1 << 0, 1, 2, qv, 7, 8, 9, dt;
+ d1 = dx1;
+ qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+ dx2 << 9, 8, 7, qv, 2, 1, 0, dt;
+ d2 = dx2;
+
+ // combinations of templates for sum()
+ compose(dx1, dx2, dx3);
+ compose(d1, d2, d3);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10); // ?? compare results from the same fonction?
+
+ compose(d1, dx2, dx3);
+ d3 = compose(dx1, d2);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+ // minus(d1, d3) should recover delta_2
+ VectorXs diffx(11);
+ Matrix<Scalar,11,1> diff;
+ between(d1, d3, diff);
+ ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
+
+ // minus(d3, d1) should recover inverse(d2)
+ diff = between(d3, d1);
+ ASSERT_MATRIX_APPROX(diff, inverse(d2), 1e-10);
+}
+
+TEST(IMU_tools, compose_between_with_state)
+{
+ VectorXs x(10), d(11), x2(10), x3(10), d2(11), d3(11);
+ Vector4s qv;
+ Scalar dt = 0.1;
+
+ qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ x << 0, 1, 2, qv, 7, 8, 9;
+ qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+ d << 9, 8, 7, qv, 2, 1, 0, dt;
+
+ composeOverState(x, d, x2);
+ x3 = composeOverState(x, d);
+ ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
+
+ // betweenStates(x, x2) should recover d
+ betweenStates(x, x2, dt, d2);
+ d3 = betweenStates(x, x2, dt);
+ ASSERT_MATRIX_APPROX(d2, d, 1e-10);
+ ASSERT_MATRIX_APPROX(d3, d, 1e-10);
+ ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10);
+
+ // x + (x2 - x) = x2
+ ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt)), x2, 1e-10);
+
+ // (x + d) - x = d
+ ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d), dt), d, 1e-10);
+}
+
+TEST(IMU_tools, lift_retract)
+{
+ VectorXs d_min(10);
+ Scalar dt_min = 0.1;
+ d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8, dt_min; // use angles in the ball theta < pi
+
+ // transform to delta
+ VectorXs delta = exp_IMU(d_min);
+
+ // expected delta -> reimplement formulas TODO
+ // Vector3s dp = d_min.head(3);
+ // Quaternions dq = v2q(d_min.segment<3>(3));
+ // Vector3s dv = d_min.tail(3);
+ // Scalar dt = d_min(9);
+ // VectorXs delta_true(11); delta_true << dp, dq.coeffs(), dv, dt;
+ // ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
+
+ // transform to a new tangent -- should be the original tangent
+ VectorXs d_from_delta = log_IMU(delta);
+ ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
+
+ // transform to a new delta -- should be the previous delta
+ VectorXs delta_from_d = exp_IMU(d_from_delta);
+ ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+}
+
+// TEST(IMU_tools, plus)
+// {
+// VectorXs d1(10), d2(10), d3(10);
+// VectorXs err(9);
+// Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+// d1 << 0, 1, 2, qv, 7, 8, 9;
+// err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
+
+// d3.head(3) = d1.head(3) + err.head(3);
+// d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
+// d3.tail(3) = d1.tail(3) + err.tail(3);
+
+// plus(d1, err, d2);
+// ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10);
+// }
+
+// TEST(IMU_tools, diff)
+// {
+// VectorXs d1(10), d2(10);
+// Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+// d1 << 0, 1, 2, qv, 7, 8, 9;
+// d2 = d1;
+
+// VectorXs err(9);
+// diff(d1, d2, err);
+// ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
+// ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10);
+
+// VectorXs d3(10);
+// d3.setRandom(); d3.segment(3,4).normalize();
+// err.head(3) = d3.head(3) - d1.head(3);
+// err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3));
+// err.tail(3) = d3.tail(3) - d1.tail(3);
+
+// ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
+
+// }
+
+// TEST(IMU_tools, compose_jacobians)
+// {
+// VectorXs d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
+// VectorXs t1(9), t2(9); // tangents
+// Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
+// Vector4s qv1, qv2;
+// Scalar dt = 0.1;
+// Scalar dx = 1e-6;
+// qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+// d1 << 0, 1, 2, qv1, 7, 8, 9;
+// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+// d2 << 9, 8, 7, qv2, 2, 1, 0;
+
+// // analytical jacobians
+// compose(d1, d2, dt, d3, J1_a, J2_a);
+
+// // numerical jacobians
+// for (unsigned int i = 0; i<9; i++)
+// {
+// t1 . setZero();
+// t1(i) = dx;
+
+// // Jac wrt first delta
+// d1_pert = plus(d1, t1); // (d1 + t1)
+// d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
+// t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
+// J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
+
+// // Jac wrt second delta
+// d2_pert = plus(d2, t1); // (d2 + t1)
+// d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
+// t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
+// J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
+// }
+
+// // check that numerical and analytical match
+// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
+// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
+// }
+
+// TEST(IMU_tools, diff_jacobians)
+// {
+// VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
+// VectorXs t1(9), t2(9); // tangents
+// Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
+// Vector4s qv1, qv2;
+// Scalar dx = 1e-6;
+// qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+// d1 << 0, 1, 2, qv1, 7, 8, 9;
+// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+// d2 << 9, 8, 7, qv2, 2, 1, 0;
+
+// // analytical jacobians
+// diff(d1, d2, d3, J1_a, J2_a);
+
+// // numerical jacobians
+// for (unsigned int i = 0; i<9; i++)
+// {
+// t1 . setZero();
+// t1(i) = dx;
+
+// // Jac wrt first delta
+// d1_pert = plus(d1, t1); // (d1 + t1)
+// d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
+// t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 }
+// J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
+
+// // Jac wrt second delta
+// d2_pert = plus(d2, t1); // (d2 + t1)
+// d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
+// t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 }
+// J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
+// }
+
+// // check that numerical and analytical match
+// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
+// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
+// }
+
+// TEST(IMU_tools, body2delta_jacobians)
+// {
+// VectorXs delta(10), delta_pert(10); // deltas
+// VectorXs body(6), pert(6);
+// VectorXs tang(9); // tangents
+// Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
+// Vector4s qv;;
+// Scalar dt = 0.1;
+// Scalar dx = 1e-6;
+// qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+// delta << 0, 1, 2, qv, 7, 8, 9;
+// body << 1, 2, 3, 3, 2, 1;
+
+// // analytical jacobians
+// body2delta(body, dt, delta, J_a);
+
+// // numerical jacobians
+// for (unsigned int i = 0; i<6; i++)
+// {
+// pert . setZero();
+// pert(i) = dx;
+
+// // Jac wrt first delta
+// delta_pert = body2delta(body + pert, dt); // delta(body + pert)
+// tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
+// J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
+// }
+
+// // check that numerical and analytical match
+// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+// }
+
+// TEST(motion2data, zero)
+// {
+// Vector6s motion;
+// Vector6s bias;
+// Quaternions q;
+
+// motion .setZero();
+// bias .setZero();
+// q .setIdentity();
+
+// Vector6s data = imu::motion2data(motion, q, bias);
+
+// Vector6s data_true; data_true << -gravity(), Vector3s::Zero();
+
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+// }
+
+// TEST(motion2data, motion)
+// {
+// Vector6s motion, g_extended;
+// Vector6s bias;
+// Quaternions q;
+
+// g_extended << gravity() , Vector3s::Zero();
+
+// motion << 1,2,3, 4,5,6;
+// bias .setZero();
+// q .setIdentity();
+
+// Vector6s data = imu::motion2data(motion, q, bias);
+
+// Vector6s data_true; data_true = motion - g_extended;
+
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+// }
+
+// TEST(motion2data, bias)
+// {
+// Vector6s motion, g_extended;
+// Vector6s bias;
+// Quaternions q;
+
+// g_extended << gravity() , Vector3s::Zero();
+
+// motion .setZero();
+// bias << 1,2,3, 4,5,6;
+// q .setIdentity();
+
+// Vector6s data = imu::motion2data(motion, q, bias);
+
+// Vector6s data_true; data_true = bias - g_extended;
+
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+// }
+
+// TEST(motion2data, orientation)
+// {
+// Vector6s motion, g_extended;
+// Vector6s bias;
+// Quaternions q;
+
+// g_extended << gravity() , Vector3s::Zero();
+
+// motion .setZero();
+// bias .setZero();
+// q = v2q(Vector3s(M_PI/2, 0, 0)); // turn 90 deg in X axis
+
+// Vector6s data = imu::motion2data(motion, q, bias);
+
+// Vector6s data_true; data_true << 0,-gravity()(2),0, 0,0,0;
+
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+// }
+
+// TEST(motion2data, AllRandom)
+// {
+// Vector6s motion, g_extended;
+// Vector6s bias;
+// Quaternions q;
+
+// motion .setRandom();
+// bias .setRandom();
+// q.coeffs() .setRandom().normalize();
+
+// g_extended << q.conjugate()*gravity() , Vector3s::Zero();
+
+// Vector6s data = imu::motion2data(motion, q, bias);
+
+// Vector6s data_true; data_true = motion + bias - g_extended;
+
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+// }
+
+// /* Integrate acc and angVel motion, obtain Delta_preintegrated
+// * Input:
+// * N: number of steps
+// * q0: initial orientaiton
+// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+// * bias_real: the real bias of the IMU
+// * bias_preint: the bias used for Delta pre-integration
+// * Output:
+// * return: the preintegrated delta
+// */
+// VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
+// {
+// VectorXs data(6);
+// VectorXs body(6);
+// VectorXs delta(10);
+// VectorXs Delta(10), Delta_plus(10);
+// Delta << 0,0,0, 0,0,0,1, 0,0,0;
+// Quaternions q(q0);
+// for (int n = 0; n<N; n++)
+// {
+// data = motion2data(motion, q, bias_real);
+// q = q*exp_q(motion.tail(3)*dt);
+// body = data - bias_preint;
+// delta = body2delta(body, dt);
+// Delta_plus = compose(Delta, delta, dt);
+// Delta = Delta_plus;
+// }
+// return Delta;
+// }
+
+// /* Integrate acc and angVel motion, obtain Delta_preintegrated
+// * Input:
+// * N: number of steps
+// * q0: initial orientaiton
+// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+// * bias_real: the real bias of the IMU
+// * bias_preint: the bias used for Delta pre-integration
+// * Output:
+// * J_D_b: the Jacobian of the preintegrated delta wrt the bias
+// * return: the preintegrated delta
+// */
+// VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+// {
+// VectorXs data(6);
+// VectorXs body(6);
+// VectorXs delta(10);
+// Matrix<Scalar, 9, 6> J_d_d, J_d_b;
+// Matrix<Scalar, 9, 9> J_D_D, J_D_d;
+// VectorXs Delta(10), Delta_plus(10);
+// Quaternions q;
+
+// Delta << 0,0,0, 0,0,0,1, 0,0,0;
+// J_D_b.setZero();
+// q = q0;
+// for (int n = 0; n<N; n++)
+// {
+// // Simulate data
+// data = motion2data(motion, q, bias_real);
+// q = q*exp_q(motion.tail(3)*dt);
+// // Motion::integrateOneStep()
+// { // IMU::computeCurrentDelta
+// body = data - bias_preint;
+// body2delta(body, dt, delta, J_d_d);
+// J_d_b = - J_d_d;
+// }
+// { // IMU::deltaPlusDelta
+// compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
+// }
+// // Motion:: jac calib
+// J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
+// // Motion:: buffer
+// Delta = Delta_plus;
+// }
+// return Delta;
+// }
+
+// TEST(IMU_tools, integral_jacobian_bias)
+// {
+// VectorXs Delta(10), Delta_pert(10); // deltas
+// VectorXs bias_real(6), pert(6), bias_pert(6), bias_preint(6);
+// VectorXs body(6), data(6), motion(6);
+// VectorXs tang(9); // tangents
+// Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
+// Scalar dt = 0.001;
+// Scalar dx = 1e-4;
+// Quaternions q0(3, 4, 5, 6); q0.normalize();
+// motion << .1, .2, .3, .3, .2, .1;
+// bias_real << .002, .004, .001, .003, .002, .001;
+// bias_preint << .004, .005, .006, .001, .002, .003;
+
+// int N = 500; // # steps of integration
+
+// // Analytical Jacobians
+// Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a);
+
+// // numerical jacobians
+// for (unsigned int i = 0; i<6; i++)
+// {
+// pert . setZero();
+// pert(i) = dx;
+
+// bias_pert = bias_preint + pert;
+// Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt);
+// tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias)
+// J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx
+// }
+
+// // check that numerical and analytical match
+// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+// }
+
+// TEST(IMU_tools, delta_correction)
+// {
+// VectorXs Delta(10), Delta_preint(10), Delta_corr; // deltas
+// VectorXs bias(6), pert(6), bias_preint(6);
+// VectorXs body(6), motion(6);
+// VectorXs tang(9); // tangents
+// Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
+// Vector4s qv;;
+// Scalar dt = 0.001;
+// Quaternions q0(3, 4, 5, 6); q0.normalize();
+// motion << 1, 2, 3, 3, 2, 1; motion *= .1;
+
+// int N = 1000; // # steps of integration
+
+// // preintegration with correct bias
+// bias << .004, .005, .006, .001, .002, .003;
+// Delta = integrateDelta(N, q0, motion, bias, bias, dt);
+
+// // preintegration with wrong bias
+// pert << .002, -.001, .003, -.0003, .0002, -.0001;
+// bias_preint = bias + pert;
+// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
+
+// // correct perturbated
+// Vector9s step = J_b*(bias-bias_preint);
+// Delta_corr = plus(Delta_preint, step);
+
+// // Corrected delta should match real delta
+// ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
+
+// // diff between real and corrected should be zero
+// ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9s::Zero(), 1e-5);
+
+// // diff between preint and corrected deltas should be the jacobian-computed step
+// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
+// }
+
+// TEST(IMU_tools, full_delta_residual)
+// {
+// VectorXs x1(10), x2(10);
+// VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
+// VectorXs Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
+// VectorXs bias(6), pert(6), bias_preint(6), bias_null(6);
+// VectorXs body(6), motion(6);
+// VectorXs tang(9); // tangents
+// Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
+// Scalar dt = 0.001;
+// Quaternions q0; q0.setIdentity();
+
+// x1 << 0,0,0, 0,0,0,1, 0,0,0;
+// motion << .3, .2, .1, .1, .2, .3; // acc and gyro
+// // motion << .3, .2, .1, .0, .0, .0; // only acc
+// // motion << .0, .0, .0, .1, .2, .3; // only gyro
+// bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
+// bias_null << 0, 0, 0, 0, 0, 0;
+// // bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
+// bias_preint = bias_null;
+
+// int N = 1000; // # steps of integration
+
+// // preintegration with no bias
+// Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
+
+// // final state
+// x2 = composeOverState(x1, Delta_real, 1000*dt);
+
+// // preintegration with the correct bias
+// Delta = integrateDelta(N, q0, motion, bias, bias, dt);
+
+// ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
+
+// // preintegration with wrong bias
+// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
+
+// // compute correction step
+// Vector9s step = J_b*(bias-bias_preint);
+
+// // correct preintegrated delta
+// Delta_corr = plus(Delta_preint, step);
+
+// // Corrected delta should match real delta
+// ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
+
+// // diff between real and corrected should be zero
+// ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9s::Zero(), 1e-3);
+
+// // expected delta
+// Delta_exp = betweenStates(x1, x2, N*dt);
+
+// ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
+
+// // compute diff between expected and corrected
+// // Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
+// diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr);
+
+// ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
+
+// // compute error between expected and preintegrated
+// Delta_err = diff(Delta_preint, Delta_exp);
+
+// // diff between preint and corrected deltas should be the jacobian-computed step
+// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
+// /* This implies:
+// * - Since D_corr is expected to be similar to D_exp
+// * step ~ diff(Delta_preint, Delta_exp)
+// * - the residual can be computed with a regular '-' :
+// * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
+// */
+
+// // WOLF_TRACE("Delta real: ", Delta_real.transpose());
+// // WOLF_TRACE("x2: ", x2.transpose());
+// // WOLF_TRACE("Delta: ", Delta.transpose());
+// // WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
+// // WOLF_TRACE("Jac bias: \n", J_b);
+// // WOLF_TRACE("Delta step: ", step.transpose());
+// // WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
+// // WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
+// // WOLF_TRACE("Delta err: ", Delta_err.transpose());
+// // WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
+// }
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
+ return RUN_ALL_TESTS();
+}
+