diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 3c61fae2fe1f3359804013ac17d1144353b3c488..364acf55b784c180b5840d75c3f08577bdb7101a 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -45,6 +45,89 @@ namespace wolf
namespace imu_with_dt {
using namespace Eigen;
+template<typename D>
+inline Matrix<typename D::Scalar, 5, 5> hat(const MatrixBase<D>& tau)
+{
+ // tagent space element vector form to matrix form
+ MatrixSizeCheck<10,1>::check(tau);
+ typedef typename D::Scalar T;
+ Map<const Matrix<T, 3, 1> > dp ( & tau( 0 ) );
+ Map<const Matrix<T, 3, 1> > doo( & tau( 3 ) );
+ Map<const Matrix<T, 3, 1> > dv ( & tau( 6 ) );
+ T& dt = tau( 9 );
+ Matrix<T, 5, 5> ret;
+
+ ret.setZero();
+ ret.block(0,0,3,3) = skew(doo);
+ ret.block(0,3,3,1) = dv;
+ ret.block(0,4,3,1) = dp;
+ ret.block(1,4,3,1) = dt;
+
+ return ret;
+}
+
+
+
+
+
+template<typename D1, typename D2>
+inline void adjoint(const MatrixBase<D1>& delta, MatrixBase<D2>& adjd)
+{
+ MatrixSizeCheck<11, 1>::check(delta);
+ MatrixSizeCheck<10, 10>::check(adjd);
+
+ 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 (in paper: pvqt)
+ adjd.setIdentity();
+ 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>
+inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
+ Matrix<typename D::Scalar, 10, 10> adjd;
+ adjoint(delta, adjd);
+ return adjd;
+}
+
+
+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 sadjd;
+}
+
template<typename D1, typename D2, typename D3, typename T>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v, T& dt)
{
@@ -182,65 +265,6 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
return ret;
}
-
-template<typename D1, typename D2>
-inline void adjoint(const MatrixBase<D1>& delta, MatrixBase<D2>& adjd)
-{
- MatrixSizeCheck<11, 1>::check(delta);
- MatrixSizeCheck<10, 10>::check(adjd);
-
- 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 (in paper: pvqt)
- adjd.setIdentity();
- 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>
-inline Matrix<typename D::Scalar, 10, 10> adjoint(const MatrixBase<D>& delta){
- Matrix<typename D::Scalar, 10, 10> adjd;
- adjoint(delta, adjd);
- return adjd;
-}
-
-
-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 sadjd;
-}
-
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& delta1,
const MatrixBase<D2>& delta2,
@@ -465,6 +489,7 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
Matrix<T, 11, 1> ret;
+ // TODO: solve strange compilation error
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) );
diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp
index 3901f067489cf825986b7563c745f81366be0186..e196bae4a14fa73442958231d74c84c6338841cd 100644
--- a/test/gtest_IMU_tools_Lie.cpp
+++ b/test/gtest_IMU_tools_Lie.cpp
@@ -153,7 +153,7 @@ TEST(IMU_tools, lift_retract)
ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
}
-TEST(IMU_tools, plus_minus)
+TEST(IMU_tools, plus_diff)
{
VectorXs delta1(11), delta2(11), delta2_eq(11);
VectorXs err(10), err_0(10);
@@ -174,10 +174,24 @@ TEST(IMU_tools, adjoint)
delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
+ // From the definition
+
+ // From direct properties
ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10);
ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10);
}
+TEST(IMU_tools, smalladjoint)
+{
+ VectorXs delta1(11), delta2(11);
+ 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;
+
+ // From the definition
+
+}
// TEST(IMU_tools, plus)
// {
@@ -218,9 +232,6 @@ TEST(IMU_tools, adjoint)
// ASSERT_MATRIX_APPROX(err, diff(delta1, delta3), 1e-10);
// }
-
-
-
TEST(IMU_tools, compose_jacobians)
{
VectorXs delta1(11), delta2(11), delta3(11); // deltas
@@ -238,6 +249,19 @@ TEST(IMU_tools, compose_jacobians)
// analytical jacobians
compose(delta1, delta2, delta3, J_comp_delta1, J_comp_delta2);
+
+ J_comp_delta1.setIdentity();
+ J_comp_delta2.setIdentity();
+
+
+ // std::cout << plus(delta2, tau) << std::endl;
+ auto tautau = J_comp_delta1*tau;
+ std::cout << tau << std::endl;
+ std::cout << tautau << std::endl;
+ std::cout << plus(delta2, tau) << std::endl; // No compilation error
+ // std::cout << plus(delta2, tautau) << std::endl; // COMPILATION ERROR -> TODO
+ // std::cout << plus(delta1, J_comp_delta1*tau) << std::endl;
+
// 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);
@@ -245,6 +269,7 @@ TEST(IMU_tools, compose_jacobians)
TEST(IMU_tools, diff_jacobians)
{
+ // !!! WRONG JACOBIANS but still works... Bad test?
VectorXs delta1(11), delta2(11), err(10); // deltas and err
VectorXs tau(10); // small tangent space element
tau.setOnes();
@@ -261,9 +286,9 @@ TEST(IMU_tools, diff_jacobians)
// analytical jacobians
diff(delta1, delta2, err, J_diff_delta1, J_diff_delta2);
- // 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);
+ // check that numerical and analytical match -> WRONG Checksize
+ ASSERT_MATRIX_APPROX(diff(plus(delta1, tau), delta2), diff(delta1, delta2) + J_diff_delta1*tau, 1e-4);
+ ASSERT_MATRIX_APPROX(diff(delta1, plus(delta2, tau)), diff(delta1, delta2) + J_diff_delta2*tau, 1e-4);
}
// TEST(IMU_tools, body2delta_jacobians)