diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h index f0297c3375350c3eb950a9fdc659febe107a96b0..bfe4df3b0f77c0b11895bb21948bfc9eeb4e635c 100644 --- a/include/IMU/math/IMU_tools_Lie.h +++ b/include/IMU/math/IMU_tools_Lie.h @@ -179,22 +179,22 @@ inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, co } template<typename D1, typename D2, typename D3> -inline void compose(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, +inline void compose(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2, MatrixBase<D3>& sum) { - MatrixSizeCheck<11, 1>::check(d1); - MatrixSizeCheck<11, 1>::check(d2); + MatrixSizeCheck<11, 1>::check(delta1); + MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<11, 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 ) ); - const typename D1::Scalar& dt1 = d1(10); - 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 ) ); - const typename D2::Scalar& dt2 = d2(10); + Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1( 0 ) ); + Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1( 3 ) ); + Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1( 7 ) ); + const typename D1::Scalar& dt1 = delta1(10); + Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2( 0 ) ); + Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2( 3 ) ); + Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2( 7 ) ); + const typename D2::Scalar& dt2 = delta2(10); 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 ) ); @@ -204,36 +204,36 @@ inline void compose(const MatrixBase<D1>& d1, } template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2) +inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2) { Matrix<typename D1::Scalar, 11, 1> ret; - compose(d1, d2, ret); + compose(delta1, delta2, ret); return ret; } template<typename D1, typename D2, typename D3, typename D4, typename D5> -inline void compose(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, +inline void compose(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2, MatrixBase<D3>& sum, - MatrixBase<D4>& J_sum_d1, - MatrixBase<D5>& J_sum_d2) + MatrixBase<D4>& J_sum_delta1, + MatrixBase<D5>& J_sum_delta2) { // Same as imu_tools.h - MatrixSizeCheck<11, 1>::check(d1); - MatrixSizeCheck<11, 1>::check(d2); + MatrixSizeCheck<11, 1>::check(delta1); + MatrixSizeCheck<11, 1>::check(delta2); MatrixSizeCheck<11, 1>::check(sum); - MatrixSizeCheck< 10, 10>::check(J_sum_d1); - MatrixSizeCheck< 10, 10>::check(J_sum_d2); + MatrixSizeCheck< 10, 10>::check(J_sum_delta1); + MatrixSizeCheck< 10, 10>::check(J_sum_delta2); // Jac wrt first delta -> adjoint inverse - J_sum_d1 = adjoint(inverse(d1)); + J_sum_delta1 = adjoint(inverse(delta1)); // Jac wrt second delta -> indentity - J_sum_d2.setIdentity(); // + J_sum_delta2.setIdentity(); // - // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable - compose(d1, d2, sum); + // compose deltas -- done here to avoid aliasing when calling with input `delta1` and result `sum` referencing the same variable + compose(delta1, delta2, sum); } template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename T> @@ -256,36 +256,36 @@ inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, co } template<typename D1, typename D2, typename D3> -inline void between(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2, - MatrixBase<D3>& d2_minus_d1) +inline void between(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2, + MatrixBase<D3>& delta2_minus_delta1) { - MatrixSizeCheck<11, 1>::check(d1); - MatrixSizeCheck<11, 1>::check(d2); - MatrixSizeCheck<11, 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) ); - const typename D1::Scalar& dt1 = d1(10); - 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) ); - const typename D2::Scalar& dt2 = d2(10); - 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) ); - typename D3::Scalar& diff_dt = d2_minus_d1(10); + MatrixSizeCheck<11, 1>::check(delta1); + MatrixSizeCheck<11, 1>::check(delta2); + MatrixSizeCheck<11, 1>::check(delta2_minus_delta1); + + Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1(0) ); + Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1(3) ); + Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1(7) ); + const typename D1::Scalar& dt1 = delta1(10); + Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2(0) ); + Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2(3) ); + Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2(7) ); + const typename D2::Scalar& dt2 = delta2(10); + Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & delta2_minus_delta1(0) ); + Map<Quaternion<typename D3::Scalar> > diff_q ( & delta2_minus_delta1(3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & delta2_minus_delta1(7) ); + typename D3::Scalar& diff_dt = delta2_minus_delta1(10); between(dp1, dq1, dv1, dt1, dp2, dq2, dv2, dt2, diff_p, diff_q, diff_v, diff_dt); } template<typename D1, typename D2> -inline Matrix<typename D1::Scalar, 11, 1> between(const MatrixBase<D1>& d1, - const MatrixBase<D2>& d2) +inline Matrix<typename D1::Scalar, 11, 1> between(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2) { Matrix<typename D1::Scalar, 11, 1> diff; - between(d1, d2, diff); + between(delta1, delta2, diff); return diff; } @@ -470,42 +470,37 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta -// 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 ) +// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename T> +// inline void plus(const MatrixBase<D1>& delta_p1, const QuaternionBase<D2>& delta_q1, const MatrixBase<D3>& delta_v1, const T& delta_t1, +// const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2, const T& dt1, +// MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v, T& plus_dt) // { -// plus_p = dp1 + dp2; -// plus_v = dv1 + dv2; -// plus_q = dq1 * exp_q(do2); +// compose() +// // plus_p = dp1 + dp2; +// // plus_v = dv1 + dv2; +// // plus_q = dq1 * exp_q(do2); // } -// 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, typename D3> +inline void plus(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2, + MatrixBase<D3>& delta_pert) +{ + MatrixSizeCheck<11, 1>::check(delta1); + MatrixSizeCheck<10, 1>::check(delta2); + MatrixSizeCheck<11, 1>::check(delta_pert); -// 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; -// } + compose(delta1, exp_IMU(delta2), delta_pert); +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2) +{ + Matrix<typename D1::Scalar, 10, 1> ret; + plus(delta1, delta2, 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, @@ -529,37 +524,40 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta // J_do_dq2 = jac_SO3_right_inv(diff_o); // } -// template<typename D1, typename D2, typename D3> -// inline void diff(const MatrixBase<D1>& d1, -// const MatrixBase<D2>& d2, -// MatrixBase<D3>& err) -// { -// 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) ); +template<typename D1, typename D2, typename D3> +inline void diff(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2, + MatrixBase<D3>& err) +{ + MatrixSizeCheck<11, 1>::check(delta1); + MatrixSizeCheck<11, 1>::check(delta2); + MatrixSizeCheck<10, 1>::check(err); -// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v); -// } + err = log_IMU(compose(inverse(delta1), delta2)); +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 10, 1> diff(const MatrixBase<D1>& delta1, + const MatrixBase<D2>& delta2) +{ + Matrix<typename D1::Scalar, 10, 1> ret; + diff(delta1, delta2, ret); + return ret; +} // template<typename D1, typename D2, typename D3, typename D4, typename D5> -// inline void diff(const MatrixBase<D1>& d1, -// const MatrixBase<D2>& d2, +// inline void diff(const MatrixBase<D1>& delta1, +// const MatrixBase<D2>& delta2, // MatrixBase<D3>& dif, -// MatrixBase<D4>& J_diff_d1, -// MatrixBase<D5>& J_diff_d2) +// MatrixBase<D4>& J_diff_delta1, +// MatrixBase<D5>& J_diff_delta2) // { -// 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<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & delta1(0) ); +// Map<const Quaternion<typename D1::Scalar> > dq1 ( & delta1(3) ); +// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & delta1(7) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & delta2(0) ); +// Map<const Quaternion<typename D2::Scalar> > dq2 ( & delta2(3) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & delta2(7) ); // Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) ); // Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) ); // Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) ); @@ -568,7 +566,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta // diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2); -// /* d = diff(d1, d2) is +// /* d = diff(delta1, delta2) is // * dp = dp2 - dp1 // * do = Log(dq1.conj * dq2) // * dv = dv2 - dv1 @@ -578,20 +576,11 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta // * J_do_dq2 = J_r_inv(theta) // */ -// J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity -// J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta) - -// J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity -// J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta) -// } +// J_diff_delta1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity +// J_diff_delta1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta) -// template<typename D1, typename D2> -// inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1, -// const MatrixBase<D2>& d2) -// { -// Matrix<typename D1::Scalar, 9, 1> ret; -// diff(d1, d2, ret); -// return ret; +// J_diff_delta2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity +// J_diff_delta2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta) // } // template<typename D1, typename D2, typename D3, typename D4, typename D5> diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp index 97fdfebb271e675fdf7dcb04b49089b9d1d0f6f1..c3f5e199a449ec12d4a6c07d2479813178ee4b26 100644 --- a/test/gtest_IMU_tools_Lie.cpp +++ b/test/gtest_IMU_tools_Lie.cpp @@ -67,40 +67,40 @@ TEST(IMU_tools, inverse) TEST(IMU_tools, compose_between) { VectorXs dx1(11), dx2(11), dx3(11); - Matrix<Scalar, 11, 1> d1, d2, d3; + Matrix<Scalar, 11, 1> delta1, delta2, delta3; Vector4s qv; Scalar dt = 0.1; qv = (Vector4s() << 3, 4, 5, 6).finished().normalized(); dx1 << 0, 1, 2, qv, 7, 8, 9, dt; - d1 = dx1; + delta1 = dx1; qv = (Vector4s() << 6, 5, 4, 3).finished().normalized(); dx2 << 9, 8, 7, qv, 2, 1, 0, dt; - d2 = dx2; + delta2 = dx2; // combinations of templates for sum() compose(dx1, dx2, dx3); - compose(d1, d2, d3); - ASSERT_MATRIX_APPROX(d3, dx3, 1e-10); // ?? compare results from the same fonction? + compose(delta1, delta2, delta3); + ASSERT_MATRIX_APPROX(delta3, dx3, 1e-10); // ?? compare results from the same fonction? - compose(d1, dx2, dx3); - d3 = compose(dx1, d2); - ASSERT_MATRIX_APPROX(d3, dx3, 1e-10); + compose(delta1, dx2, dx3); + delta3 = compose(dx1, delta2); + ASSERT_MATRIX_APPROX(delta3, dx3, 1e-10); - // minus(d1, d3) should recover delta_2 + // minus(delta1, delta3) should recover delta_2 VectorXs diffx(11); Matrix<Scalar,11,1> diff; - between(d1, d3, diff); - ASSERT_MATRIX_APPROX(diff, d2, 1e-10); + between(delta1, delta3, diff); + ASSERT_MATRIX_APPROX(diff, delta2, 1e-10); - // minus(d3, d1) should recover inverse(d2) - diff = between(d3, d1); - ASSERT_MATRIX_APPROX(diff, inverse(d2), 1e-10); + // minus(delta3, delta1) should recover inverse(delta2) + diff = between(delta3, delta1); + ASSERT_MATRIX_APPROX(diff, inverse(delta2), 1e-10); } TEST(IMU_tools, compose_between_with_state) { - VectorXs x(10), d(11), x2(10), x3(10), d2(11), d3(11); + VectorXs x(10), d(11), x2(10), x3(10), delta2(11), delta3(11); Vector4s qv; Scalar dt = 0.1; @@ -114,10 +114,10 @@ TEST(IMU_tools, compose_between_with_state) ASSERT_MATRIX_APPROX(x3, x2, 1e-10); // betweenStates(x, x2) should recover d - betweenStates(x, x2, dt, d2); - d3 = betweenStates(x, x2, dt); - ASSERT_MATRIX_APPROX(d2, d, 1e-10); - ASSERT_MATRIX_APPROX(d3, d, 1e-10); + betweenStates(x, x2, dt, delta2); + delta3 = betweenStates(x, x2, dt); + ASSERT_MATRIX_APPROX(delta2, d, 1e-10); + ASSERT_MATRIX_APPROX(delta3, d, 1e-10); ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10); // x + (x2 - x) = x2 @@ -153,59 +153,59 @@ TEST(IMU_tools, lift_retract) ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10); } -// TEST(IMU_tools, plus) -// { -// VectorXs d1(10), d2(10), d3(10); -// VectorXs err(9); -// Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized(); -// d1 << 0, 1, 2, qv, 7, 8, 9; -// err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09; - -// d3.head(3) = d1.head(3) + err.head(3); -// d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs(); -// d3.tail(3) = d1.tail(3) + err.tail(3); - -// plus(d1, err, d2); -// ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10); -// } +TEST(IMU_tools, plus) +{ + 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) // { -// VectorXs d1(10), d2(10); +// VectorXs delta1(10), delta2(10); // Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized(); -// d1 << 0, 1, 2, qv, 7, 8, 9; -// d2 = d1; +// delta1 << 0, 1, 2, qv, 7, 8, 9; +// delta2 = delta1; // VectorXs err(9); -// diff(d1, d2, err); +// diff(delta1, delta2, err); // ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10); -// ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10); +// ASSERT_MATRIX_APPROX(diff(delta1, delta2), VectorXs::Zero(9), 1e-10); -// VectorXs d3(10); -// d3.setRandom(); d3.segment(3,4).normalize(); -// err.head(3) = d3.head(3) - d1.head(3); -// err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3)); -// err.tail(3) = d3.tail(3) - d1.tail(3); +// VectorXs delta3(10); +// delta3.setRandom(); delta3.segment(3,4).normalize(); +// err.head(3) = delta3.head(3) - delta1.head(3); +// err.segment(3,3) = log_q(Quaternions(delta1.data()+3).conjugate()*Quaternions(delta3.data()+3)); +// err.tail(3) = delta3.tail(3) - delta1.tail(3); -// ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10); +// ASSERT_MATRIX_APPROX(err, diff(delta1, delta3), 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 delta1(10), delta2(10), delta3(10), delta1_pert(10), delta2_pert(10), delta3_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; +// delta1 << 0, 1, 2, qv1, 7, 8, 9; // qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized(); -// d2 << 9, 8, 7, qv2, 2, 1, 0; +// delta2 << 9, 8, 7, qv2, 2, 1, 0; // // analytical jacobians -// compose(d1, d2, dt, d3, J1_a, J2_a); +// compose(delta1, delta2, dt, delta3, J1_a, J2_a); // // numerical jacobians // for (unsigned int i = 0; i<9; i++) @@ -214,16 +214,16 @@ TEST(IMU_tools, lift_retract) // 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 +// delta1_pert = plus(delta1, t1); // (delta1 + t1) +// delta3_pert = compose(delta1_pert, delta2, dt); // (delta1 + t1) + delta2 +// t2 = diff(delta3, delta3_pert); // { (delta2 + t1) + delta2 } - { delta1 + delta2 } +// J1_n.col(i) = t2/dx; // [ { (delta2 + t1) + delta2 } - { delta1 + delta2 } ] / 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 +// delta2_pert = plus(delta2, t1); // (delta2 + t1) +// delta3_pert = compose(delta1, delta2_pert, dt); // delta1 + (delta2 + t1) +// t2 = diff(delta3, delta3_pert); // { delta1 + (delta2 + t1) } - { delta1 + delta2 } +// J2_n.col(i) = t2/dx; // [ { delta1 + (delta2 + t1) } - { delta1 + delta2 } ] / dx // } // // check that numerical and analytical match @@ -233,18 +233,18 @@ TEST(IMU_tools, lift_retract) // TEST(IMU_tools, diff_jacobians) // { -// VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas +// VectorXs delta1(10), delta2(10), delta3(9), delta1_pert(10), delta2_pert(10), delta3_pert(9); // deltas // VectorXs t1(9), t2(9); // tangents // Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n; // Vector4s qv1, qv2; // Scalar dx = 1e-6; // qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized(); -// d1 << 0, 1, 2, qv1, 7, 8, 9; +// delta1 << 0, 1, 2, qv1, 7, 8, 9; // qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized(); -// d2 << 9, 8, 7, qv2, 2, 1, 0; +// delta2 << 9, 8, 7, qv2, 2, 1, 0; // // analytical jacobians -// diff(d1, d2, d3, J1_a, J2_a); +// diff(delta1, delta2, delta3, J1_a, J2_a); // // numerical jacobians // for (unsigned int i = 0; i<9; i++) @@ -253,16 +253,16 @@ TEST(IMU_tools, lift_retract) // t1(i) = dx; // // Jac wrt first delta -// d1_pert = plus(d1, t1); // (d1 + t1) -// d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1) -// t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 } -// J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx +// delta1_pert = plus(delta1, t1); // (delta1 + t1) +// delta3_pert = diff(delta1_pert, delta2); // delta2 - (delta1 + t1) +// t2 = delta3_pert - delta3; // { delta2 - (delta1 + t1) } - { delta2 - delta1 } +// J1_n.col(i) = t2/dx; // [ { delta2 - (delta1 + t1) } - { delta2 - delta1 } ] / dx // // Jac wrt second delta -// d2_pert = plus(d2, t1); // (d2 + t1) -// d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1 -// t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 } -// J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx +// delta2_pert = plus(delta2, t1); // (delta2 + t1) +// delta3_pert = diff(delta1, delta2_pert); // (delta2 + t1) - delta1 +// t2 = delta3_pert - delta3; // { (delta2 + t1) - delta1 } - { delta2 - delta1 } +// J2_n.col(i) = t2/dx; // [ { (delta2 + t1) - delta1 } - { delta2 - delta1 } ] / dx // } // // check that numerical and analytical match