New imu tools body2delta()

src/imu_tools.h

@@ -429,6 +429,49 @@ inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
 }
 
 
+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;
+
+    Matrix<T, 3, 1> a_dt = body.block(0,0,3,1) * dt;
+    Matrix<T, 3, 1> w_dt = body.block(3,0,3,1) * dt;
+
+    Quaternion<T> delta_q = exp_q(w_dt);
+
+    delta.block(0,0,3,1) = 0.5 * a_dt * dt;
+    delta.block(3,0,4,1) = delta_q.coeffs();
+    delta.block(7,0,3,1) = a_dt;
+    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();
+}
+
+
 } // namespace imu
 } // namespace wolf
 

src/test/gtest_imu_tools.cpp

@@ -201,6 +201,38 @@ TEST(IMU_tools, compose_jacobians)
     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);
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);