Fix imu_tools plus() and diff() ...

Some MatrixSizeCheck are still missing
in plus() and diff()

src/imu_tools.h

@@ -150,8 +150,8 @@ inline void compose(const MatrixBase<D1>& d1,
     MatrixSizeCheck<10, 1>::check(d1);
     MatrixSizeCheck<10, 1>::check(d2);
     MatrixSizeCheck<10, 1>::check(sum);
-    MatrixSizeCheck< 9, 1>::check(J_sum_d1);
-    MatrixSizeCheck< 9, 1>::check(J_sum_d2);
+    MatrixSizeCheck< 9, 9>::check(J_sum_d1);
+    MatrixSizeCheck< 9, 9>::check(J_sum_d2);
 
     // Maps over provided data
     Map<const Matrix<typename D1::Scalar, 3, 1> >   dp1    ( & d1( 0 ) );
@@ -364,19 +364,79 @@ Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_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_q = dq1 * exp_q(do2);
+        plus_v = dv1 + dv2;
+}
+
+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_o = log_q(dq1.conjugate() * dq2);
+        diff_v = dv2 - dv1;
+}
+
+
 template<typename D1, typename D2, typename D3>
 inline void diff(const MatrixBase<D1>& d1,
                  const MatrixBase<D2>& d2,
                  MatrixBase<D3>& err)
 {
-    err = lift( between(d1, d2, 0.0) );
+    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>
 inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
                                               const MatrixBase<D2>& d2)
 {
-    return lift( between(d1, d2, 0.0) );
+    Matrix<typename D1::Scalar, 9, 1> ret;
+    diff(d1, d2, ret);
+    return ret;
 }
 
 

src/test/gtest_imu_tools.cpp

@@ -12,6 +12,7 @@
 
 using namespace Eigen;
 using namespace wolf;
+using namespace imu;
 
 TEST(IMU_tools, identity)
 {
@@ -19,7 +20,7 @@ TEST(IMU_tools, identity)
     id_true.setZero(10);
     id_true(6) = 1.0;
 
-    VectorXs id = imu::identity<Scalar>();
+    VectorXs id = identity<Scalar>();
     ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
 }
 
@@ -28,12 +29,12 @@ TEST(IMU_tools, identity_split)
     VectorXs p(3), qv(4), v(3);
     Quaternions q;
 
-    imu::identity(p,q,v);
+    identity(p,q,v);
     ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
     ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
     ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
 
-    imu::identity(p,qv,v);
+    identity(p,qv,v);
     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);
@@ -48,16 +49,16 @@ TEST(IMU_tools, inverse)
     qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
     d << 0, 1, 2, qv, 7, 8, 9;
 
-    id   = imu::inverse(d, dt);
+    id   = inverse(d, dt);
 
-    imu::compose(id, d, dt, I);
-    ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
-    imu::compose(d, id, -dt, I); // Observe -dt is reversed !!
-    ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
+    compose(id, d, dt, I);
+    ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
+    compose(d, id, -dt, I); // Observe -dt is reversed !!
+    ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
 
-    imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
+    inverse(id, -dt, iid); // Observe -dt is reversed !!
     ASSERT_MATRIX_APPROX( d,  iid, 1e-10);
-    iiid = imu::inverse(iid, dt);
+    iiid = inverse(iid, dt);
     ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
 }
 
@@ -76,23 +77,23 @@ TEST(IMU_tools, compose_between)
     d2 = dx2;
 
     // combinations of templates for sum()
-    imu::compose(dx1, dx2, dt, dx3);
-    imu::compose(d1, d2, dt, d3);
+    compose(dx1, dx2, dt, dx3);
+    compose(d1, d2, dt, d3);
     ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
 
-    imu::compose(d1, dx2, dt, dx3);
-    d3 = imu::compose(dx1, d2, dt);
+    compose(d1, dx2, dt, dx3);
+    d3 = compose(dx1, d2, dt);
     ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
 
     // minus(d1, d3) should recover delta_2
     VectorXs diffx(10);
     Matrix<Scalar,10,1> diff;
-    imu::between(d1, d3, dt, diff);
+    between(d1, d3, dt, diff);
     ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
 
     // minus(d3, d1, -dt) should recover inverse(d2, dt)
-    diff = imu::between(d3, d1, -dt);
-    ASSERT_MATRIX_APPROX(diff, imu::inverse(d2, dt), 1e-10);
+    diff = between(d3, d1, -dt);
+    ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
 }
 
 TEST(IMU_tools, compose_between_with_state)
@@ -106,22 +107,22 @@ TEST(IMU_tools, compose_between_with_state)
     qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
     d << 9, 8, 7, qv, 2, 1, 0;
 
-    imu::composeOverState(x, d, dt, x2);
-    x3 = imu::composeOverState(x, d, dt);
+    composeOverState(x, d, dt, x2);
+    x3 = composeOverState(x, d, dt);
     ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
 
     // betweenStates(x, x2) should recover d
-    imu::betweenStates(x, x2, dt, d2);
-    d3 = imu::betweenStates(x, x2, dt);
+    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(imu::betweenStates(x, x2, dt), d, 1e-10);
+    ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10);
 
     // x + (x2 - x) = x2
-    ASSERT_MATRIX_APPROX(imu::composeOverState(x, imu::betweenStates(x, x2, dt), dt), x2, 1e-10);
+    ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10);
 
     // (x + d) - x = d
-    ASSERT_MATRIX_APPROX(imu::betweenStates(x, imu::composeOverState(x, d, dt), dt), d, 1e-10);
+    ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
 }
 
 TEST(IMU_tools, lift_retract)
@@ -129,7 +130,7 @@ TEST(IMU_tools, lift_retract)
     VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
 
     // transform to delta
-    VectorXs delta = imu::retract(d_min);
+    VectorXs delta = retract(d_min);
 
     // expected delta
     Vector3s dp = d_min.head(3);
@@ -139,11 +140,11 @@ TEST(IMU_tools, lift_retract)
     ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
 
     // transform to a new tangent -- should be the original tangent
-    VectorXs d_from_delta = imu::lift(delta);
+    VectorXs d_from_delta = lift(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 = imu::retract(d_from_delta);
+    VectorXs delta_from_d = retract(d_from_delta);
     ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
 }
 
@@ -155,9 +156,49 @@ TEST(IMU_tools, diff)
     d2 = d1;
 
     VectorXs err(9);
-    imu::diff(d1, d2, err);
+    diff(d1, d2, err);
     ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
-    ASSERT_MATRIX_APPROX(imu::diff(d1, d2), VectorXs::Zero(9), 1e-10);
+    ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 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);
 }
 
 int main(int argc, char **argv)