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

Merge branch 'threeD' into 'master' 

Cleanup and improve doc

See merge request mobile_robotics/wolf!179
parents 21368b5d bb95ce4a
No related branches found
No related tags found
1 merge request!179Cleanup and improve doc
Hide whitespace changes
Inline Side-by-side
Showing
with 36 additions and 44 deletions
src/three_D_tools.h
+ 36
− 44
View file @ c0d83193
...@@ -13,32 +13,24 @@ ...@@ -13,32 +13,24 @@
#include "rotations.h" #include "rotations.h"
/* /*
* Most functions in this file are explained in the document: * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
* *
* Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
* * Delta = [Dp, Dq]
* They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
*
* The Delta is defined as
* Delta = [Dp, Dq, Dv]
* with * with
* Dp : position delta * Dp : position delta
* Dq : quaternion delta * Dq : quaternion delta
* Dv : velocity delta
* *
* They are listed below: * The functions are listed below:
* *
* - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], Dv = [0,0,0] * - compose: Dc = D1 (+) D2
* - inverse: so that D (+) D.inv = I * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
* - compose: Dc = D1 (+) D2 * - inverse: so that D (+) D.inv = D.inv (+) D = I
* - between: Db = D2 (-) D1, so that D2 = D1 (+) Db * - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
* - composeOverState: x2 = x1 (+) D * - lift: go from Delta manifold to tangent space (equivalent to log() in rotations)
* - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D * - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
* - lift: got from Delta manifold to tangent space (equivalent to log() in rotations) * - plus: D2 = D1 (+) retract(d)
* - retract: go from tangent space to delta manifold (equivalent to exp() in rotations) * - diff: d = lift( D2 (-) D1 )
* - plus: D2 = D1 (+) retract(d)
* - diff: d = lift( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/ */
...@@ -80,8 +72,8 @@ inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, ...@@ -80,8 +72,8 @@ inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
MatrixSizeCheck<3, 1>::check(dp); MatrixSizeCheck<3, 1>::check(dp);
MatrixSizeCheck<3, 1>::check(idp); MatrixSizeCheck<3, 1>::check(idp);
idp = - ( dq.conjugate() * dp ); idp = - dq.conjugate() * dp ;
idq = dq.conjugate(); idq = dq.conjugate() ;
} }
template<typename D1, typename D2> template<typename D1, typename D2>
...@@ -108,7 +100,7 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d) ...@@ -108,7 +100,7 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
return id; return id;
} }
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8, typename D6> template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q ) MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
...@@ -180,17 +172,17 @@ inline void compose(const MatrixBase<D1>& d1, ...@@ -180,17 +172,17 @@ inline void compose(const MatrixBase<D1>& d1,
compose(d1, d2, sum); compose(d1, d2, sum);
} }
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8, typename D6> template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q) MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
{ {
MatrixSizeCheck<3, 1>::check(dp1); MatrixSizeCheck<3, 1>::check(dp1);
MatrixSizeCheck<3, 1>::check(dp2); MatrixSizeCheck<3, 1>::check(dp2);
MatrixSizeCheck<3, 1>::check(diff_p); MatrixSizeCheck<3, 1>::check(dp12);
diff_p = dq1.conjugate() * ( dp2 - dp1 ); dp12 = dq1.conjugate() * ( dp2 - dp1 );
diff_q = dq1.conjugate() * dq2; dq12 = dq1.conjugate() * dq2;
} }
template<typename D1, typename D2, typename D3> template<typename D1, typename D2, typename D3>
...@@ -206,20 +198,20 @@ inline void between(const MatrixBase<D1>& d1, ...@@ -206,20 +198,20 @@ inline void between(const MatrixBase<D1>& d1,
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); 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> > dp2 ( & d2(0) );
Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) ); Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & d2_minus_d1(0) ); Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(3) ); Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) );
between(dp1, dq1, dp2, dq2, diff_p, diff_q); between(dp1, dq1, dp2, dq2, dp12, dq12);
} }
template<typename D1, typename D2> template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2 ) const MatrixBase<D2>& d2 )
{ {
Matrix<typename D1::Scalar, 7, 1> diff; Matrix<typename D1::Scalar, 7, 1> d12;
between(d1, d2, diff); between(d1, d2, d12);
return diff; return d12;
} }
template<typename Derived> template<typename Derived>
...@@ -258,7 +250,7 @@ Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in) ...@@ -258,7 +250,7 @@ Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
return ret; return ret;
} }
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8, typename D6> template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q) MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
...@@ -270,25 +262,25 @@ inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, ...@@ -270,25 +262,25 @@ inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
template<typename D1, typename D2, typename D3> template<typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1, inline void plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2, const MatrixBase<D2>& d2,
MatrixBase<D3>& d_pert) MatrixBase<D3>& d_plus)
{ {
Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) ); Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); 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> > dp2 ( & d2(0) );
Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) ); Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) ); Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) ); Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) );
plus(dp1, dq1, dp2, do2, dp_p, dq_p); plus(dp1, dq1, dp2, do2, dp_p, dq_p);
} }
template<typename D1, typename D2> template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1, inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2) const MatrixBase<D2>& d2)
{ {
Matrix<typename D1::Scalar, 7, 1> ret; Matrix<typename D1::Scalar, 7, 1> d_plus;
plus(d1, d2, ret); plus(d1, d2, d_plus);
return ret; return d_plus;
} }
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8> template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
...@@ -362,7 +354,7 @@ inline void diff(const MatrixBase<D1>& d1, ...@@ -362,7 +354,7 @@ inline void diff(const MatrixBase<D1>& d1,
J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity 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_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.setIdentity(6,6); // 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) J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
} }
......
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