diff --git a/src/imu_tools.h b/src/imu_tools.h
index 9a5a83b736ae5b07a84f455c3a923500ad18e41c..a1831bde54104c142533510dbb64afca79f37a01 100644
--- a/src/imu_tools.h
+++ b/src/imu_tools.h
@@ -150,8 +150,8 @@ inline void compose(const MatrixBase<D1>& d1,
MatrixSizeCheck<10, 1>::check(d1);
MatrixSizeCheck<10, 1>::check(d2);
MatrixSizeCheck<10, 1>::check(sum);
- MatrixSizeCheck< 9, 1>::check(J_sum_d1);
- MatrixSizeCheck< 9, 1>::check(J_sum_d2);
+ MatrixSizeCheck< 9, 9>::check(J_sum_d1);
+ MatrixSizeCheck< 9, 9>::check(J_sum_d2);
// Maps over provided data
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
@@ -364,19 +364,79 @@ Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in)
return ret;
}
+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 )
+{
+ plus_p = dp1 + dp2;
+ plus_q = dq1 * exp_q(do2);
+ plus_v = dv1 + dv2;
+}
+
+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>
+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;
+}
+
+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,
+ const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+ MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v )
+{
+ diff_p = dp2 - dp1;
+ diff_o = log_q(dq1.conjugate() * dq2);
+ diff_v = dv2 - dv1;
+}
+
+
template<typename D1, typename D2, typename D3>
inline void diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& err)
{
- err = lift( between(d1, d2, 0.0) );
+ 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) );
+
+ diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
{
- return lift( between(d1, d2, 0.0) );
+ Matrix<typename D1::Scalar, 9, 1> ret;
+ diff(d1, d2, ret);
+ return ret;
}
diff --git a/src/test/gtest_imu_tools.cpp b/src/test/gtest_imu_tools.cpp
index 37be31e3bde637e1b55ea65e627d1605d263ae24..b44ddfebb8052a49957c2130bc2b9a6beced5cf4 100644
--- a/src/test/gtest_imu_tools.cpp
+++ b/src/test/gtest_imu_tools.cpp
@@ -12,6 +12,7 @@
using namespace Eigen;
using namespace wolf;
+using namespace imu;
TEST(IMU_tools, identity)
{
@@ -19,7 +20,7 @@ TEST(IMU_tools, identity)
id_true.setZero(10);
id_true(6) = 1.0;
- VectorXs id = imu::identity<Scalar>();
+ VectorXs id = identity<Scalar>();
ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
}
@@ -28,12 +29,12 @@ TEST(IMU_tools, identity_split)
VectorXs p(3), qv(4), v(3);
Quaternions q;
- imu::identity(p,q,v);
+ identity(p,q,v);
ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
- imu::identity(p,qv,v);
+ identity(p,qv,v);
ASSERT_MATRIX_APPROX(p , Vector3s::Zero(), 1e-10);
ASSERT_MATRIX_APPROX(qv, (Vector4s()<<0,0,0,1).finished(), 1e-10);
ASSERT_MATRIX_APPROX(v , Vector3s::Zero(), 1e-10);
@@ -48,16 +49,16 @@ TEST(IMU_tools, inverse)
qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
d << 0, 1, 2, qv, 7, 8, 9;
- id = imu::inverse(d, dt);
+ id = inverse(d, dt);
- imu::compose(id, d, dt, I);
- ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
- imu::compose(d, id, -dt, I); // Observe -dt is reversed !!
- ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
+ compose(id, d, dt, I);
+ ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
+ compose(d, id, -dt, I); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
- imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
+ inverse(id, -dt, iid); // Observe -dt is reversed !!
ASSERT_MATRIX_APPROX( d, iid, 1e-10);
- iiid = imu::inverse(iid, dt);
+ iiid = inverse(iid, dt);
ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
}
@@ -76,23 +77,23 @@ TEST(IMU_tools, compose_between)
d2 = dx2;
// combinations of templates for sum()
- imu::compose(dx1, dx2, dt, dx3);
- imu::compose(d1, d2, dt, d3);
+ compose(dx1, dx2, dt, dx3);
+ compose(d1, d2, dt, d3);
ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
- imu::compose(d1, dx2, dt, dx3);
- d3 = imu::compose(dx1, d2, dt);
+ compose(d1, dx2, dt, dx3);
+ d3 = compose(dx1, d2, dt);
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);
+ between(d1, d3, dt, diff);
ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
// minus(d3, d1, -dt) should recover inverse(d2, dt)
- diff = imu::between(d3, d1, -dt);
- ASSERT_MATRIX_APPROX(diff, imu::inverse(d2, dt), 1e-10);
+ diff = between(d3, d1, -dt);
+ ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
}
TEST(IMU_tools, compose_between_with_state)
@@ -106,22 +107,22 @@ TEST(IMU_tools, compose_between_with_state)
qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
d << 9, 8, 7, qv, 2, 1, 0;
- imu::composeOverState(x, d, dt, x2);
- x3 = imu::composeOverState(x, d, dt);
+ composeOverState(x, d, dt, x2);
+ x3 = composeOverState(x, d, dt);
ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
// betweenStates(x, x2) should recover d
- imu::betweenStates(x, x2, dt, d2);
- d3 = imu::betweenStates(x, x2, dt);
+ betweenStates(x, x2, dt, d2);
+ d3 = betweenStates(x, x2, dt);
ASSERT_MATRIX_APPROX(d2, d, 1e-10);
ASSERT_MATRIX_APPROX(d3, d, 1e-10);
- ASSERT_MATRIX_APPROX(imu::betweenStates(x, x2, dt), d, 1e-10);
+ ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10);
// x + (x2 - x) = x2
- ASSERT_MATRIX_APPROX(imu::composeOverState(x, imu::betweenStates(x, x2, dt), dt), x2, 1e-10);
+ ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10);
// (x + d) - x = d
- ASSERT_MATRIX_APPROX(imu::betweenStates(x, imu::composeOverState(x, d, dt), dt), d, 1e-10);
+ ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
}
TEST(IMU_tools, lift_retract)
@@ -129,7 +130,7 @@ 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);
+ VectorXs delta = retract(d_min);
// expected delta
Vector3s dp = d_min.head(3);
@@ -139,11 +140,11 @@ TEST(IMU_tools, lift_retract)
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);
+ VectorXs d_from_delta = 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);
+ VectorXs delta_from_d = retract(d_from_delta);
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
}
@@ -155,9 +156,49 @@ TEST(IMU_tools, diff)
d2 = d1;
VectorXs err(9);
- imu::diff(d1, d2, err);
+ diff(d1, d2, err);
ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
- ASSERT_MATRIX_APPROX(imu::diff(d1, d2), VectorXs::Zero(9), 1e-10);
+ ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 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 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;
+ qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+ d2 << 9, 8, 7, qv2, 2, 1, 0;
+
+ // analytical jacobians
+ compose(d1, d2, dt, d3, J1_a, J2_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<9; i++)
+ {
+ t1 . setZero();
+ 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
+
+ // 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
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
+ ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
}
int main(int argc, char **argv)