Skip to content
Snippets Groups Projects
Commit 21368b5d authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'newfile' into 'master' 

Add three_D_tools.h

See merge request mobile_robotics/wolf!178
parents 70cbc862 7b0f0402
No related branches found
No related tags found
1 merge request!178Add three_D_tools.h
Hide whitespace changes
Inline Side-by-side
Showing
with 383 additions and 0 deletions
src/three_D_tools.h 0 → 100644
+ 383
− 0
View file @ 21368b5d
/*
* three_D_tools.h
*
* Created on: Mar 15, 2018
* Author: jsola
*/
#ifndef THREE_D_TOOLS_H_
#define THREE_D_TOOLS_H_
#include "wolf.h"
#include "rotations.h"
/*
* Most functions in this file are explained in the document:
*
* Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
*
* They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
*
* The Delta is defined as
* Delta = [Dp, Dq, Dv]
* with
* Dp : position delta
* Dq : quaternion delta
* Dv : velocity delta
*
* They are listed below:
*
* - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], Dv = [0,0,0]
* - inverse: so that D (+) D.inv = I
* - compose: Dc = D1 (+) D2
* - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
* - composeOverState: x2 = x1 (+) D
* - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
* - lift: got from Delta manifold to tangent space (equivalent to log() in rotations)
* - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
* - plus: D2 = D1 (+) retract(d)
* - diff: d = lift( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
namespace wolf
{
namespace three_D {
using namespace Eigen;
template<typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
{
p = MatrixBase<D1>::Zero(3,1);
q = QuaternionBase<D2>::Identity();
}
template<typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
{
typedef typename D1::Scalar T1;
typedef typename D2::Scalar T2;
p << T1(0), T1(0), T1(0);
q << T2(0), T2(0), T2(0), T2(1);
}
template<typename T = wolf::Scalar>
inline Matrix<T, 7, 1> identity()
{
Matrix<T, 7, 1> ret;
ret<< T(0), T(0), T(0),
T(0), T(0), T(0), T(1);
return ret;
}
template<typename D1, typename D2, typename D4, typename D5>
inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
{
MatrixSizeCheck<3, 1>::check(dp);
MatrixSizeCheck<3, 1>::check(idp);
idp = - ( dq.conjugate() * dp );
idq = dq.conjugate();
}
template<typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d,
MatrixBase<D2>& id)
{
MatrixSizeCheck<7, 1>::check(d);
MatrixSizeCheck<7, 1>::check(id);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
inverse(dp, dq, idp, idq);
}
template<typename D>
inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
{
Matrix<typename D::Scalar, 7, 1> id;
inverse(d, id);
return id;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8, typename D6>
inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
{
MatrixSizeCheck<3, 1>::check(dp1);
MatrixSizeCheck<3, 1>::check(dp2);
MatrixSizeCheck<3, 1>::check(sum_p);
sum_p = dp1 + dq1*dp2;
sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
}
template<typename D1, typename D2, typename D3>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& sum)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 )
{
Matrix<typename D1::Scalar, 7, 1> ret;
compose(d1, d2, ret);
return ret;
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& sum,
MatrixBase<D4>& J_sum_d1,
MatrixBase<D5>& J_sum_d2)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
MatrixSizeCheck< 6, 6>::check(J_sum_d1);
MatrixSizeCheck< 6, 6>::check(J_sum_d2);
// Some useful temporaries
Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
// Jac wrt second delta
J_sum_d2.setIdentity(); //
J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
// J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, sum);
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8, typename D6>
inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q)
{
MatrixSizeCheck<3, 1>::check(dp1);
MatrixSizeCheck<3, 1>::check(dp2);
MatrixSizeCheck<3, 1>::check(diff_p);
diff_p = dq1.conjugate() * ( dp2 - dp1 );
diff_q = dq1.conjugate() * dq2;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d2_minus_d1)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(d2_minus_d1);
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & d2_minus_d1(0) );
Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(3) );
between(dp1, dq1, dp2, dq2, diff_p, diff_q);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 )
{
Matrix<typename D1::Scalar, 7, 1> diff;
between(d1, d2, diff);
return diff;
}
template<typename Derived>
Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<7, 1>::check(delta_in);
Matrix<typename Derived::Scalar, 6, 1> ret;
Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
dp_ret = dp_in;
do_ret = log_q(dq_in);
return ret;
}
template<typename Derived>
Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
Matrix<typename Derived::Scalar, 7, 1> ret;
Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
dp = dp_in;
dq = exp_q(do_in);
return ret;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8, typename D6>
inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
{
plus_p = dp1 + dp2;
plus_q = dq1 * exp_q(do2);
}
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 D2::Scalar, 3, 1> > dp2 ( & d2(0) );
Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) );
Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) );
plus(dp1, dq1, dp2, do2, dp_p, dq_p);
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 7, 1> ret;
plus(d1, d2, ret);
return ret;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
{
diff_p = dp2 - dp1;
diff_o = log_q(dq1.conjugate() * dq2);
}
template<typename D1, typename D2, 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<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
{
diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
J_do_dq1 = - jac_SO3_left_inv(diff_o);
J_do_dq2 = jac_SO3_right_inv(diff_o);
}
template<typename D1, typename D2, typename D3>
inline void diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& err)
{
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
inline void diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& dif,
MatrixBase<D4>& J_diff_d1,
MatrixBase<D5>& J_diff_d2)
{
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 ( & 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(d1, d2) 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_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 6, 1> diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 6, 1> ret;
diff(d1, d2, ret);
return ret;
}
} // namespace three_d
} // namespace wolf
#endif /* THREE_D_TOOLS_H_ */
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment