From fb57fa2cbeeef82b21fa2acae2383ad268c9a0a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr> Date: Wed, 9 Oct 2019 20:48:35 +0200 Subject: [PATCH] plu_minus test passes --- include/IMU/math/IMU_tools_Lie.h | 21 ++++++++-------- test/gtest_IMU_tools_Lie.cpp | 42 +++++++++++++++++++++++--------- 2 files changed, 41 insertions(+), 22 deletions(-) diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index 3c4d2a4c2..05ad1b2ba 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -483,24 +483,24 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta // } template<typename D1, typename D2, typename D3> -inline void plus(const MatrixBase<D1>& delta1, - const MatrixBase<D2>& delta2, +inline void plus(const MatrixBase<D1>& delta, + const MatrixBase<D2>& d, MatrixBase<D3>& delta_pert) { - MatrixSizeCheck<11, 1>::check(delta1); - MatrixSizeCheck<10, 1>::check(delta2); + MatrixSizeCheck<11, 1>::check(delta); + MatrixSizeCheck<10, 1>::check(d); MatrixSizeCheck<11, 1>::check(delta_pert); - compose(delta1, exp_IMU(delta2), delta_pert); + compose(delta, exp_IMU(d), delta_pert); } template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& delta1, - const MatrixBase<D2>& delta2) +inline Matrix<typename D1::Scalar, 11, 1> plus(const MatrixBase<D1>& delta, + const MatrixBase<D2>& d) { - Matrix<typename D1::Scalar, 10, 1> ret; - plus(delta1, delta2, ret); - return ret; + Matrix<typename D1::Scalar, 11, 1> delta_ret; + plus(delta, d, delta_ret); + return delta_ret; } // template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9> @@ -530,6 +530,7 @@ inline void diff(const MatrixBase<D1>& delta1, const MatrixBase<D2>& delta2, MatrixBase<D3>& err) { + // diff(A, B) = B - A MatrixSizeCheck<11, 1>::check(delta1); MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<10, 1>::check(err); diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp index c3f5e199a..5b65dc2e9 100644 --- a/test/gtest_IMU_tools_Lie.cpp +++ b/test/gtest_IMU_tools_Lie.cpp @@ -153,24 +153,40 @@ TEST(IMU_tools, lift_retract) ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10); } -TEST(IMU_tools, plus) +TEST(IMU_tools, plus_minus) { - VectorXs delta1(11), delta2(11), delta3(11); - VectorXs err(10); - Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized(); - delta1 << 0, 1, 2, qv, 7, 8, 9, 0.1; + VectorXs delta1(11), delta2(11), delta2_eq(11); + VectorXs err(10), err_0(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; err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1; - delta3.head(3) = delta1.head(3) + err.head(3); - delta3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs(); - delta3.tail(3) = delta1.tail(3) + err.tail(3); - - plus(delta1, err, delta2); - ASSERT_MATRIX_APPROX(diff(delta3, delta2), VectorXs::Zero(10), 1e-10); + delta2_eq = plus(delta1, diff(delta1, delta2)); + ASSERT_MATRIX_APPROX(delta2_eq, delta2, 1e-10); } +// TEST(IMU_tools, plus) +// { + // FORSTER's formulas -> TODO remove? +// VectorXs delta1(11), delta2(11), delta3(11); +// VectorXs err(10); +// Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized(); +// delta1 << 0, 1, 2, qv, 7, 8, 9, 0.1; +// err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1; + +// delta3.head(3) = delta1.head(3) + err.head(3); +// delta3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs(); +// delta3.tail(3) = delta1.tail(3) + err.tail(3); + +// plus(delta1, err, delta2); +// ASSERT_MATRIX_APPROX(diff(delta3, delta2), VectorXs::Zero(10), 1e-10); +// } + // TEST(IMU_tools, diff) // { + // FORSTER's formulas -> TODO remove? // VectorXs delta1(10), delta2(10); // Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized(); // delta1 << 0, 1, 2, qv, 7, 8, 9; @@ -188,9 +204,11 @@ TEST(IMU_tools, plus) // err.tail(3) = delta3.tail(3) - delta1.tail(3); // ASSERT_MATRIX_APPROX(err, diff(delta1, delta3), 1e-10); - // } + + + // TEST(IMU_tools, compose_jacobians) // { // VectorXs delta1(10), delta2(10), delta3(10), delta1_pert(10), delta2_pert(10), delta3_pert(10); // deltas -- GitLab