From 5f72621e336a53c60c51d4e761569b411cafc049 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Thu, 10 Oct 2019 12:48:03 +0200
Subject: [PATCH] Implemented tests for compose and diff jacobians, WIP

---
 include/IMU/math/IMU_tools_Lie.h | 144 +++++++++++++++----------------
 test/gtest_IMU_tools_Lie.cpp     | 110 ++++++++---------------
 2 files changed, 109 insertions(+), 145 deletions(-)

diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 6ea135922..3c61fae2f 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -184,27 +184,26 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
 
 
 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);
 
-    // Adjoint matrix associated to the adjoint operator
-    Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
-    Map<const Quaternion<typename D1::Scalar> >   dq ( &d(3) );
-    Matrix<typename D1::Scalar, 3, 3>             dR = q2R(dq);
-    Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 7 ) );
-    const typename D1::Scalar&                    dt = d(10);
+    Map<const Matrix<typename D1::Scalar, 3, 1> > deltap ( &   delta( 0 ) );
+    Map<const Quaternion<typename D1::Scalar> >   deltaq ( &   delta(3) );
+    Matrix<typename D1::Scalar, 3, 3>             deltaR = q2R(deltaq);
+    Map<const Matrix<typename D1::Scalar, 3, 1> > deltav ( &   delta( 7 ) );
+    const typename D1::Scalar&                    deltat =     delta(10);
     
-    // pqvt impl
+    // pqvt impl (in paper: pvqt)
     adjd.setIdentity();
-    adjd.block(0,0,3,3) = dR;
-    adjd.block(0,3,3,3) = skew(dp - dv * dt) * dR;
-    adjd.block(0,6,3,3) = -dR * dt;
-    adjd.block(0,9,3,1) = dv;
-    adjd.block(3,3,3,3) = dR;
-    adjd.block(6,3,3,3) = skew(dv) * dR;
-    adjd.block(6,6,3,3) = dR;
+    adjd.block(0,0,3,3) = deltaR;
+    adjd.block(0,3,3,3) = skew(deltap - deltav * deltat) * deltaR;
+    adjd.block(0,6,3,3) = -deltaR * deltat;
+    adjd.block(0,9,3,1) = deltav;
+    adjd.block(3,3,3,3) = deltaR;
+    adjd.block(6,3,3,3) = skew(deltav) * deltaR;
+    adjd.block(6,6,3,3) = deltaR;
 }
 
 template<typename D>
@@ -215,14 +214,32 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
 }
 
 
-// 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  
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
+{
+    MatrixSizeCheck<10, 1>::check(d);
+    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>
 inline void compose(const MatrixBase<D1>& delta1,
@@ -444,29 +461,27 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
 
 {
     MatrixSizeCheck<10, 1>::check(d_in);
-
     typedef typename Derived::Scalar T;
 
-
     Matrix<T, 11, 1> ret;
 
     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> >   dv_in  ( & d_in(6) );
-    const T&                      dt_in =    d_in(9);
-    Map<Matrix<T, 3, 1> >         dp     ( &  ret(0) );
-    Map<Quaternion<T> >           dq     ( &  ret(3) );
-    Map<Matrix<T, 3, 1> >         dv     ( &  ret(7) );
-    T&                            dt    =     ret(10);
+    const T&                      dt_in  =   d_in(9);
+    Map<Matrix<T, 3, 1> >         dp_ret ( &  ret(0) );
+    Map<Quaternion<T> >           dq_ret ( &  ret(3) );
+    Map<Matrix<T, 3, 1> >         dv_ret ( &  ret(7) );
+    T&                            dt_ret =    ret(10);
 
     Matrix<T, 3, 3> Q;
     Matrix<T, 3, 3> P;
     QandPmat(do_in, Q, P);
 
-    dp = Q*dp_in + P*dv_in*dt_in;
-    dv = Q*dv_in;
-    dq = exp_q(do_in);
-    dt = dt_in;
+    dp_ret = Q*dp_in + P*dv_in*dt_in;
+    dv_ret = Q*dv_in;
+    dq_ret = exp_q(do_in);
+    dt_ret = dt_in;
 
     return ret;
 }
@@ -497,7 +512,6 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
     dv_ret = Qinv*dv_in;
     dt_ret = dt_in;
 
-//    std::cout << "Log ret" << ret.transpose() << std::endl;
     return ret;
 }
 
@@ -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,
 //                  MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v, T& plus_dt)
 // {
-//     compose()
+    // TODO ?
 //     // plus_p = dp1 + dp2;
 //     // plus_v = dv1 + dv2;
 //     // plus_q = dq1 * exp_q(do2);
@@ -579,43 +593,27 @@ inline Matrix<typename D1::Scalar, 10, 1> diff(const MatrixBase<D1>& delta1,
     return ret;
 }
 
-// template<typename D1, typename D2, typename D3, typename D4, typename D5>
-// inline void diff(const MatrixBase<D1>& delta1,
-//                  const MatrixBase<D2>& delta2,
-//                  MatrixBase<D3>& dif,
-//                  MatrixBase<D4>& J_diff_delta1,
-//                  MatrixBase<D5>& J_diff_delta2)
-// {
-//     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & delta1(0) );
-//     Map<const Quaternion<typename D1::Scalar> >     dq1    ( & delta1(3) );
-//     Map<const Matrix<typename D1::Scalar, 3, 1> >   dv1    ( & delta1(7) );
-//     Map<const Matrix<typename D2::Scalar, 3, 1> >   dp2    ( & delta2(0) );
-//     Map<const Quaternion<typename D2::Scalar> >     dq2    ( & delta2(3) );
-//     Map<const Matrix<typename D2::Scalar, 3, 1> >   dv2    ( & delta2(7) );
-//     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(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>
+inline void diff(const MatrixBase<D1>& delta1,
+                 const MatrixBase<D2>& delta2,
+                 MatrixBase<D3>& err,
+                 MatrixBase<D4>& J_diff_delta1,
+                 MatrixBase<D5>& J_diff_delta2)
+{
+    MatrixSizeCheck<11, 1>::check(delta1);
+    MatrixSizeCheck<11, 1>::check(delta2);
+    MatrixSizeCheck<10, 1>::check(err);
+    MatrixSizeCheck<10, 10>::check(J_diff_delta1);
+    MatrixSizeCheck<10, 10>::check(J_diff_delta2);
+
+    diff(delta1, delta2, err);
+
+    Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+
+    J_diff_delta1 = - Matrix<typename D4::Scalar, 10, 10>::Identity();
+
+    J_diff_delta2 = adjoint(inverse(delta1)) * adjoint(delta2);
+}
 
 // template<typename D1, typename D2, typename D3, typename D4, typename D5>
 // inline void body2delta(const MatrixBase<D1>& a,
diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp
index 2b6fa4653..3901f0674 100644
--- a/test/gtest_IMU_tools_Lie.cpp
+++ b/test/gtest_IMU_tools_Lie.cpp
@@ -221,84 +221,50 @@ TEST(IMU_tools, adjoint)
 
 
 
-// TEST(IMU_tools, compose_jacobians)
-// {
-//     VectorXs delta1(10), delta2(10), delta3(10), delta1_pert(10), delta2_pert(10), delta3_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();
-//     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
-//     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
-//     }
+TEST(IMU_tools, compose_jacobians)
+{
+    VectorXs delta1(11), delta2(11), delta3(11); // deltas
+    VectorXs tau(10); // small tangent space element
+    tau.setOnes();
+    tau *= 1e-6;
+    Matrix<Scalar, 10, 10> J_comp_delta1, J_comp_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;
 
-//     // check that numerical and analytical match
-//     ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
-//     ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-// }
+    // analytical jacobians
+    compose(delta1, delta2, delta3, J_comp_delta1, J_comp_delta2);
 
-// TEST(IMU_tools, diff_jacobians)
-// {
-//     VectorXs delta1(10), delta2(10), delta3(9), delta1_pert(10), delta2_pert(10), delta3_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();
-//     delta1 << 0, 1, 2, qv1, 7, 8, 9;
-//     qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
-//     delta2 << 9, 8, 7, qv2, 2, 1, 0;
+    // 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);
+    // ASSERT_MATRIX_APPROX(compose(delta1, plus(delta2, tau)), plus(compose(delta1, delta2), J_comp_delta2*tau), 1e-4);
+}
 
-//     // analytical jacobians
-//     diff(delta1, delta2, delta3, J1_a, J2_a);
+TEST(IMU_tools, diff_jacobians)
+{
+    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
-//         delta1_pert = plus(delta1, t1);                 //          (delta1 + t1)
-//         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
-//     }
+    // analytical jacobians
+    diff(delta1, delta2, err, J_diff_delta1, J_diff_delta2);
 
-//     // check that numerical and analytical match
-//     ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
-//     ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-// }
+    // check that numerical and analytical match
+    ASSERT_MATRIX_APPROX(diff(plus(delta1, tau), delta2), plus(diff(delta1, delta2), J_diff_delta1*tau), 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)
 // {
-- 
GitLab