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

Add operations of IMU deltas with states

composeWithState() is x2 = x1+d
betweenStates() is d=x2-x1
parent 1b2fa60f
No related branches found
No related tags found
No related merge requests found
...@@ -83,9 +83,6 @@ inline void compose(const MatrixBase<D1>& d1, ...@@ -83,9 +83,6 @@ inline void compose(const MatrixBase<D1>& d1,
sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
} }
template<typename D1, typename D2>
inline void a() {}
template<typename D1, typename D2> template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 10, 1> compose(const MatrixBase<D1>& d1, inline Matrix<typename D1::Scalar, 10, 1> compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2, const MatrixBase<D2>& d2,
...@@ -177,6 +174,76 @@ inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1, ...@@ -177,6 +174,76 @@ inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
return d_bet; return d_bet;
} }
template<typename D1, typename D2, typename D3>
inline void composeOverState(const MatrixBase<D1>& x,
const MatrixBase<D2>& d,
Scalar dt,
MatrixBase<D3>& x_plus_d)
{
MatrixSizeCheck<10, 1>::check(x);
MatrixSizeCheck<10, 1>::check(d);
MatrixSizeCheck<10, 1>::check(x_plus_d);
Map<const Matrix<typename D1::Scalar, 3, 1> > p ( &x( 0 ) );
Map<const Quaternion<typename D1::Scalar> > q ( &x( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > v ( &x( 7 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( &d( 0 ) );
Map<const Quaternion<typename D2::Scalar> > dq ( &d( 3 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( &d( 7 ) );
Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( &x_plus_d( 0 ));
Map<Quaternion<typename D3::Scalar> > q_plus_d ( &x_plus_d( 3 ));
Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( &x_plus_d( 7 ));
p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
v_plus_d = v + gravity()*dt + q*dv;
q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_plus_d
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>& x,
const MatrixBase<D2>& d,
Scalar dt)
{
Matrix<typename D1::Scalar, 10, 1> ret;
composeOverState(x, d, dt, ret);
return ret;
}
template<typename D1, typename D2, typename D3>
inline void betweenStates(const MatrixBase<D1>& x1,
const MatrixBase<D2>& x2,
Scalar dt,
MatrixBase<D3>& x2_minus_x1)
{
MatrixSizeCheck<10, 1>::check(x1);
MatrixSizeCheck<10, 1>::check(x2);
MatrixSizeCheck<10, 1>::check(x2_minus_x1);
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( &x1(0) );
Map<const Quaternion<typename D1::Scalar> > q1 ( &x1(3) );
Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( &x1(7) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( &x2(0) );
Map<const Quaternion<typename D2::Scalar> > q2 ( &x2(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( &x2(7) );
Map<Matrix<typename D3::Scalar, 3, 1> > dp (&x2_minus_x1(0));
Map<Quaternion<typename D3::Scalar> > dq (&x2_minus_x1(3));
Map<Matrix<typename D3::Scalar, 3, 1> > dv (&x2_minus_x1(7));
dp = q1.conjugate() * ( p2 - p1 - v1*dt - 0.5*gravity()*dt*dt );
dq = q1.conjugate() * q2;
dv = q1.conjugate() * ( v2 - v1 - gravity()*dt );
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1,
const MatrixBase<D2>& x2,
Scalar dt)
{
Matrix<typename D1::Scalar, 10, 1> ret;
betweenStates(x1, x2, dt, ret);
return ret;
}
template<typename Derived> template<typename Derived>
Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in) Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
{ {
......
...@@ -79,6 +79,35 @@ TEST(IMU_tools, compose_between) ...@@ -79,6 +79,35 @@ TEST(IMU_tools, compose_between)
ASSERT_MATRIX_APPROX(diff, imu::inverse(d2, dt), 1e-10); ASSERT_MATRIX_APPROX(diff, imu::inverse(d2, dt), 1e-10);
} }
TEST(IMU_tools, compose_between_with_state)
{
VectorXs x(10), d(10), x2(10), x3(10), d2(10), d3(10);
Vector4s qv;
Scalar dt = 0.1;
qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
x << 0, 1, 2, qv, 7, 8, 9;
qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
d << 9, 8, 7, qv, 2, 1, 0;
imu::composeOverState(x, d, dt, x2);
x3 = imu::composeOverState(x, d, dt);
ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
// betweenStates(x, x2) should recover d
imu::betweenStates(x, x2, dt, d2);
d3 = imu::betweenStates(x, x2, dt);
ASSERT_MATRIX_APPROX(d2, d, 1e-10);
ASSERT_MATRIX_APPROX(d3, d, 1e-10);
ASSERT_MATRIX_APPROX(imu::betweenStates(x, x2, dt), d, 1e-10);
// x + (x2 - x) = x2
ASSERT_MATRIX_APPROX(imu::composeOverState(x, imu::betweenStates(x, x2, dt), dt), x2, 1e-10);
// (x + d) - x = d
ASSERT_MATRIX_APPROX(imu::betweenStates(x, imu::composeOverState(x, d, dt), dt), d, 1e-10);
}
TEST(IMU_tools, lift_retract) 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 VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
......
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