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

Implemented tests for compose and diff jacobians, WIP

parent 3cf48ae1
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
+ 71
− 73
View file @ 5f72621e
...@@ -184,27 +184,26 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1, ...@@ -184,27 +184,26 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
template<typename D1, typename D2> template<typename D1, typename D2>
inline void adjoint(const MatrixBase<D1>& d, MatrixBase<D2>& adjd) inline void adjoint(const MatrixBase<D1>& delta, MatrixBase<D2>& adjd)
{ {
MatrixSizeCheck<11, 1>::check(d); MatrixSizeCheck<11, 1>::check(delta);
MatrixSizeCheck<10, 10>::check(adjd); MatrixSizeCheck<10, 10>::check(adjd);
// Adjoint matrix associated to the adjoint operator Map<const Matrix<typename D1::Scalar, 3, 1> > deltap ( & delta( 0 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) ); Map<const Quaternion<typename D1::Scalar> > deltaq ( & delta(3) );
Map<const Quaternion<typename D1::Scalar> > dq ( &d(3) ); Matrix<typename D1::Scalar, 3, 3> deltaR = q2R(deltaq);
Matrix<typename D1::Scalar, 3, 3> dR = q2R(dq); Map<const Matrix<typename D1::Scalar, 3, 1> > deltav ( & delta( 7 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 7 ) ); const typename D1::Scalar& deltat = delta(10);
const typename D1::Scalar& dt = d(10);
// pqvt impl // pqvt impl (in paper: pvqt)
adjd.setIdentity(); adjd.setIdentity();
adjd.block(0,0,3,3) = dR; adjd.block(0,0,3,3) = deltaR;
adjd.block(0,3,3,3) = skew(dp - dv * dt) * dR; adjd.block(0,3,3,3) = skew(deltap - deltav * deltat) * deltaR;
adjd.block(0,6,3,3) = -dR * dt; adjd.block(0,6,3,3) = -deltaR * deltat;
adjd.block(0,9,3,1) = dv; adjd.block(0,9,3,1) = deltav;
adjd.block(3,3,3,3) = dR; adjd.block(3,3,3,3) = deltaR;
adjd.block(6,3,3,3) = skew(dv) * dR; adjd.block(6,3,3,3) = skew(deltav) * deltaR;
adjd.block(6,6,3,3) = dR; adjd.block(6,6,3,3) = deltaR;
} }
template<typename D> template<typename D>
...@@ -215,14 +214,32 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){ ...@@ -215,14 +214,32 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
} }
// template<typename D1, typename D2> template<typename D1, typename D2>
// inline Matrix<T, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd) inline Matrix<typename D1::Scalar, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
// { {
// // Adjoint matrix associated to the adjoint operator MatrixSizeCheck<10, 1>::check(d);
// TODO MatrixSizeCheck<10, 10>::check(sadjd);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<typename D1::Scalar> > doo ( &d( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 6 ) );
const typename D1::Scalar& dt = d( 9 );
const Matrix<typename D1::Scalar, 3, 3> Id3 = Matrix<typename D1::Scalar, 3, 3>::identity(3,3);
const Matrix<typename D1::Scalar, 3, 3> dox = skew(doo);
// pqvt impl (in paper: pvqt)
sadjd.setIdentity();
sadjd.block(0,0,3,3) = dox;
sadjd.block(0,3,3,3) = skew(dp);
sadjd.block(0,6,3,3) = -Id3*dt;
sadjd.block(0,9,3,1) = dv;
sadjd.block(3,3,3,3) = dox;
sadjd.block(6,3,3,3) = skew(dv);
sadjd.block(6,6,3,3) = dox;
// return ret; return sadjd;
// } }
template<typename D1, typename D2, typename D3, typename D4, typename D5> template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& delta1, inline void compose(const MatrixBase<D1>& delta1,
...@@ -444,29 +461,27 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in) ...@@ -444,29 +461,27 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
{ {
MatrixSizeCheck<10, 1>::check(d_in); MatrixSizeCheck<10, 1>::check(d_in);
typedef typename Derived::Scalar T; typedef typename Derived::Scalar T;
Matrix<T, 11, 1> ret; Matrix<T, 11, 1> ret;
Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) ); Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) );
Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) ); Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) );
Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) ); Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) );
const T& dt_in = d_in(9); const T& dt_in = d_in(9);
Map<Matrix<T, 3, 1> > dp ( & ret(0) ); Map<Matrix<T, 3, 1> > dp_ret ( & ret(0) );
Map<Quaternion<T> > dq ( & ret(3) ); Map<Quaternion<T> > dq_ret ( & ret(3) );
Map<Matrix<T, 3, 1> > dv ( & ret(7) ); Map<Matrix<T, 3, 1> > dv_ret ( & ret(7) );
T& dt = ret(10); T& dt_ret = ret(10);
Matrix<T, 3, 3> Q; Matrix<T, 3, 3> Q;
Matrix<T, 3, 3> P; Matrix<T, 3, 3> P;
QandPmat(do_in, Q, P); QandPmat(do_in, Q, P);
dp = Q*dp_in + P*dv_in*dt_in; dp_ret = Q*dp_in + P*dv_in*dt_in;
dv = Q*dv_in; dv_ret = Q*dv_in;
dq = exp_q(do_in); dq_ret = exp_q(do_in);
dt = dt_in; dt_ret = dt_in;
return ret; return ret;
} }
...@@ -497,7 +512,6 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta ...@@ -497,7 +512,6 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
dv_ret = Qinv*dv_in; dv_ret = Qinv*dv_in;
dt_ret = dt_in; dt_ret = dt_in;
// std::cout << "Log ret" << ret.transpose() << std::endl;
return ret; return ret;
} }
...@@ -508,7 +522,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta ...@@ -508,7 +522,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
// const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2, const T& dt1, // const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2, const T& dt1,
// MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v, T& plus_dt) // MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v, T& plus_dt)
// { // {
// compose() // TODO ?
// // plus_p = dp1 + dp2; // // plus_p = dp1 + dp2;
// // plus_v = dv1 + dv2; // // plus_v = dv1 + dv2;
// // plus_q = dq1 * exp_q(do2); // // plus_q = dq1 * exp_q(do2);
...@@ -579,43 +593,27 @@ inline Matrix<typename D1::Scalar, 10, 1> diff(const MatrixBase<D1>& delta1, ...@@ -579,43 +593,27 @@ inline Matrix<typename D1::Scalar, 10, 1> diff(const MatrixBase<D1>& delta1,
return ret; return ret;
} }
// template<typename D1, typename D2, typename D3, typename D4, typename D5> template<typename D1, typename D2, typename D3, typename D4, typename D5>
// inline void diff(const MatrixBase<D1>& delta1, inline void diff(const MatrixBase<D1>& delta1,
// const MatrixBase<D2>& delta2, const MatrixBase<D2>& delta2,
// MatrixBase<D3>& dif, MatrixBase<D3>& err,
// MatrixBase<D4>& J_diff_delta1, MatrixBase<D4>& J_diff_delta1,
// MatrixBase<D5>& J_diff_delta2) MatrixBase<D5>& J_diff_delta2)
// { {
// Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1(0) ); MatrixSizeCheck<11, 1>::check(delta1);
// Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1(3) ); MatrixSizeCheck<11, 1>::check(delta2);
// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1(7) ); MatrixSizeCheck<10, 1>::check(err);
// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2(0) ); MatrixSizeCheck<10, 10>::check(J_diff_delta1);
// Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2(3) ); MatrixSizeCheck<10, 10>::check(J_diff_delta2);
// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2(7) );
// Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) ); diff(delta1, delta2, err);
// 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;
// Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2; J_diff_delta1 = - Matrix<typename D4::Scalar, 10, 10>::Identity();
// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2); J_diff_delta2 = adjoint(inverse(delta1)) * adjoint(delta2);
}
// /* d = diff(delta1, delta2) 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_delta1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity
// J_diff_delta1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
// J_diff_delta2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
// J_diff_delta2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
// }
// template<typename D1, typename D2, typename D3, typename D4, typename D5> // template<typename D1, typename D2, typename D3, typename D4, typename D5>
// inline void body2delta(const MatrixBase<D1>& a, // inline void body2delta(const MatrixBase<D1>& a,
......
test/gtest_IMU_tools_Lie.cpp
+ 38
− 72
View file @ 5f72621e
...@@ -221,84 +221,50 @@ TEST(IMU_tools, adjoint) ...@@ -221,84 +221,50 @@ TEST(IMU_tools, adjoint)
// TEST(IMU_tools, compose_jacobians) TEST(IMU_tools, compose_jacobians)
// { {
// VectorXs delta1(10), delta2(10), delta3(10), delta1_pert(10), delta2_pert(10), delta3_pert(10); // deltas VectorXs delta1(11), delta2(11), delta3(11); // deltas
// VectorXs t1(9), t2(9); // tangents VectorXs tau(10); // small tangent space element
// Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n; tau.setOnes();
// Vector4s qv1, qv2; tau *= 1e-6;
// Scalar dt = 0.1; Matrix<Scalar, 10, 10> J_comp_delta1, J_comp_delta2;
// Scalar dx = 1e-6; Vector4s qv1, qv2;
// qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized(); Scalar dt = 0.1;
// delta1 << 0, 1, 2, qv1, 7, 8, 9; qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized(); delta1 << 0, 1, 2, qv1, 7, 8, 9, dt;
// delta2 << 9, 8, 7, qv2, 2, 1, 0; qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
delta2 << 9, 8, 7, qv2, 2, 1, 0, dt;
// // analytical jacobians
// compose(delta1, delta2, dt, delta3, J1_a, J2_a);
// // numerical jacobians
// for (unsigned int i = 0; i<9; i++)
// {
// t1 . setZero();
// t1(i) = dx;
// // Jac wrt first delta
// delta1_pert = plus(delta1, t1); // (delta1 + t1)
// delta3_pert = compose(delta1_pert, delta2, dt); // (delta1 + t1) + delta2
// t2 = diff(delta3, delta3_pert); // { (delta2 + t1) + delta2 } - { delta1 + delta2 }
// J1_n.col(i) = t2/dx; // [ { (delta2 + t1) + delta2 } - { delta1 + delta2 } ] / dx
// // Jac wrt second delta
// delta2_pert = plus(delta2, t1); // (delta2 + t1)
// delta3_pert = compose(delta1, delta2_pert, dt); // delta1 + (delta2 + t1)
// t2 = diff(delta3, delta3_pert); // { delta1 + (delta2 + t1) } - { delta1 + delta2 }
// J2_n.col(i) = t2/dx; // [ { delta1 + (delta2 + t1) } - { delta1 + delta2 } ] / dx
// }
// // check that numerical and analytical match // analytical jacobians
// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4); compose(delta1, delta2, delta3, J_comp_delta1, J_comp_delta2);
// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
// }
// TEST(IMU_tools, diff_jacobians) // That first order approximation holds TODO -> comp problem!
// { // ASSERT_MATRIX_APPROX(compose(plus(delta1, tau), delta2), plus(compose(delta1, delta2), J_comp_delta1*tau), 1e-4);
// VectorXs delta1(10), delta2(10), delta3(9), delta1_pert(10), delta2_pert(10), delta3_pert(9); // deltas // ASSERT_MATRIX_APPROX(compose(delta1, plus(delta2, tau)), plus(compose(delta1, delta2), J_comp_delta2*tau), 1e-4);
// 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();
// delta1 << 0, 1, 2, qv1, 7, 8, 9;
// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
// delta2 << 9, 8, 7, qv2, 2, 1, 0;
// // analytical jacobians TEST(IMU_tools, diff_jacobians)
// diff(delta1, delta2, delta3, J1_a, J2_a); {
VectorXs delta1(11), delta2(11), err(10); // deltas and err
VectorXs tau(10); // small tangent space element
tau.setOnes();
tau *= 1e-6;
Matrix<Scalar, 10, 10> J_diff_delta1, J_diff_delta2;
Vector4s qv1, qv2;
Scalar dt = 0.1;
qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
delta1 << 0, 1, 2, qv1, 7, 8, 9, dt;
qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
delta2 << 9, 8, 7, qv2, 2, 1, 0, dt;
// // numerical jacobians
// for (unsigned int i = 0; i<9; i++)
// {
// t1 . setZero();
// t1(i) = dx;
// // Jac wrt first delta // analytical jacobians
// delta1_pert = plus(delta1, t1); // (delta1 + t1) diff(delta1, delta2, err, J_diff_delta1, J_diff_delta2);
// delta3_pert = diff(delta1_pert, delta2); // delta2 - (delta1 + t1)
// t2 = delta3_pert - delta3; // { delta2 - (delta1 + t1) } - { delta2 - delta1 }
// J1_n.col(i) = t2/dx; // [ { delta2 - (delta1 + t1) } - { delta2 - delta1 } ] / dx
// // Jac wrt second delta
// delta2_pert = plus(delta2, t1); // (delta2 + t1)
// delta3_pert = diff(delta1, delta2_pert); // (delta2 + t1) - delta1
// t2 = delta3_pert - delta3; // { (delta2 + t1) - delta1 } - { delta2 - delta1 }
// J2_n.col(i) = t2/dx; // [ { (delta2 + t1) - delta1 } - { delta2 - delta1 } ] / dx
// }
// // check that numerical and analytical match // check that numerical and analytical match
// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4); ASSERT_MATRIX_APPROX(diff(plus(delta1, tau), delta2), plus(diff(delta1, delta2), J_diff_delta1*tau), 1e-4);
// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4); ASSERT_MATRIX_APPROX(diff(delta1, plus(delta2, tau)), plus(diff(delta1, delta2), J_diff_delta2*tau), 1e-4);
// } }
// TEST(IMU_tools, body2delta_jacobians) // TEST(IMU_tools, body2delta_jacobians)
// { // {
......
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