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

testing

parent d14f4cf2
No related branches found
No related tags found
No related merge requests found
...@@ -516,12 +516,13 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in) ...@@ -516,12 +516,13 @@ Matrix<typename Derived::Scalar, 11, 1> exp_IMU(const MatrixBase<Derived>& d_in)
typedef typename Derived::Scalar T; typedef typename Derived::Scalar T;
Matrix<T, 11, 1> ret;
Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) ); Map<const Matrix<T, 3, 1> > dp_in ( & d_in(0) );
Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) ); Map<const Matrix<T, 3, 1> > do_in ( & d_in(3) );
Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) ); Map<const Matrix<T, 3, 1> > dv_in ( & d_in(6) );
const T& dt_in = d_in(9); const T& dt_in = d_in(9);
Matrix<T, 11, 1> ret;
Map<Matrix<T, 3, 1> > dp ( & ret(0) ); Map<Matrix<T, 3, 1> > dp ( & ret(0) );
Map<Quaternion<T> > dq ( & ret(3) ); Map<Quaternion<T> > dq ( & ret(3) );
Map<Matrix<T, 3, 1> > dv ( & ret(7) ); Map<Matrix<T, 3, 1> > dv ( & ret(7) );
......
...@@ -169,13 +169,21 @@ TEST(IMU_tools, plus_minus) ...@@ -169,13 +169,21 @@ TEST(IMU_tools, plus_minus)
TEST(IMU_tools, adjoint) TEST(IMU_tools, adjoint)
{ {
VectorXs delta1(11), delta2(11); VectorXs delta1(11), delta2(11);
VectorXs tau(10);
Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized(); Vector4s qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized(); Vector4s qv2 = (Vector4s() << 6, 5, 4, 3).finished().normalized();
delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1; delta1 << 0, 1, 2, qv1, 7, 8, 9, 0.1;
delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3; delta2 << 10, 11, 12, qv2, 17, 18, 19, 0.3;
tau << .1, .2, .3, -.1, -.2, -.3, .3, .2, .1, 0.1;
ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10); ASSERT_MATRIX_APPROX(adjoint(delta1).inverse(), adjoint(inverse(delta1)), 1e-10);
ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10); ASSERT_MATRIX_APPROX(adjoint(compose(delta1, delta2)), adjoint(delta1)*adjoint(delta2), 1e-10);
MatrixXs tau2 = adjoint(delta1)*tau;
// MatrixXs d3 = compose(delta1, exp_IMU(tau));
// MatrixXs d4 = compose(exp_IMU(tau2),delta1);
ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau)), compose(exp_IMU(tau2),delta1), 1e-10);
// ASSERT_MATRIX_APPROX(compose(delta1, exp_IMU(tau2)), compose(exp_IMU(tau),delta1), 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