diff --git a/src/IMU_tools.h b/src/IMU_tools.h index 4f6ecda67b6695723b11ddecc56403da57a931a0..6d231223554ec4ea5e196cb7220a40e0eb42c001 100644 --- a/src/IMU_tools.h +++ b/src/IMU_tools.h @@ -34,10 +34,10 @@ * - 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 ) + * - log: got from Delta manifold to tangent space (equivalent to log() in rotations) + * - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations) + * - plus: D2 = D1 (+) exp_IMU(d) + * - diff: d = log_IMU( D2 (-) D1 ) * - body2delta: construct a delta from body magnitudes of linAcc and angVel */ @@ -345,7 +345,7 @@ inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1 } template<typename Derived> -Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in) +Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in) { MatrixSizeCheck<10, 1>::check(delta_in); @@ -366,7 +366,7 @@ Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in) } template<typename Derived> -Matrix<typename Derived::Scalar, 10, 1> retract(const MatrixBase<Derived>& d_in) +Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in) { MatrixSizeCheck<9, 1>::check(d_in); diff --git a/src/test/gtest_IMU_tools.cpp b/src/test/gtest_IMU_tools.cpp index 14957c9791a4754c254319b90f1d93c4e7763225..9f0f8c398e1f7b51690228f5ff3f75be4e66b01d 100644 --- a/src/test/gtest_IMU_tools.cpp +++ b/src/test/gtest_IMU_tools.cpp @@ -129,7 +129,7 @@ TEST(IMU_tools, lift_retract) VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi // transform to delta - VectorXs delta = retract(d_min); + VectorXs delta = exp_IMU(d_min); // expected delta Vector3s dp = d_min.head(3); @@ -139,11 +139,11 @@ TEST(IMU_tools, lift_retract) ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10); // transform to a new tangent -- should be the original tangent - VectorXs d_from_delta = lift(delta); + VectorXs d_from_delta = log_IMU(delta); ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10); // transform to a new delta -- should be the previous delta - VectorXs delta_from_d = retract(d_from_delta); + VectorXs delta_from_d = exp_IMU(d_from_delta); ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10); }