diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 364acf55b784c180b5840d75c3f08577bdb7101a..58d9618bfaada339c635da71d59323f92b15b6db 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -45,89 +45,6 @@ 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)
{
@@ -265,6 +182,116 @@ 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>& d, MatrixBase<D2>& adjd)
+{
+ MatrixSizeCheck<11, 1>::check(d);
+ 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);
+
+ // pqvt impl
+ 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;
+}
+
+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 D>
+inline Matrix<typename D::Scalar, 5, 5> hat(const MatrixBase<D> tau)
+{
+ MatrixSizeCheck<10, 1>::check(tau);
+
+ typedef typename D::Scalar T;
+
+
+ Map<const Matrix<T, 3, 1> > p ( & tau(0) );
+ Map<const Matrix<T, 3, 1> > th ( & tau(3) );
+ Map<const Matrix<T, 3, 1> > v ( & tau(6) );
+ const T& t = tau(9);
+
+ Matrix<T, 5, 5> taux;
+ taux.setZero();
+
+ taux.block<3,3>(0,0) = skew(th);
+ taux.block<3,1>(0,3) = v;
+ taux.block<3,1>(0,4) = p;
+ taux.block<3,1>(3,4) = t;
+
+ return taux;
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 1> vee(const MatrixBase<D> taux)
+{
+ MatrixSizeCheck<5, 5>::check(taux);
+
+ Matrix<typename D::Scalar, 5, 5> tau;
+
+ tau.segment<3>(0) = taux.block<3,1>(0,4); // p
+ tau.segment<3>(3) = vee(taux.block<3,3>(0,0)); // th
+ tau.segment<3>(6) = taux.block<3,1>(0,3); // v
+ tau (9) = taux (3,4); // t
+
+ return tau;
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 10, 10> smalladjoint(const MatrixBase<D> tau)
+{
+ MatrixSizeCheck<10, 1>::check(tau);
+
+ typedef typename D::Scalar T;
+
+ Map<const Matrix<T, 3, 1> > p ( & tau(0) );
+ Map<const Matrix<T, 3, 1> > th ( & tau(3) );
+ Map<const Matrix<T, 3, 1> > v ( & tau(6) );
+ const T& t = tau(9);
+
+ // Adjoint matrix associated to the adjoint operator
+ Matrix<T, 3, 3> px = skew(p);
+ Matrix<T, 3, 3> thx = skew(th);
+ Matrix<T, 3, 3> vx = skew(v);
+
+ Matrix<T, 10, 10> adj;
+ adj.setZero();
+
+ /* Attention to variable order in the adjoint!
+ *
+ * Paper: PVOT = 0639
+ * WOLF: POVT = 0369
+ *
+ */
+
+ adj.block<3,3>(0,0) = thx; // 0,0 in paper
+ adj.block<3,3>(0,3) = px; // 0,6 in paper
+ adj.block<3,3>(0,6) = - Matrix<T,3,3>::Identity() * t; // 0,3 in paper
+ adj.block<3,1>(0,9) = v; // 0,9 in paper
+ adj.block<3,3>(3,3) = thx; // 6,6 in paper
+ adj.block<3,3>(6,3) = vx; // 3,6 in paper
+ adj.block<3,3>(6,6) = thx; // 3,3 in paper
+
+ return adj;
+}
+
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& delta1,
const MatrixBase<D2>& delta2,
@@ -472,7 +499,7 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
Q = Id
+ ((1 - cos_thn)/thn)*ux
- + ((thn - sin_thn)/thn2)*(ux2);
+ + ((thn - sin_thn)/thn)*(ux2);
P = 0.5*Id
+ ((thn - sin_thn)/thn2)*ux
@@ -487,17 +514,18 @@ 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;
- // 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) );
- 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);
+ const T& dt_in = d_in(9);
+
+ Matrix<T, 11, 1> ret;
+
+ 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;
diff --git a/test/gtest_IMU_tools_Lie.cpp b/test/gtest_IMU_tools_Lie.cpp
index e196bae4a14fa73442958231d74c84c6338841cd..f38cb0e49b401bbc3861634d91b74368f7c3be0a 100644
--- a/test/gtest_IMU_tools_Lie.cpp
+++ b/test/gtest_IMU_tools_Lie.cpp
@@ -169,16 +169,24 @@ TEST(IMU_tools, plus_diff)
TEST(IMU_tools, adjoint)
{
VectorXs delta1(11), delta2(11);
+ VectorXs tau(10);
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;
+ tau << .1, .2, .3, -.1, -.2, -.3, .3, .2, .1, 0.1;
// 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);
+
+ MatrixXs tau2 = adjoint(delta1)*tau;
+// MatrixXs d3 = compose(delta1, exp_IMU(tau));
+// MatrixXs d4 = compose(exp_IMU(tau2),delta1);
+ ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau)), compose(exp_IMU(tau2),delta1), 1e-10);
+// ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau2)), compose(exp_IMU(tau),delta1), 1e-10);
}
TEST(IMU_tools, smalladjoint)