From b068c5d3501f541813b0944d9021f6a5ff7b68f1 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 19:47:17 +0200
Subject: [PATCH] plus minus WIP
---
include/IMU/math/IMU_tools_Lie.h | 231 +++++++++++++++----------------
test/gtest_IMU_tools_Lie.cpp | 138 +++++++++---------
2 files changed, 179 insertions(+), 190 deletions(-)
diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index f0297c337..bfe4df3b0 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 97fdfebb2..c3f5e199a 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
--
GitLab