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

New between() with return value

parent 721b382f
No related branches found
No related tags found
1 merge request!123Calibration
......@@ -155,6 +155,16 @@ inline void between(const MatrixBase<D1>& d1,
diff_v = dq1.conjugate() * ( dv2 - dv1 );
}
template<typename D1, typename D2>
inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
Scalar dt)
{
Matrix<typename D1::Scalar, 10, 1> d_bet;
between(d1, d2, dt, d_bet);
return d_bet;
}
template<typename Derived>
Matrix<typename Derived::Scalar, 9, 1> lift(const MatrixBase<Derived>& delta_in)
{
......@@ -202,15 +212,10 @@ inline void compare(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& err)
{
Matrix<typename D3::Scalar, 10, 1> delta_err;
between(d1, d2, 0.0, delta_err);
err = lift(delta_err);
err = lift( between(d1, d2, 0.0) );
}
} // namespace imu
} // namespace wolf
......
......@@ -25,7 +25,7 @@ TEST(IMU_tools, identity)
TEST(IMU_tools, inverse)
{
VectorXs d(10), id(10), iid(10), iiid(10);
VectorXs d(10), id(10), iid(10), iiid(10), I(10);
Vector4s qv;
Scalar dt = 0.1;
......@@ -33,10 +33,15 @@ TEST(IMU_tools, inverse)
d << 0, 1, 2, qv, 7, 8, 9;
id = imu::inverse(d, dt);
imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
iiid = imu::inverse(iid, dt);
imu::compose(id, d, dt, I);
ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
imu::compose(d, id, -dt, I); // Observe -dt is reversed !!
ASSERT_MATRIX_APPROX(I, imu::identity(), 1e-10);
imu::inverse(id, -dt, iid); // Observe -dt is reversed !!
ASSERT_MATRIX_APPROX( d, iid, 1e-10);
iiid = imu::inverse(iid, dt);
ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
}
......
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