From d9ed52ea5286714a137b0934a6bbbfde408969a5 Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Mon, 4 Nov 2019 14:38:22 +0100 Subject: [PATCH] testing --- include/IMU/math/IMU_tools_Lie.h | 5 +++-- test/gtest_IMU_tools_Lie.cpp | 8 ++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 72b3e5236..41a046762 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -516,12 +516,13 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& 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); + + Matrix<T, 11, 1> ret; + Map<Matrix<T, 3, 1> > dp ( & ret(0) ); Map<Quaternion<T> > dq ( & ret(3) ); Map<Matrix<T, 3, 1> > dv ( & ret(7) ); diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp index 2b6fa4653..adf91fba9 100644 --- a/test/gtest_IMU_tools_Lie.cpp +++ b/test/gtest_IMU_tools_Lie.cpp @@ -169,13 +169,21 @@ TEST(IMU_tools, plus_minus) TEST(IMU_tools, adjoint) { VectorXs delta1(11), delta2(11); + VectorXs tau(10); Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized(); Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized(); delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1; delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3; + tau << .1, .2, .3, -.1, -.2, -.3, .3, .2, .1, 0.1; ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10); ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10); + + MatrixXs tau2 = adjoint(delta1)*tau; +// MatrixXs d3 = compose(delta1, exp_IMU(tau)); +// MatrixXs d4 = compose(exp_IMU(tau2),delta1); + ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau)), compose(exp_IMU(tau2),delta1), 1e-10); +// ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau2)), compose(exp_IMU(tau),delta1), 1e-10); } -- GitLab