Skip to content
Snippets Groups Projects
Commit 3d29a70a authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Merge remote-tracking branch 'origin/imu_preintegration_Lie' into imu_preintegration_Lie

parents c2f650c3 d9ed52ea
No related tags found
No related merge requests found
...@@ -45,89 +45,6 @@ namespace wolf ...@@ -45,89 +45,6 @@ namespace wolf
namespace imu_with_dt { namespace imu_with_dt {
using namespace Eigen; 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> template<typename D1, typename D2, typename D3, typename T>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v, T& dt) 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, ...@@ -265,6 +182,116 @@ inline Matrix<typename D1::Scalar, 11, 1> compose(const MatrixBase<D1>& delta1,
return ret; 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> template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& delta1, inline void compose(const MatrixBase<D1>& delta1,
const MatrixBase<D2>& delta2, const MatrixBase<D2>& delta2,
...@@ -472,7 +499,7 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3> ...@@ -472,7 +499,7 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
Q = Id Q = Id
+ ((1 - cos_thn)/thn)*ux + ((1 - cos_thn)/thn)*ux
+ ((thn - sin_thn)/thn2)*(ux2); + ((thn - sin_thn)/thn)*(ux2);
P = 0.5*Id P = 0.5*Id
+ ((thn - sin_thn)/thn2)*ux + ((thn - sin_thn)/thn2)*ux
...@@ -487,17 +514,18 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in) ...@@ -487,17 +514,18 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
MatrixSizeCheck<10, 1>::check(d_in); MatrixSizeCheck<10, 1>::check(d_in);
typedef typename Derived::Scalar T; 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> > dp_in ( & d_in(0) );
Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) ); Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) );
Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) ); Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) );
const T& dt_in = d_in(9); const T& dt_in = d_in(9);
Map<Matrix<T, 3, 1> > dp_ret ( & ret(0) );
Map<Quaternion<T> > dq_ret ( & ret(3) ); Matrix<T, 11, 1> ret;
Map<Matrix<T, 3, 1> > dv_ret ( & ret(7) );
T& dt_ret = ret(10); 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> Q;
Matrix<T, 3, 3> P; Matrix<T, 3, 3> P;
......
...@@ -169,16 +169,24 @@ TEST(IMU_tools, plus_diff) ...@@ -169,16 +169,24 @@ TEST(IMU_tools, plus_diff)
TEST(IMU_tools, adjoint) TEST(IMU_tools, adjoint)
{ {
VectorXs delta1(11), delta2(11); VectorXs delta1(11), delta2(11);
VectorXs tau(10);
Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized(); Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized(); Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized();
delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1; delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3; 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 the definition
// From direct properties // From direct properties
ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10); ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10);
ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 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) TEST(IMU_tools, smalladjoint)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment