diff --git a/src/three_D_tools.h b/src/three_D_tools.h
index eaec7185ce9b8ad68767cbbde204538794a23c48..28a15f29757116ffc1c9bc10f9507df58f9a5452 100644
--- a/src/three_D_tools.h
+++ b/src/three_D_tools.h
@@ -13,32 +13,24 @@
#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
- *
- * They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
- *
- * The Delta is defined as
- * Delta = [Dp, Dq, Dv]
+ * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
+ * Delta = [Dp, Dq]
* with
* Dp : position 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]
- * - 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
+ * - compose: Dc = D1 (+) D2
+ * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
+ * - inverse: so that D (+) D.inv = D.inv (+) D = I
+ * - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
+ * - lift: go 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 )
*/
@@ -80,8 +72,8 @@ inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
MatrixSizeCheck<3, 1>::check(dp);
MatrixSizeCheck<3, 1>::check(idp);
- idp = - ( dq.conjugate() * dp );
- idq = dq.conjugate();
+ idp = - dq.conjugate() * dp ;
+ idq = dq.conjugate() ;
}
template<typename D1, typename D2>
@@ -108,7 +100,7 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
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,
const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
@@ -180,17 +172,17 @@ inline void compose(const MatrixBase<D1>& d1,
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,
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(dp2);
- MatrixSizeCheck<3, 1>::check(diff_p);
+ MatrixSizeCheck<3, 1>::check(dp12);
- diff_p = dq1.conjugate() * ( dp2 - dp1 );
- diff_q = dq1.conjugate() * dq2;
+ dp12 = dq1.conjugate() * ( dp2 - dp1 );
+ dq12 = dq1.conjugate() * dq2;
}
template<typename D1, typename D2, typename D3>
@@ -206,20 +198,20 @@ inline void between(const MatrixBase<D1>& d1,
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) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
+ 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>
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;
- between(d1, d2, diff);
- return diff;
+ Matrix<typename D1::Scalar, 7, 1> d12;
+ between(d1, d2, d12);
+ return d12;
}
template<typename Derived>
@@ -258,7 +250,7 @@ Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
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,
const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
@@ -270,25 +262,25 @@ inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
template<typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1,
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 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) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
+ Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(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)
+ const MatrixBase<D2>& d2)
{
- Matrix<typename D1::Scalar, 7, 1> ret;
- plus(d1, d2, ret);
- return ret;
+ Matrix<typename D1::Scalar, 7, 1> d_plus;
+ plus(d1, d2, d_plus);
+ return d_plus;
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
@@ -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.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)
}