New file imu_tools.h with gtest

src/imu_tools.h

+/*
+ * imu_tools.h
+ *
+ *  Created on: Jul 29, 2017
+ *      Author: jsola
+ */
+
+#ifndef IMU_TOOLS_H_
+#define IMU_TOOLS_H_
+
+
+#include "wolf.h"
+#include "rotations.h"
+
+namespace wolf 
+{
+namespace imu {
+using namespace Eigen;
+
+template<typename T = wolf::Scalar>
+inline Matrix<T, 10, 1> identity()
+{
+    Matrix<T, 10, 1> ret;
+    ret.setZero();
+    ret(6) = 1.0;
+    return ret;
+}
+
+//template<>
+//inline Matrix<Scalar, 10, 1> identity()
+//{
+//    Matrix<Scalar, 10, 1> ret;
+//    ret.setZero();
+//    ret(6, 1) = 1.0;
+//    return ret;
+//}
+
+template<typename D1, typename D2, typename D3>
+inline void compose(const MatrixBase<D1>& d1,
+                 const MatrixBase<D2>& d2,
+                 const Scalar& dt,
+                 MatrixBase<D3>& sum)
+{
+    MatrixSizeCheck<10, 1>::check(d1);
+    MatrixSizeCheck<10, 1>::check(d2);
+    MatrixSizeCheck<10, 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 ) );
+    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> >         sum_p  ( &sum( 0 ));
+    Map<Quaternion<typename D3::Scalar> >           sum_q  ( &sum( 3 ));
+    Map<Matrix<typename D3::Scalar, 3, 1> >         sum_v  ( &sum( 7 ));
+
+    sum_p = dp1 + dv1*dt + dq1*dp2;
+    sum_q =                dq1*dq2;
+    sum_v = dv1 +          dq1*dv2;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+                  const MatrixBase<D2>& d2,
+                  const Scalar& dt,
+                  MatrixBase<D3>& d2_minus_d1)
+{
+    MatrixSizeCheck<10, 1>::check(d1);
+    MatrixSizeCheck<10, 1>::check(d2);
+    MatrixSizeCheck<10, 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) );
+    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 (&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));
+
+    diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
+    diff_q = dq1.conjugate() *   dq2;
+    diff_v = dq1.conjugate() * ( dv2 - dv1 );
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
+{
+    MatrixSizeCheck<10, 1>::check(delta_in);
+
+    Matrix<typename Derived::Scalar, 9, 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) );
+    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) );
+
+    dp_ret = dp_in;
+    do_ret = log_q(dq_in);
+    dv_ret = dv_in;
+
+    return ret;
+}
+
+template<typename Derived>
+Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
+{
+    MatrixSizeCheck<9, 1>::check(d_in);
+
+    Matrix<typename Derived::Scalar, 10, 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) );
+    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) );
+
+    dp = dp_in;
+    dq = exp_q(do_in);
+    dv = dv_in;
+
+    return ret;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void compare(const MatrixBase<D1>& d1,
+                  const MatrixBase<D2>& d2,
+                  MatrixBase<D3>& err)
+{
+    Matrix<typename D3::Scalar, 10, 1> delta_err;
+
+    between(d1, d2, 0.0, delta_err);
+
+    err = lift(delta_err);
+}
+
+
+
+} // namespace imu
+} // namespace wolf
+
+#endif /* IMU_TOOLS_H_ */

src/test/CMakeLists.txt

@@ -60,6 +60,10 @@ target_link_libraries(gtest_frame_base ${PROJECT_NAME})
 wolf_add_gtest(gtest_frame_imu gtest_frame_imu.cpp)
 target_link_libraries(gtest_frame_imu ${PROJECT_NAME})
 
+# IMU tools test
+wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
+target_link_libraries(gtest_imu_tools ${PROJECT_NAME})
+
 # LocalParametrizationXxx classes test
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
 target_link_libraries(gtest_local_param ${PROJECT_NAME})

src/test/gtest_imu_tools.cpp

+/*
+ * gtest_imu_tools.cpp
+ *
+ *  Created on: Jul 29, 2017
+ *      Author: jsola
+ */
+
+#include "imu_tools.h"
+
+#include "utils_gtest.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+TEST(IMU_tools, identity)
+{
+    VectorXs id_true;
+    id_true.setZero(10);
+    id_true(6) = 1.0;
+
+    VectorXs id = imu::identity();
+    ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
+}
+
+TEST(IMU_tools, compose_between)
+{
+    VectorXs dx1(10), dx2(10), dx3(10);
+    Matrix<Scalar, 10, 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;
+    d1 = dx1;
+    qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
+    dx2 << 9, 8, 7, qv, 2, 1, 0;
+    d2 = dx2;
+
+    // combinations of templates for sum()
+    imu::compose(dx1, dx2, dt, dx3);
+    imu::compose(d1, d2, dt, d3);
+    ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+    imu::compose(d1, dx2, dt, dx3);
+    imu::compose(dx1, d2, dt, d3);
+    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);
+    ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
+
+}
+
+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);
+
+    // expected delta
+    Vector3s dp = d_min.head(3);
+    Quaternions dq = v2q(d_min.segment(3,3));
+    Vector3s dv = d_min.tail(3);
+    VectorXs delta_true(10); delta_true << dp, dq.coeffs(), dv;
+    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);
+    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);
+    ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+}
+
+TEST(IMU_tools, compare)
+{
+    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);
+    imu::compare(d1, d2, err);
+    ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+