diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 6ea135922cdb919ef1c0927f162359328dcfc13e..3c61fae2fe1f3359804013ac17d1144353b3c488 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -184,27 +184,26 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
template<typename D1, typename D2>
-inline void adjoint(const MatrixBase<D1>& d, MatrixBase<D2>& adjd)
+inline void adjoint(const MatrixBase<D1>& delta, MatrixBase<D2>& adjd)
{
- MatrixSizeCheck<11, 1>::check(d);
+ MatrixSizeCheck<11, 1>::check(delta);
MatrixSizeCheck<10, 10>::check(adjd);
- // Adjoint matrix associated to the adjoint operator
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( &d(3) );
- Matrix<typename D1::Scalar, 3, 3> dR = q2R(dq);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 7 ) );
- const typename D1::Scalar& dt = d(10);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > deltap ( & delta( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > deltaq ( & delta(3) );
+ Matrix<typename D1::Scalar, 3, 3> deltaR = q2R(deltaq);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > deltav ( & delta( 7 ) );
+ const typename D1::Scalar& deltat = delta(10);
- // pqvt impl
+ // pqvt impl (in paper: pvqt)
adjd.setIdentity();
- adjd.block(0,0,3,3) = dR;
- adjd.block(0,3,3,3) = skew(dp - dv * dt) * dR;
- adjd.block(0,6,3,3) = -dR * dt;
- adjd.block(0,9,3,1) = dv;
- adjd.block(3,3,3,3) = dR;
- adjd.block(6,3,3,3) = skew(dv) * dR;
- adjd.block(6,6,3,3) = dR;
+ adjd.block(0,0,3,3) = deltaR;
+ adjd.block(0,3,3,3) = skew(deltap - deltav * deltat) * deltaR;
+ adjd.block(0,6,3,3) = -deltaR * deltat;
+ adjd.block(0,9,3,1) = deltav;
+ adjd.block(3,3,3,3) = deltaR;
+ adjd.block(6,3,3,3) = skew(deltav) * deltaR;
+ adjd.block(6,6,3,3) = deltaR;
}
template<typename D>
@@ -215,14 +214,32 @@ inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
}
-// template<typename D1, typename D2>
-// inline Matrix<T, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
-// {
-// // Adjoint matrix associated to the adjoint operator
-// TODO
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
+{
+ MatrixSizeCheck<10, 1>::check(d);
+ MatrixSizeCheck<10, 10>::check(sadjd);
+
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > doo ( &d( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 6 ) );
+ const typename D1::Scalar& dt = d( 9 );
+
+ const Matrix<typename D1::Scalar, 3, 3> Id3 = Matrix<typename D1::Scalar, 3, 3>::identity(3,3);
+ const Matrix<typename D1::Scalar, 3, 3> dox = skew(doo);
+
+ // pqvt impl (in paper: pvqt)
+ sadjd.setIdentity();
+ sadjd.block(0,0,3,3) = dox;
+ sadjd.block(0,3,3,3) = skew(dp);
+ sadjd.block(0,6,3,3) = -Id3*dt;
+ sadjd.block(0,9,3,1) = dv;
+ sadjd.block(3,3,3,3) = dox;
+ sadjd.block(6,3,3,3) = skew(dv);
+ sadjd.block(6,6,3,3) = dox;
-// return ret;
-// }
+ return sadjd;
+}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& delta1,
@@ -444,29 +461,27 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<10, 1>::check(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);
- Map<Matrix<T, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<T> > dq ( & ret(3) );
- Map<Matrix<T, 3, 1> > dv ( & ret(7) );
- T& dt = ret(10);
+ const T& dt_in = d_in(9);
+ Map<Matrix<T, 3, 1> > dp_ret ( & ret(0) );
+ Map<Quaternion<T> > dq_ret ( & ret(3) );
+ Map<Matrix<T, 3, 1> > dv_ret ( & ret(7) );
+ T& dt_ret = ret(10);
Matrix<T, 3, 3> Q;
Matrix<T, 3, 3> P;
QandPmat(do_in, Q, P);
- dp = Q*dp_in + P*dv_in*dt_in;
- dv = Q*dv_in;
- dq = exp_q(do_in);
- dt = dt_in;
+ dp_ret = Q*dp_in + P*dv_in*dt_in;
+ dv_ret = Q*dv_in;
+ dq_ret = exp_q(do_in);
+ dt_ret = dt_in;
return ret;
}
@@ -497,7 +512,6 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
dv_ret = Qinv*dv_in;
dt_ret = dt_in;
-// std::cout << "Log ret" << ret.transpose() << std::endl;
return ret;
}
@@ -508,7 +522,7 @@ Matrix<typename Derived::Scalar, 10, 1> log_IMU(const MatrixBase<Derived>& delta
// 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)
// {
-// compose()
+ // TODO ?
// // plus_p = dp1 + dp2;
// // plus_v = dv1 + dv2;
// // plus_q = dq1 * exp_q(do2);
@@ -579,43 +593,27 @@ inline Matrix<typename D1::Scalar, 10, 1> diff(const MatrixBase<D1>& delta1,
return ret;
}
-// template<typename D1, typename D2, typename D3, typename D4, typename D5>
-// inline void diff(const MatrixBase<D1>& delta1,
-// const MatrixBase<D2>& delta2,
-// MatrixBase<D3>& dif,
-// MatrixBase<D4>& J_diff_delta1,
-// MatrixBase<D5>& J_diff_delta2)
-// {
-// 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) );
-
-// Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
-
-// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
-
-// /* d = diff(delta1, delta2) is
-// * dp = dp2 - dp1
-// * do = Log(dq1.conj * dq2)
-// * dv = dv2 - dv1
-// *
-// * With trivial Jacobians for dp and dv, and:
-// * J_do_dq1 = - J_l_inv(theta)
-// * J_do_dq2 = 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)
-
-// 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>
+inline void diff(const MatrixBase<D1>& delta1,
+ const MatrixBase<D2>& delta2,
+ MatrixBase<D3>& err,
+ MatrixBase<D4>& J_diff_delta1,
+ MatrixBase<D5>& J_diff_delta2)
+{
+ MatrixSizeCheck<11, 1>::check(delta1);
+ MatrixSizeCheck<11, 1>::check(delta2);
+ MatrixSizeCheck<10, 1>::check(err);
+ MatrixSizeCheck<10, 10>::check(J_diff_delta1);
+ MatrixSizeCheck<10, 10>::check(J_diff_delta2);
+
+ diff(delta1, delta2, err);
+
+ Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+
+ J_diff_delta1 = - Matrix<typename D4::Scalar, 10, 10>::Identity();
+
+ J_diff_delta2 = adjoint(inverse(delta1)) * adjoint(delta2);
+}
// template<typename D1, typename D2, typename D3, typename D4, typename D5>
// inline void body2delta(const MatrixBase<D1>& a,
diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp
index 2b6fa4653f54d107ec01e755f3cadbb20201de2b..3901f067489cf825986b7563c745f81366be0186 100644
--- a/test/gtest_IMU_tools_Lie.cpp
+++ b/test/gtest_IMU_tools_Lie.cpp
@@ -221,84 +221,50 @@ TEST(IMU_tools, adjoint)
-// TEST(IMU_tools, compose_jacobians)
-// {
-// 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();
-// delta1 << 0, 1, 2, qv1, 7, 8, 9;
-// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
-// delta2 << 9, 8, 7, qv2, 2, 1, 0;
-
-// // analytical jacobians
-// compose(delta1, delta2, dt, delta3, J1_a, J2_a);
-
-// // numerical jacobians
-// for (unsigned int i = 0; i<9; i++)
-// {
-// t1 . setZero();
-// t1(i) = dx;
-
-// // Jac wrt first delta
-// 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
-// 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
-// }
+TEST(IMU_tools, compose_jacobians)
+{
+ VectorXs delta1(11), delta2(11), delta3(11); // deltas
+ VectorXs tau(10); // small tangent space element
+ tau.setOnes();
+ tau *= 1e-6;
+ Matrix<Scalar, 10, 10> J_comp_delta1, J_comp_delta2;
+ Vector4s qv1, qv2;
+ Scalar dt = 0.1;
+ qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ delta1 << 0, 1, 2, qv1, 7, 8, 9, dt;
+ qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+ delta2 << 9, 8, 7, qv2, 2, 1, 0, dt;
-// // check that numerical and analytical match
-// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
-// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-// }
+ // analytical jacobians
+ compose(delta1, delta2, delta3, J_comp_delta1, J_comp_delta2);
-// TEST(IMU_tools, diff_jacobians)
-// {
-// 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();
-// delta1 << 0, 1, 2, qv1, 7, 8, 9;
-// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
-// delta2 << 9, 8, 7, qv2, 2, 1, 0;
+ // That first order approximation holds TODO -> comp problem!
+ // ASSERT_MATRIX_APPROX(compose(plus(delta1, tau), delta2), plus(compose(delta1, delta2), J_comp_delta1*tau), 1e-4);
+ // ASSERT_MATRIX_APPROX(compose(delta1, plus(delta2, tau)), plus(compose(delta1, delta2), J_comp_delta2*tau), 1e-4);
+}
-// // analytical jacobians
-// diff(delta1, delta2, delta3, J1_a, J2_a);
+TEST(IMU_tools, diff_jacobians)
+{
+ VectorXs delta1(11), delta2(11), err(10); // deltas and err
+ VectorXs tau(10); // small tangent space element
+ tau.setOnes();
+ tau *= 1e-6;
+ Matrix<Scalar, 10, 10> J_diff_delta1, J_diff_delta2;
+ Vector4s qv1, qv2;
+ Scalar dt = 0.1;
+ qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
+ delta1 << 0, 1, 2, qv1, 7, 8, 9, dt;
+ qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
+ delta2 << 9, 8, 7, qv2, 2, 1, 0, dt;
-// // numerical jacobians
-// for (unsigned int i = 0; i<9; i++)
-// {
-// t1 . setZero();
-// t1(i) = dx;
-// // Jac wrt first delta
-// 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
-// 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
-// }
+ // analytical jacobians
+ diff(delta1, delta2, err, J_diff_delta1, J_diff_delta2);
-// // check that numerical and analytical match
-// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
-// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-// }
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(diff(plus(delta1, tau), delta2), plus(diff(delta1, delta2), J_diff_delta1*tau), 1e-4);
+ ASSERT_MATRIX_APPROX(diff(delta1, plus(delta2, tau)), plus(diff(delta1, delta2), J_diff_delta2*tau), 1e-4);
+}
// TEST(IMU_tools, body2delta_jacobians)
// {