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

Added necessary files, adapting the functions in the API one by one, WIP

parent 41af6a35
No related branches found
No related tags found
No related merge requests found
......@@ -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
)
......
/*
* 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_ */
......@@ -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})
......
/*
* 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();
}
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